RealMonoCamera.cpp

Go to the documentation of this file.

#include "RealMonoCamera.hpp"

namespace virtual_camera {


/*PUBLIC METHODS*/
RealMonoCamera::RealMonoCamera(std::string name,
                const boost::shared_ptr<tf::TransformListener> & listener):
        transformListener(listener) {
        this->name = name;
        this->positionCache = TableP2Ptr(new TableP2());
}


bool RealMonoCamera::isReady() {
        if (this->camInfo) {
                if (this->image) {
                        return true;
                }
        }
        return false;
}


sensor_msgs::CameraInfoConstPtr RealMonoCamera::getCameraInfo() {
        return this->camInfo;
}


void RealMonoCamera::setCameraInfo(const sensor_msgs::CameraInfoConstPtr & camInfo) {
        this->camInfo = camInfo;
        kMat[0][0] = camInfo->K[0];kMat[0][1] = camInfo->K[1];kMat[0][2] = camInfo->K[2];
        kMat[1][0] = camInfo->K[3];kMat[1][1] = camInfo->K[4];kMat[1][2] = camInfo->K[5];
        kMat[2][0] = camInfo->K[6];kMat[2][1] = camInfo->K[7];kMat[2][2] = camInfo->K[8];
}


void RealMonoCamera::updateImage(const sensor_msgs::ImageConstPtr & image) {
        this->image = image;
}


void RealMonoCamera::setPointsCache(std::string frame, const TableV3Ptr & points) {
        this->requestFrame = frame;
        this->requestPoints = points;
        this->buildInnerCache();
}


bool RealMonoCamera::validateCache() {
        //XXX: doesn't test if internal camera parameters changed
        tf::StampedTransform transform;
        try {
                this->transformListener->lookupTransform(this->name, this->requestFrame,
                          ros::Time(0), transform);
        } catch (tf::TransformException & ex){
                    ROS_ERROR("%s",ex.what());
        }
        if (areTransformsEqual(transform, this->usedTransform)){
                return true;
        }else {
                this->buildInnerCache();
                return false;
        }
}


std::string RealMonoCamera::getName() {
        return this->name;
}


TableP2ConstPtr RealMonoCamera::getCachedPositions() {
        return this->positionCache;
}


sensor_msgs::ImageConstPtr RealMonoCamera::getImage() {
        return this->image;
}


float RealMonoCamera::getWeight(int row, int col) {
        const Point2D & position = (*(this->positionCache))[row][col];
        if(position.x == NOT_VISIBLE) {
                return 0;
        }
        float xW = 1 - std::fabs((2.0*position.x/camInfo->width)-1);
        float yW = 1 - std::fabs((2.0*position.y/camInfo->height)-1);
        return std::min(xW, yW);
}

/*PUBLIC METHODS END*/


/*PRIVATE METHODS*/
void RealMonoCamera::buildInnerCache() {
        tf::StampedTransform & transform = this->usedTransform;
        try {
                this->transformListener->lookupTransform(this->name, this->requestFrame,
                  ros::Time(0), transform);
        } catch (tf::TransformException & ex){
            ROS_ERROR("%s",ex.what());
        }
        int rows = this->requestPoints->shape()[0];
        int cols = this->requestPoints->shape()[1];
        this->positionCache->resize(boost::extents[rows][cols]);
        for (int y=0; y < rows; y++) { //row major
                for (int x=0; x < cols; x++) {
                        this->mapPointToInnerCache(y, x, transform);
                }
        }
}


void RealMonoCamera::mapPointToInnerCache(
                int row, int col, tf::StampedTransform & transform) {
        tf::Vector3 localPt = transform*(*(this->requestPoints))[row][col];
        Point2D & res = (*(this->positionCache))[row][col];
        if (localPt[2] > 0) {
                this->applyPlumbBobDistortion(localPt);
                tf::Vector3 imPt = (this->kMat)*localPt;
                if (imPt[0] >= 0 && imPt[0] < this->camInfo->width &&
                        imPt[1] >= 0 && imPt[1] < this->camInfo->height) {
                        res.x = static_cast<int>(std::floor(imPt[0]));
                        res.y = static_cast<int>(std::floor(imPt[1]));
                        return;
                }
        }
        //point not in image cameras view
        res.x = NOT_VISIBLE;
        res.y = NOT_VISIBLE;
}


void RealMonoCamera::applyPlumbBobDistortion(tf::Vector3 & p) {
        float x = p[0]/p[2];
        float y = p[1]/p[2];
        float r2 = x*x + y*y;
        float dx1 = 2*camInfo->D[2]*x*y + camInfo->D[3]*(r2+2*x*x);
        float dx2 = camInfo->D[2]*(r2 + 2*y*y) + 2*camInfo->D[3]*x*y;
  // Allow variable number of distortion coefficients following OpenCV implementation, see
  // http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
  float rad = 1 + camInfo->D[0]*r2 + camInfo->D[1]*r2*r2;
  if (camInfo->D.size() >= 5) {
    rad += camInfo->D[4]*r2*r2*r2;
  }
  if (camInfo->D.size() >= 8) {
    rad /= 1 + camInfo->D[5]*r2 + camInfo->D[6]*r2*r2 + camInfo->D[7]*r2*r2*r2;
  }
        p[0] = rad*x + dx1;
        p[1] = rad*y + dx2;
        p[2] = 1;
}


bool RealMonoCamera::areTransformsEqual(
                const tf::StampedTransform & t1, const tf::StampedTransform & t2) {
        const tf::Vector3 & o1 = t1.getOrigin();
        const tf::Vector3 & o2 = t2.getOrigin();
        if (fabs(o1.getX() - o2.getX()) > 0.000001 ||
                fabs(o1.getY() - o2.getY()) > 0.000001 ||
                fabs(o1.getZ() - o2.getZ()) > 0.000001) {
                return false;
        }
        const tf::Quaternion & r1 = t1.getRotation();
        const tf::Quaternion & r2 = t2.getRotation();
  // r1 and -r1 represents the same rotation
  if (fabs(r1.dot(r2)) < 0.999) {
                return false;
        }
        return true;
}
/*PRIVATE METHODS END*/

}