BasicCylinderCameraModel.cpp

Go to the documentation of this file.

#include "BasicCylinderCameraModel.hpp"

namespace virtual_camera {


std::string BasicCylinderCameraModel::getIndentifier() {
        return "cylindric";
}


TableV3Ptr BasicCylinderCameraModel::createPointCloud(
                const VirtualCameraParametersPtr & params) {
        const int & width = params->viewportWidth;
        const int & height = params->viewportHeight;
        const double & radius = params->approxDistance;
        TableV3Ptr ret = TableV3Ptr(new TableV3(boost::extents[height][width]));
        double vFovRad = angles::from_degrees(params->verticalFov);
        double vLength = 2*radius*std::tan(vFovRad/2);
        double vStep = vLength/height;
        double angStep = 2*boost::math::constants::pi<double>()/width;
        double angStart = 1.5*boost::math::constants::pi<double>();
        for (int y=0; y<height; y++) {
                double yPos = (y-height/2)*vStep;
                for (int x=0; x<width; x++) {
                        tf::Vector3 & pt = (*(ret))[y][x];
                        pt[0] = radius*std::cos(angStart-x*angStep);
                        pt[1] = yPos;
                        pt[2] = radius*std::sin(angStart-x*angStep);
                }
        }
        return ret;
}


}