PinholeCameraModel.cpp

Go to the documentation of this file.

#include "PinholeCameraModel.hpp"

namespace virtual_camera {

std::string PinholeCameraModel::getIndentifier() {
        return "pinhole";
}


TableV3Ptr PinholeCameraModel::createPointCloud(
                const VirtualCameraParametersPtr & params) {
        const int & width = params->viewportWidth;
        const int & height = params->viewportHeight;
        const double & distance = params->approxDistance;
        TableV3Ptr ret = TableV3Ptr(new TableV3(boost::extents[height][width]));
        double hFovRad = angles::from_degrees(params->horizontalFov);
        double vFovRad = angles::from_degrees(params->verticalFov);
        double hLength = 2*distance*std::tan(hFovRad/2);
        double vLength = 2*distance*std::tan(vFovRad/2);
        double hStep = hLength/width;
        double vStep = vLength/height;

        for (int y=0; y<height; y++) {
                double yPos = (y-height/2)*vStep;
                for (int x=0; x<width; x++) {
                        double xPos = (x-width/2)*hStep;
                        tf::Vector3 & pt = (*(ret))[y][x];
                        pt[0] = xPos;
                        pt[1] = yPos;
                        pt[2] = distance;
                }
        }
        return ret;
}


}