lhm.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
|                     Mobile Robot Programming Toolkit (MRPT)               |
|                          http://www.mrpt.org/                             |
|                                                                           |
| Copyright (c) 2005-2017, 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    |
+---------------------------------------------------------------------------+ */

#ifndef LHM_

#define LHM_

#include <iostream>

#include <mrpt/utils/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/StdVector>


#define TOL_LHM 0.00001
#define EPSILON_LHM 0.00000001

namespace mrpt
{
    namespace vision
    {
        namespace pnp
        {
            /** \addtogroup pnp Perspective-n-Point pose estimation
             *  \ingroup mrpt_vision_grp
             *  @{  
             */
             
            /**
             * @class lhm
             * @author Chandra Mangipudi
             * @date 10/08/16
             * @file lhm.h
             * @brief Lu Hage Mjolsness - Iterative PnP Algorithm (Eigen Implementation
             */
            class lhm
            {

                Eigen::MatrixXd obj_pts; //! Object points in Camera Co-ordinate system
                Eigen::MatrixXd img_pts; //! Image points in pixel co-ordinates
                Eigen::MatrixXd cam_intrinsic; //! Camera intrinsic matrix
                Eigen::MatrixXd P; //! Trnaspose of Object points @obj_pts
                Eigen::MatrixXd Q; //! Transpose of Image points @img_pts

                Eigen::Matrix3d R; //! Matrix for internal computations
                Eigen::Matrix3d G; //! Rotation Matrix 
                Eigen::Vector3d t; //! Translation Vector

                std::vector<Eigen::Matrix3d> F; //! Storage matrix for each point
                double err;  //! Error variable for convergence selection
                double err2; //! Error variable for convergence selection

                int n; //! Number of 2d/3d correspondences

            public:

                //! Constructor for the LHM class
                lhm(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd cam_, int n0);
                //~lhm();
                
                /**
                 * @brief Function to compute pose using LHM PnP algorithm
                 * @param R_ Rotation matrix
                 * @param t_ Trnaslation Vector
                 * @return Success flag
                 */
                bool compute_pose(Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_);

                /**
                 * @brief Function to compute pose during an iteration
                 */
                void absKernel();

                /**
                 * @brief Function to estimate translation given an estimated rotation matrix
                 */
                void estimate_t();

                /**
                 * @brief Transform object points in Body frame (Landmark Frame) to estimated Camera Frame
                 */
                void xform();

                /**
                 * @brief Iternal Function of quaternion
                 * @param q Quaternion
                 * @return 
                 */
                Eigen::Matrix4d qMatQ(Eigen::VectorXd q);

                /**
                 * @brief Iternal Function of quaternion
                 * @param q Quaternion
                 * @return 
                 */
                Eigen::Matrix4d qMatW(Eigen::VectorXd q);
            };
            
            /** @}  */ // end of grouping
            
        }
    }
}
#endif