SphericalCameraModel.cpp

Go to the documentation of this file.

#include "SphericalCameraModel.hpp"

namespace virtual_camera {


std::string SphericalCameraModel::getIndentifier() {
        return "spherical";
}


TableV3Ptr SphericalCameraModel::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 verStep = boost::math::constants::pi<double>()/height;
        double verStart = -0.5*boost::math::constants::pi<double>();
        double horStep = 2*boost::math::constants::pi<double>()/width;
        double horStart = 1.5*boost::math::constants::pi<double>();
        for (int y=0; y<height; y++) {
                double cosVer = std::cos(verStart+y*verStep);
                double sinVer = std::sin(verStart+y*verStep);
                for (int x=0; x<width; x++) {
                        tf::Vector3 & pt = (*(ret))[y][x];
                        pt[0] = radius*std::cos(horStart-x*horStep)*cosVer;
                        pt[1] = radius*sinVer;
                        pt[2] = radius*std::sin(horStart-x*horStep)*cosVer;
                }
        }
        return ret;
}


}