InterpolatingImageComposer.cpp

Go to the documentation of this file.

#include "InterpolatingImageComposer.hpp"

namespace virtual_camera {

/*PUBLIC METHODS*/

InterpolatingImageComposer::InterpolatingImageComposer(
                const RealCameraPtrListPtr & realCameras) {
        this->realCameras = realCameras;
        this->cacheSize = 0;
}


void InterpolatingImageComposer::refreshCache(uint rows, uint cols) {
        std::list<RealCameraPtr>::iterator it = this->realCameras->begin();
        uint cams = this->realCameras->size();
        this->camsPos.clear();
        this->camsPos.reserve(cams);
        while (it != this->realCameras->end()) {
                this->camsPos.push_back((*it)->getCachedPositions());
                ++it;
        }
        if (rows*cols*cams != this->cacheSize) {
                this->weightCache.reset(new float[rows*cols*cams]);
        }
        float totalSum;
        std::vector<float> buffer(cams);
        this->camsImg.reserve(cams);
        this->camsNeeded.reserve(cams);
        for (uint i=0; i<cams; i++) { this->camsNeeded[i] = false; }
        for (uint i=0; i<rows; i++) {
                for (uint j=0; j<cols; j++) {
                        uint k = 0;
                        totalSum = 0;
                        it = this->realCameras->begin();
                        while (it != this->realCameras->end()) {
                                float w = (*it)->getWeight(i, j);
                                buffer[k] = w;
                                totalSum += w;
                                ++it;
                                ++k;
                        }
                        for (k=0; k<cams; k++) {
                                float w = buffer[k]/totalSum;
                                this->weightCache[i*cols*cams+j*cams+k] = w;
                                if (w > 0.000001){
                                        this->camsNeeded[k] = true;
                                }
                        }
                }
        }
        //ROS_INFO("Weight cache refreshed.");
}


void InterpolatingImageComposer::compose(
                const cv_bridge::CvImagePtr & target, const std::string & encoding) {
        this->prepareRealCameraImages(encoding);
        uint cams = this->realCameras->size();
        uint nChannels = target->image.channels();
        uint nRows = target->image.rows;
        uint nCols = target->image.cols;
        uchar *vP;
        const uchar * rP;
        for (uint i=0; i < nRows; i++) {
                vP = target->image.ptr<uchar>(i);
                for (uint j=0; j < nCols; j++) {
                        for(uint k=0; k < cams; k++) {
                                float weight = this->weightCache[i*nCols*cams+j*cams+k];
                                if (weight > 0) {
                                const Point2D & pt = (*(this->camsPos[k]))[i][j];
                                rP = this->camsImg[k]->image.ptr(pt.y, pt.x);
                                        for (uint l=0; l < nChannels; l++) {
                                                vP[nChannels*j+l] += static_cast<uchar>(weight*rP[l]);
                                        }
                                }
                        }
                }
        }
}


/*PUBLIC METHODS END*/
/*PRIVATE METHODS*/
void InterpolatingImageComposer::prepareRealCameraImages(const std::string & enc) {
        std::list<RealCameraPtr>::iterator cam = this->realCameras->begin();
        std::list<RealCameraPtr>::iterator end = this->realCameras->end();
        int i = 0;
        this->camsImg.clear();
        //ROS_INFO("new image composition");
        for (; cam != end; ++cam, ++i){
                if (this->camsNeeded[i]) {
                        this->camsImg.push_back(cv_bridge::toCvShare((*cam)->getImage(), enc));
                        //ROS_INFO_STREAM((*cam)->getImage()->header.stamp.toNSec());
                }else {
                        this->camsImg.push_back(cv_bridge::CvImageConstPtr());
                }
        }
}
/*PRIVATE METHODS END*/


}