GeneralVirtualCamera.cpp

Go to the documentation of this file.

#include "GeneralVirtualCamera.hpp"

namespace virtual_camera {

/*PUBLIC METHODS*/


GeneralVirtualCamera::GeneralVirtualCamera(const std::string & name,
                const VirtualCameraParametersPtr & params):
                        realCameras(new std::list<RealCameraPtr>())     {
        this->name = name;
        this->composer = ImageComposerPtr(new InterpolatingImageComposer(this->realCameras));
        this->updateVirtualCameraParameters(params);
        this->seqId = 0;
        this->composerCacheFlag = true;
        this->updatePointCloudFlag = false;
        for (int i=0; i<5; i++) {
                this->Dci.push_back(0);
        }
        for (int i=0; i<9; i++) {
                Rci0[i] = Kci0[i] = Pci0[i] = 0;
        }
        Pci0[9] = Pci0[10] = Pci0[11] = 0;
        Rci[0] = 1; Rci[1] = 0; Rci[2] = 0;
        Rci[3] = 0; Rci[4] = 1; Rci[5] = 0;
        Rci[6] = 0; Rci[7] = 0; Rci[8] = 1;
        this->isValidCameraInfo = false;
}

sensor_msgs::CameraInfoConstPtr GeneralVirtualCamera::getCameraInfo() {
        sensor_msgs::CameraInfoPtr camInfo(new sensor_msgs::CameraInfo());
        camInfo->header.stamp = ros::Time::now();
        camInfo->header.frame_id = this->name;
        camInfo->width = static_cast<uint32_t>(this->parameters->viewportWidth);
        camInfo->height = static_cast<uint32_t>(this->parameters->viewportHeight);
        camInfo->distortion_model = "plumb_bob";
        camInfo->D = this->Dci;
        camInfo->K = this->isValidCameraInfo ? this->Kci : this->Kci0;
        camInfo->R = this->isValidCameraInfo ? this->Rci : this->Rci0;
        camInfo->P = this->isValidCameraInfo ? this->Pci : this->Pci0;
        return camInfo;
}


sensor_msgs::ImageConstPtr GeneralVirtualCamera::generateImage() {
        //ROS_INFO_STREAM(this->name << " - Generating new image");
        this->broadcast();
        this->updateCachesIfNeeded();
        cv_bridge::CvImagePtr image = this->createEmptyImage();
        this->composer->compose(image, this->parameters->encoding);
        return image->toImageMsg();
}


bool GeneralVirtualCamera::addRealCamera(const RealCameraPtr & realCamera) {
        std::list<RealCameraPtr>::iterator it = this->realCameras->begin();
        while (it != this->realCameras->end()){
                if ((*it) == realCamera) {
                        return false;
                }
                ++it;
        }
        this->realCameras->push_back(realCamera);
        realCamera->setPointsCache(this->name, this->pointCloud);
        this->composerCacheFlag = true;
        return true;
}


bool GeneralVirtualCamera::removeRealCamera(const RealCameraPtr & realCamera) {
        ROS_ERROR("Remove camera is unsupported operation yet");
        return false;
}


void GeneralVirtualCamera::updateVirtualCameraParameters(
                const VirtualCameraParametersPtr & params) {
        this->parameters = params;
        this->updateExternalParameters(params);
        this->updateInternalParameters(params);
}


double GeneralVirtualCamera::getFrameRate() {
        return this->parameters->frameRate;
}


std::string GeneralVirtualCamera::getName() {
        return this->name;
}
/*PUBLIC METHODS END*/

/*PRIVATE METHODS*/


void GeneralVirtualCamera::updateExternalParameters(
                const VirtualCameraParametersPtr & params) {
        this->transform.setOrigin(tf::Vector3(params->posX, params->posY, params->posZ));
        double roll = angles::from_degrees(-params->tilt);
        double yaw = angles::from_degrees(params->pan);
        this->transform.setRotation(tf::createQuaternionFromRPY(roll, 0, yaw));
}


void GeneralVirtualCamera::updateInternalParameters(
                const VirtualCameraParametersPtr & params) {
        this->hFovRad = angles::from_degrees(params->horizontalFov);
        this->vFovRad = angles::from_degrees(params->verticalFov);
        this->composerCacheFlag = true;
        this->updateCameraModel(params->model);
        this->pointCloud = this->model->createPointCloud(this->parameters);
    this->updatePointCloudFlag = true;
}


void GeneralVirtualCamera::updateCameraModel(const std::string & identifier) {
        if (!(this->model) || identifier != this->model->getIndentifier()) {
                if (identifier == "cylindric") {
                        this->model = CameraModelPtr(new BasicCylinderCameraModel());
                        this->isValidCameraInfo = false;
                } else if (identifier == "spherical") {
                        this->model = CameraModelPtr(new SphericalCameraModel());
                        this->isValidCameraInfo = false;
                } else {
                        this->model = CameraModelPtr(new PinholeCameraModel());
                        this->isValidCameraInfo = true;
                }
        }
}




cv_bridge::CvImagePtr GeneralVirtualCamera::createEmptyImage() {
        cv_bridge::CvImagePtr image(new cv_bridge::CvImage());
        const int & width = this->parameters->viewportWidth;
        const int & height = this->parameters->viewportHeight;
        image->encoding = this->parameters->encoding;
        image->header.seq = this->seqId++;
        image->header.stamp = ros::Time::now();
        image->header.frame_id = this->name;
        if (this->parameters->encoding == "bgr8") {
                image->image = cv::Mat(height, width,
                                CV_8UC3, cv::Scalar_<uchar>::all(0));
        }else if (this->parameters->encoding == "mono8") {
                image->image = cv::Mat(height, width,
                                                CV_8UC1, cv::Scalar_<uchar>::all(0));
        }
        return image;
}


void GeneralVirtualCamera::updateCachesIfNeeded() {
        std::list<RealCameraPtr>::iterator it = this->realCameras->begin();
        while (it != this->realCameras->end()) {
                if (this->updatePointCloudFlag) {
                        (*it)->setPointsCache(this->name, this->pointCloud);
                }else if (!(*it)->validateCache()){
                        this->composerCacheFlag = true;
                }
                ++it;
        }
        if (this->composerCacheFlag) {
                this->composer->refreshCache(this->parameters->viewportHeight,
                                this->parameters->viewportWidth);
                this->composerCacheFlag = false;
        }
        this->updatePointCloudFlag = false;
}


void GeneralVirtualCamera::broadcast() {
        this->broadcaster.sendTransform(tf::StampedTransform(this->transform,
                        ros::Time::now(), this->parameters->parentFrameId, this->name));
}


void GeneralVirtualCamera::updateCameraInfo() {
        //Dci and Rci are updated only once in constructor
        const int & width = this->parameters->viewportWidth;
        const int & height = this->parameters->viewportHeight;
        double fx = width/(2*std::tan(this->hFovRad/2));
        double fy = height/(2*std::tan(this->vFovRad/2));
        double px = (width+1)/2;
        double py = (height+1)/2;
        Kci[0] = fx; Kci[1] = 0; Kci[2] = px;
        Kci[3] = 0; Kci[4] = fy; Kci[5] = py;
        Kci[6] = 0; Kci[7] = 0; Kci[8] = 1;
        Pci[0] = fx; Pci[1] = 0; Pci[2] = px; Pci[3] = 0;
        Pci[4] = 0; Pci[5] = fy; Pci[6] = py; Pci[7] = 0;
        Pci[8] = 0; Pci[9] = 0; Pci[10] = 1; Pci[11] = 0;

}
/*PRIVATE METHODS END*/


}