srba/models/landmarks.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 <srba/landmark_render_models.h>
#include <srba/landmark_jacob_families.h>

namespace srba {
namespace landmarks {

    struct Euclidean3D
    {
        static const size_t  LM_DIMS = 3; 
        static const landmark_jacob_family_t  jacob_family = jacob_point_landmark;  
        //static const size_t  LM_EUCLIDEAN_DIMS = 3; //!< Either 2 or 3, depending on the real minimum number of coordinates needed to parameterize the landmark.
        typedef srba::landmark_rendering_as_point render_mode_t;

        template <class VECTOR> inline static void relativeEuclideanLocation(const VECTOR &posParams, mrpt::math::TPoint3D &posEuclidean)
        {
            posEuclidean.x = posParams[0];
            posEuclidean.y = posParams[1];
            posEuclidean.z = posParams[2];
        }

        template <class POSE,class VECTOR>
        inline static void composePosePoint(VECTOR & pt, const POSE & pose) {
            pose.composePoint(pt[0],pt[1],pt[2], pt[0],pt[1],pt[2]);
        }
        template <class POSE,class VECTOR>
        static inline void inverseComposePosePoint(const VECTOR &lm_global,VECTOR &lm_local,const POSE &pose) {
            pose.inverseComposePoint(lm_global[0],lm_global[1],lm_global[2], lm_local[0],lm_local[1],lm_local[2]);
        }
    };

    struct Euclidean2D
    {
        static const size_t  LM_DIMS = 2; 
        static const landmark_jacob_family_t  jacob_family = jacob_point_landmark;  
        //static const size_t  LM_EUCLIDEAN_DIMS = 2; //!< Either 2 or 3, depending on the real minimum number of coordinates needed to parameterize the landmark.
        typedef srba::landmark_rendering_as_point render_mode_t;

        template <class VECTOR> inline static void relativeEuclideanLocation(const VECTOR &posParams, mrpt::math::TPoint3D &posEuclidean)
        {
            posEuclidean.x = posParams[0];
            posEuclidean.y = posParams[1];
            posEuclidean.z = 0;
        }

        template <class POSE,class VECTOR>
        inline static void composePosePoint(VECTOR & pt, const POSE & pose) {
            double lx,ly,lz;
            pose.composePoint(pt[0],pt[1],0, lx,ly,lz);
            pt[0]=lx; pt[1]=ly;
            ASSERTMSG_(std::abs(lz)<1e-2, "Error: Using a 3D transformation to obtain a 2D point but it results in |z|>eps")
        }
        template <class POSE,class VECTOR>
        static inline void inverseComposePosePoint(const VECTOR &lm_global,VECTOR &lm_local,const POSE &pose) {
            pose.inverseComposePoint(lm_global[0],lm_global[1], lm_local[0],lm_local[1]);
        }
    };

    struct RelativePoses2D
    {
        static const size_t  LM_DIMS = 3; 
        static const landmark_jacob_family_t  jacob_family = jacob_relpose_landmark;  
        //static const size_t  LM_EUCLIDEAN_DIMS = 3; //!< Either 2 or 3, depending on the real minimum number of coordinates needed to parameterize the landmark.
        typedef srba::landmark_rendering_as_pose_constraints render_mode_t;

        template <class POSE,class VECTOR>
        inline static void composePosePoint(VECTOR & pt, const POSE & pose) {
            MRPT_UNUSED_PARAM(pt);
            MRPT_UNUSED_PARAM(pose);
            // Not applicable: nothing to do on "pt"
        }
        template <class POSE,class VECTOR>
        static inline void inverseComposePosePoint(const VECTOR &lm_global,VECTOR &lm_local,const POSE &pose) {
            // Not applicable
            MRPT_UNUSED_PARAM(pose);
            lm_local=lm_global;
        }

    };

    struct RelativePoses3D
    {
        static const size_t  LM_DIMS = 6; 
        static const landmark_jacob_family_t  jacob_family = jacob_relpose_landmark;  
        //static const size_t  LM_EUCLIDEAN_DIMS = 3; //!< Either 2 or 3, depending on the real minimum number of coordinates needed to parameterize the landmark.
        typedef srba::landmark_rendering_as_pose_constraints render_mode_t;

        template <class POSE,class VECTOR>
        inline static void composePosePoint(VECTOR & pt, const POSE & pose) {
            MRPT_UNUSED_PARAM(pt);
            MRPT_UNUSED_PARAM(pose);
            // Not applicable: nothing to do on "pt"
        }
        template <class POSE,class VECTOR>
        static inline void inverseComposePosePoint(const VECTOR &lm_global,VECTOR &lm_local,const POSE &pose) {
            // Not applicable
            MRPT_UNUSED_PARAM(pose);
            lm_local=lm_global;
        }

    };

}
} // end NS