LaserScan.hpp

Go to the documentation of this file.

#ifndef BASE_SAMPLES_LASER_H__
#define BASE_SAMPLES_LASER_H__

#include <vector>
#include <Eigen/Geometry>
#include <base/Float.hpp>
#include <boost/cstdint.hpp>
#include <base/Time.hpp>
#include <base/Deprecated.hpp>

namespace base { namespace samples {
    enum LASER_RANGE_ERRORS {
        TOO_FAR            = 1, // too far
        TOO_NEAR           = 2,
        MEASUREMENT_ERROR  = 3,
        OTHER_RANGE_ERRORS = 4,
        MAX_RANGE_ERROR    = 5,
        END_LASER_RANGE_ERRORS
    };

    struct LaserScan {
        typedef boost::uint32_t uint32_t;

        Time time;

        double start_angle;

        double angular_resolution;

        double speed;

        std::vector<uint32_t> ranges;

        uint32_t minRange;
        
        uint32_t maxRange;
        
        std::vector<float> remission;

        LaserScan()
            : start_angle(0), angular_resolution(0), speed(0), minRange(0), maxRange(0) {}
            
        bool isValidBeam(const unsigned int i) const;
        
        //resets the sample
        void reset();

        bool isRangeValid(uint32_t range) const;

        template<typename T>
        void convertScanToPointCloud(std::vector<T> &points,
                                     const Eigen::Affine3d& transform = Eigen::Affine3d::Identity(),
                                     bool skip_invalid_points = true)const
        {
            points.clear();
            
            //give the vector a hint about the size it might be
            points.reserve(ranges.size());

            //this is optimized for runtime
            //moving the check for skip_invalid_points
            //out of the loop speeds up the execution 
            //time by ~25 %
            if(!skip_invalid_points)
            {
                for(unsigned int i = 0; i < ranges.size(); i++) {
                    Eigen::Vector3d point;
                    if(getPointFromScanBeamXForward(i, point)) {
                        point = transform * point;
                        points.push_back(point);
                    } else {
                        points.push_back(Eigen::Vector3d(base::unknown<double>(), base::unknown<double>(), base::unknown<double>()));
                    }
                }
            } else {
                for(unsigned int i = 0; i < ranges.size(); i++) {
                    Eigen::Vector3d point;
                    if(getPointFromScanBeamXForward(i, point)) {
                        point = transform * point;
                        points.push_back(point);
                    }
                }
            }
        }
            
        bool getPointFromScanBeamXForward(const unsigned int i, Eigen::Vector3d &point) const;

        bool getPointFromScanBeam(const unsigned int i, Eigen::Vector3d &point) const;

        std::vector<Eigen::Vector3d> convertScanToPointCloud(const Eigen::Affine3d& transform) const BASE_TYPES_DEPRECATED;
    };
}} // namespaces

#endif