ConverterBase.hpp

Go to the documentation of this file.

/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */

#ifndef DEPTH_MAP_PREPROCESSING_CONVERTERBASE_TASK_HPP
#define DEPTH_MAP_PREPROCESSING_CONVERTERBASE_TASK_HPP

#include "depth_map_preprocessing/ConverterBaseBase.hpp"
#include <depth_map_preprocessing/PointCloudConversion.hpp>

#include <Eigen/StdVector>

namespace depth_map_preprocessing{

    class ConverterBase : public ConverterBaseBase
    {
        friend class ConverterBaseBase;
    protected:

        struct SampleTransforms
        {
            base::Time sample_time_id;
            PointCloudConversion::TransformationVector laser_in_odometry;

            SampleTransforms(const base::Time& sample_time_id) : sample_time_id(sample_time_id) {}

            void clear()
            {
                sample_time_id.microseconds = 0;
                laser_in_odometry.clear();
            }
        };

        virtual void newSampleCallback(const base::Time &ts, const base::samples::DepthMap &depth_map_sample) = 0;

        virtual void depth_mapAcquisitionTimeTransformerCallback(const base::Time &ts, const base::Time &depth_map_sample_start);

        virtual void depth_mapTransformerCallback(const base::Time &ts, const base::samples::DepthMap &depth_map_sample);

        virtual void pullPorts();

        template<class T>
        bool convertToPointCloud(const base::Time &ts, const base::samples::DepthMap &depth_map_sample,
                                std::vector<T>& pointcloud);

        void registerAcquisitionTimeStream();

        template<class ElementIterator>
        void pushDepthMapTimestamps(ElementIterator it_first, ElementIterator it_last, const base::samples::DepthMap &depth_map_sample);

    protected:

        int depth_map_acquisition_times_idx_tr;
        std::list<SampleTransforms> depth_map_acquisition_transforms;
        MotionCompensation motion_compensation;
        unsigned acquisition_timestamps;

    public:
        ConverterBase(std::string const& name = "depth_map_preprocessing::ConverterBase");

        ConverterBase(std::string const& name, RTT::ExecutionEngine* engine);

        ~ConverterBase();

        bool configureHook();

        bool startHook();

        void updateHook();

        void errorHook();

        void stopHook();

        void cleanupHook();
    };


template<class T>
bool ConverterBase::convertToPointCloud(const base::Time &ts, const base::samples::DepthMap &depth_map_sample,
                        std::vector<T>& pointcloud)
{
    if(motion_compensation == NoCompensation)
    {
        depth_map_sample.convertDepthMapToPointCloud(pointcloud, true);
        return true;
    }

    // drop older acquisition transforms sets
    std::list<SampleTransforms>::iterator acquisition_transforms = depth_map_acquisition_transforms.begin();
    while(acquisition_transforms->sample_time_id < depth_map_sample.time && depth_map_acquisition_transforms.size() > 1)
        acquisition_transforms = depth_map_acquisition_transforms.erase(acquisition_transforms);

    // check if acquisition transforms are the right ones
    if(acquisition_transforms->sample_time_id == depth_map_sample.time)
    {
        // convert scan to pointcloud with motion compensation
        Eigen::Affine3d depth_map_end_transform;
        if(_laser2odometry.get(ts, depth_map_end_transform, true))
        {
            acquisition_transforms->laser_in_odometry.push_back(depth_map_end_transform);
            return PointCloudConversion::convertToPointCloud(depth_map_sample, acquisition_transforms->laser_in_odometry,
                                                             motion_compensation, pointcloud);
        }
    }
    else
        LOG_DEBUG_S << "No acquisition transforms for the current sample available!";

    LOG_ERROR_S << "Failed to convert depth map to point cloud, missing scan acquisition transformations!";
    return false;
}

template<class ElementIterator>
void ConverterBase::pushDepthMapTimestamps(ElementIterator it_first, ElementIterator it_last, const base::samples::DepthMap& depth_map_sample)
{
    for(;it_first != it_last; it_first++)
        _transformer.pushData(depth_map_acquisition_times_idx_tr, *it_first, depth_map_sample.time);
    _transformer.pushData(depth_map_idx_tr, *it_last, depth_map_sample);
}

}

#endif