StereoOdometer.hpp

Go to the documentation of this file.

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

#ifndef VISO2_STEREOODOMETER_TASK_HPP
#define VISO2_STEREOODOMETER_TASK_HPP

#include "viso2/StereoOdometerBase.hpp"

#include <opencv/cv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv/highgui.h>

#include "frame_helper/FrameHelper.h" 
#include "frame_helper/FrameHelperTypes.h" 
#include "frame_helper/Calibration.h" 
#include "frame_helper/CalibrationCv.h" 
#include <base/samples/RigidBodyState.hpp>
#include <base/samples/DistanceImage.hpp>

#include <vector>
#include <map>

#include <boost/shared_ptr.hpp> 
#include <boost/circular_buffer.hpp> 
#include <boost/unordered_map.hpp>

#include <viso2/matrix.h>
#include <viso2/viso_stereo.h>
#include <ctime>

#include<Eigen/StdVector>

namespace viso2 {


    struct HashPoint
    {
        size_t idx; /* Index in the point cloud **/
        base::Vector3d point; 
        base::Vector4d color; 
        base::Matrix3d cov; 
        base::Vector3d jacobian; 
    };


    class StereoOdometer : public StereoOdometerBase
    {
        friend class StereoOdometerBase;

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    protected:
        static const int DEFAULT_CIRCULAR_BUFFER_SIZE = 2;

    protected:

        /**************************/
        /*** Property Variables ***/
        /**************************/
        // set most important visual odometry parameters
        // for a full parameter list, look at: viso2/viso_stereo.h
        VisualOdometryStereo::parameters viso2param;

        //Intrinsic and extrinsic parameters for the pinhole camera model
        frame_helper::StereoCalibration cameracalib;

        frame_helper::StereoCalibrationCv cameracalibCv;

        /******************************************/
        /*** General Internal Storage Variables ***/
        /******************************************/
        boost::shared_ptr<VisualOdometryStereo> viso; 
        boost::circular_buffer<base::samples::frame::FramePair> imagePair; 
        frame_helper::FrameHelper frameHelperLeft, frameHelperRight; 
        Eigen::Matrix4d Q; 
        ::base::samples::frame::Frame leftColorImage;
        ::base::Matrix2d pxleftVar, pxrightVar; 
        boost::unordered_map< int32_t, int32_t > hashIdx; 
        boost::circular_buffer< std::map < int32_t, HashPoint, std::less<int32_t>,
        Eigen::aligned_allocator< std::pair < const int32_t, HashPoint > > > > hashPointcloud; 
        /***************************/
        /***************************/
        Eigen::Affine3d pose; 
        base::samples::RigidBodyState poseOut; 
        RTT::extras::ReadOnlyPointer<base::samples::frame::Frame> frame_out; 
    protected:

        virtual void left_frameTransformerCallback(const base::Time &ts, const ::RTT::extras::ReadOnlyPointer< ::base::samples::frame::Frame > &left_frame_sample);
        virtual void right_frameTransformerCallback(const base::Time &ts, const ::RTT::extras::ReadOnlyPointer< ::base::samples::frame::Frame > &right_frame_sample);

    public:
        StereoOdometer(std::string const& name = "viso2::StereoOdometer");

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

        ~StereoOdometer();

        bool configureHook();

        bool startHook();

        void updateHook();

        void errorHook();

        void stopHook();

        void cleanupHook();

        viso2::Viso2Info computeStereoOdometer(const base::Time &ts, const Eigen::Affine3d &tf);

        void drawMatches(const base::samples::frame::Frame &image1, const base::samples::frame::Frame &image2,
                        const std::vector<Matcher::p_match> &matches, const std::vector<int32_t>& inlier_indices, base::samples::frame::Frame &imageOutput);

        void createDistanceImage(const base::samples::frame::Frame &image1, const base::samples::frame::Frame &image2,
                        const std::vector<Matcher::p_match> &matches, const VisualOdometryStereo::parameters &viso2param,
                        base::samples::DistanceImage &distImage);

        void createPointCloud(const Eigen::Affine3d &tf,
                        const base::samples::frame::Frame &image1,
                        const std::vector<Matcher::p_match> &matches,
                        const std::vector<int32_t>& inlier_indices,
                        const Eigen::Matrix4d &Q,
                        const Eigen::Affine3d &deltaPose,
                        boost::unordered_map< int32_t, int32_t > &hashIdx,
                        boost::circular_buffer< std::map < int32_t, HashPoint, std::less<int32_t>,
                                    Eigen::aligned_allocator< std::pair < const int32_t, HashPoint > > > > &hashPointcloud);

        base::Vector3d computeFeaturesJacobian (const Eigen::Affine3d &deltaPose,
                                            const base::Vector3d &point);

        void postProcessPointCloud (boost::unordered_map< int32_t, int32_t > & hashIdx,
                                    boost::circular_buffer< std::map < int32_t, HashPoint, std::less<int32_t>,
                                        Eigen::aligned_allocator< std::pair < const int32_t, HashPoint > > > > &hashPointcloud,
                                    base::samples::Pointcloud &pointcloud,
                                    std::vector<base::Matrix3d> &pointsVar,
                                    std::vector<unsigned int> &pointsIdx,
                                    base::MatrixXd &deltaJacobCurr,
                                    base::MatrixXd &deltaJacobPrev);

    };
}

#endif