srba/srba_options_sensor_pose.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 <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/math/lightweight_geom_data.h>

namespace srba {
namespace options
{
        namespace internal {
            template <class SENSOR_POSE_CLASS, size_t KF_POSE_DIMS> struct resulting_pose_t;
        }

        struct sensor_pose_on_robot_none
        {
            struct parameters_t
            {
            };

            template <class POSE>
            static inline void robot2sensor(const POSE & robot, POSE & sensor, const parameters_t &p) {
                MRPT_UNUSED_PARAM(p);
                sensor = robot;
            }

            template <class KF_POSE>
            static inline void pose_robot2sensor(const KF_POSE & pose_wrt_robot,
                KF_POSE & pose_wrt_sensor, const parameters_t &p) {
                MRPT_UNUSED_PARAM(p);
                pose_wrt_sensor = pose_wrt_robot;
            }

            template <class landmark_t, class POINT>
            static inline void point_robot2sensor(const POINT & pt_wrt_robot,
                POINT & pt_wrt_sensor, const parameters_t &p) {
                MRPT_UNUSED_PARAM(p);
                pt_wrt_sensor=pt_wrt_robot;
            }

            template <class MATRIX>
            static inline void jacob_dh_dx_rotate( MATRIX & dh_dx, const parameters_t &p) {
                MRPT_UNUSED_PARAM(dh_dx);
                MRPT_UNUSED_PARAM(p);
                /* nothing to do, since there's no displacement */
            }
            template <class LANDMARK_T>
            static inline void sensor2robot_point(typename landmark_traits<LANDMARK_T>::array_landmark_t & pt,
                const parameters_t &p) {
                MRPT_UNUSED_PARAM(pt); MRPT_UNUSED_PARAM(p);
                /* nothing to do, since there's no displacement */
            }
        };

        namespace internal {
            template <> struct resulting_pose_t<sensor_pose_on_robot_none,3> {
                typedef mrpt::poses::CPose2D pose_t; };
            template <> struct resulting_pose_t<sensor_pose_on_robot_none,6> {
                typedef mrpt::poses::CPose3D pose_t; };
        }

        struct sensor_pose_on_robot_se3
        {
            struct parameters_t
            {
                mrpt::poses::CPose3D  relative_pose;
            };

            template <class KF_POSE>
            static inline void robot2sensor(const KF_POSE & robot, mrpt::poses::CPose3D & sensor,
                const parameters_t &p) {
                sensor.composeFrom(robot, p.relative_pose);
            }

            template <class KF_POSE>
            static inline void pose_robot2sensor(const KF_POSE & pose_wrt_robot,
                mrpt::poses::CPose3D & pose_wrt_sensor, const parameters_t &p) {
                pose_wrt_sensor.inverseComposeFrom(pose_wrt_robot, p.relative_pose);
            }

            template <class landmark_t,class POINT>
            static inline void point_robot2sensor(const POINT & pt_wrt_robot,
                POINT & pt_wrt_sensor, const parameters_t &p) {
                landmark_t::inverseComposePosePoint(pt_wrt_robot,pt_wrt_sensor,p.relative_pose);
            }

            template <class MATRIX>
            static inline void jacob_dh_dx_rotate( MATRIX & dh_dx, const parameters_t &p) {
                // dh(R2S+x)_dx = dh(x')_dx' * d(x')_dx
                dh_dx = dh_dx * p.relative_pose.getRotationMatrix().transpose();
            }

            template <class LANDMARK_T>
            static inline void sensor2robot_point(typename landmark_traits<LANDMARK_T>::array_landmark_t & pt, const parameters_t &p) {
                landmark_traits<LANDMARK_T>::composePosePoint(pt, p.relative_pose);
            }
        };
        namespace internal {
            template <> struct resulting_pose_t<sensor_pose_on_robot_se3,3> { typedef mrpt::poses::CPose3D pose_t; };
            template <> struct resulting_pose_t<sensor_pose_on_robot_se3,6> { typedef mrpt::poses::CPose3D pose_t; };
        }

} } // End of namespaces