DistanceImage.hpp

Go to the documentation of this file.

#ifndef __BASE_SAMPLES_DISTANCE_IMAGE_H__
#define __BASE_SAMPLES_DISTANCE_IMAGE_H__

#include <base/Time.hpp>
#include <base/Eigen.hpp>
#include "Pointcloud.hpp"
#include <vector>
#include <boost/math/special_functions/fpclassify.hpp>

namespace base
{
namespace samples
{
    struct DistanceImage
    {
        base::Time time;

        uint16_t width;
        uint16_t height;

        typedef float scalar;
        scalar scale_x;
        scalar scale_y;

        scalar center_x;
        scalar center_y;

        std::vector<scalar> data;

        DistanceImage(): width(0), height(0), scale_x(1.0), scale_y(1.0), center_x(0.0), center_y(0.0)
        {
        }

        DistanceImage(uint16_t width, uint16_t height): width(width), height(height), scale_x(1.0), scale_y(1.0), center_x(0.0), center_y(0.0)
        {
        }

        
        void clear();

        template <class Scalar_>
        bool getScenePoint( size_t x, size_t y, Eigen::Matrix<Scalar_,3,1>& point ) const
        {
            // check bounds. x and y are always positive
            if( (x >= width) || (y >= height) )
                return false;

            // only process vector if distance value is not NaN or inf
            const scalar d = data[width*y+x];
            if( boost::math::isnormal( d ) ) 
            {
                point = Eigen::Matrix<Scalar_,3,1>( (x*scale_x)+center_x, (y*scale_y)+center_y, 1.0 );
                point *= d;
                return true;
            }
            return false;
        }

        template <class Scalar_>
        bool getImagePoint( const Eigen::Matrix<Scalar_,3,1>& point, size_t &x, size_t &y ) const 
        {
            if( point.z() <= 0 )
                return false;

            size_t u = ((point.x() / point.z()) - center_x) / scale_x;
            size_t v = ((point.y() / point.z()) - center_y) / scale_y;

            // check bounds. u and v are always positive
            if( (u >= width) || (v >= height) )
                return false;

            x = u;
            y = v;

            return true;
        }

        base::samples::Pointcloud getPointCloud() const;
        
        template <class Scalar_>
        Eigen::Matrix<Scalar_,3,3> getIntrinsic() const
        {
            // parameters in this class give the inverse
            // so, we need to invert them
            Eigen::Matrix<Scalar_,3,3> result;
            result << 
                1.0/scale_x, 0, -center_x/scale_x,
                0, 1.0/scale_y, -center_y/scale_y,
                0, 0, 1;

            return result;
        }

        void setIntrinsic( double f_x, double f_y, double c_x, double c_y );

        void setSize( uint16_t width, uint16_t height );
    };
}
}
#endif