srba/models/observations_StereoCamera.h

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#pragma once

#include "observations_MonocularCamera.h"


#include <mrpt/utils/TCamera.h>
#include <mrpt/utils/TStereoCamera.h>
#include <mrpt/tfest.h>  // for use in landmark_matcher<>

namespace srba {
namespace observations {
    struct StereoCamera
    {
        static const size_t  OBS_DIMS = 4; 
        
        struct obs_data_t
        {
            mrpt::utils::TPixelCoordf  left_px, right_px;

            template <class ARRAY>
            inline void getAsArray(ARRAY &obs) const {
                obs[0] = left_px.x;  obs[1] = left_px.y;
                obs[2] = right_px.x; obs[3] = right_px.y;
            }
        };

        struct TObservationParams
        {
            mrpt::utils::TStereoCamera camera_calib;
        };
    };

    template <> struct landmark_matcher<StereoCamera>
    {
        template <class POSE>
        static bool find_relative_pose(
            const mrpt::aligned_containers<StereoCamera::obs_data_t>::vector_t & new_kf_obs,
            const mrpt::aligned_containers<StereoCamera::obs_data_t>::vector_t & old_kf_obs,
            const StereoCamera::TObservationParams &params,
            POSE &pose_new_kf_wrt_old_kf
            )
        {
            ASSERT_(new_kf_obs.size()==old_kf_obs.size())
            const size_t N=new_kf_obs.size();
            // project stereo points to 3D and use them to find out the relative pose:
            const double cx = params.camera_calib.leftCamera.cx(), cy = params.camera_calib.leftCamera.cy(), baseline = params.camera_calib.rightCameraPose.x(), f = params.camera_calib.leftCamera.fx();
            mrpt::utils::TMatchingPairList matches;
            matches.reserve(N);
            for (size_t i=0;i<N;i++)
            {
                // Point 1:
                const double disparity_old = old_kf_obs[i].left_px.x - old_kf_obs[i].right_px.x;
                if (disparity_old<=.0) continue; // Invalid 3D point
                const mrpt::math::TPoint3D pt_old(
                    ( old_kf_obs[i].left_px.x - cx )*baseline/disparity_old,
                    ( old_kf_obs[i].left_px.y - cy )*baseline/disparity_old,
                    f*baseline/disparity_old );
                // Point 2:
                const double disparity_new = new_kf_obs[i].left_px.x - new_kf_obs[i].right_px.x;
                if (disparity_new<=.0) continue; // Invalid 3D point
                const mrpt::math::TPoint3D pt_new(
                    ( new_kf_obs[i].left_px.x - cx )*baseline/disparity_new,
                    ( new_kf_obs[i].left_px.y - cy )*baseline/disparity_new,
                    f*baseline/disparity_new );

                matches.push_back( mrpt::utils::TMatchingPair(i,i, pt_old.x,pt_old.y,pt_old.z, pt_new.x,pt_new.y,pt_new.z ) );
            }
            // Least-square optimal transformation:
            if (POSE::rotation_dimensions==2)
            { // SE(2)
                mrpt::math::TPose2D found_pose;
                if (!mrpt::tfest::se2_l2(matches,found_pose))
                    return false;
                pose_new_kf_wrt_old_kf = POSE( mrpt::poses::CPose2D(found_pose));
            }
            else
            {  // SE(3)
                mrpt::poses::CPose3DQuat found_pose;
                double found_scale;
                if (!mrpt::tfest::se3_l2(matches,found_pose,found_scale))
                    return false;
                pose_new_kf_wrt_old_kf = POSE(found_pose);
            }
            return true;
        }
    };

}
} // end NS