1 // This file is part of OpenMVG, an Open Multiple View Geometry C++ library. 2 3 // Copyright (c) 2012, 2013 Pierre MOULON. 4 5 // This Source Code Form is subject to the terms of the Mozilla Public 6 // License, v. 2.0. If a copy of the MPL was not distributed with this 7 // file, You can obtain one at http://mozilla.org/MPL/2.0/. 8 9 #ifndef OPENMVG_GEOMETRY_RIGID_TRANSFORMATION_3D_SRT_HPP 10 #define OPENMVG_GEOMETRY_RIGID_TRANSFORMATION_3D_SRT_HPP 11 12 #include "openMVG/numeric/lm.hpp" 13 #include "openMVG/numeric/eigen_alias_definition.hpp" 14 15 namespace openMVG 16 { 17 namespace geometry 18 { 19 20 /** 3D rigid transformation estimation (7 dof) 21 * Compute a Scale Rotation and Translation rigid transformation. 22 * This transformation provide a distortion-free transformation 23 * using the following formulation Xb = S * R * Xa + t. 24 * "Least-squares estimation of transformation parameters between two point patterns", 25 * Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573 26 * 27 * \param[in] x1 The first 3xN matrix of euclidean points 28 * \param[in] x2 The second 3xN matrix of euclidean points 29 * \param[out] S The scale factor 30 * \param[out] t The 3x1 translation 31 * \param[out] R The 3x3 rotation 32 * 33 * \return true if the transformation estimation has succeeded 34 * 35 * \note Need at least 3 points 36 */ 37 bool FindRTS 38 ( 39 const Mat &x1, 40 const Mat &x2, 41 double * S, 42 Vec3 * t, 43 Mat3 * R 44 ); 45 46 /** 47 * @brief Eigen Levemberg-Marquardt functor to refine translation, Rotation and Scale parameter. 48 */ 49 struct lm_SRTRefine_functor : Functor<double> 50 { 51 /** 52 * @brief Constructor 53 * @param inputs Number of inputs (nb elements to refine) 54 * @param values Number of samples 55 * @param x1 Input samples for first dataset 56 * @param x2 Input samples for second dataset 57 * @param S Scale 58 * @param R Rotation 59 * @param t Translation 60 */ 61 lm_SRTRefine_functor( int inputs, int values, 62 const Mat &x1, const Mat &x2, 63 const double &S, const Mat3 & R, const Vec &t ); 64 65 /** 66 * @brief Computes error given a sample 67 * @param x a Sample 68 * @param[out] fvec Error for each values 69 */ 70 int operator()( const Vec &x, Vec &fvec ) const; 71 72 Mat x1_, x2_; 73 Vec3 t_; 74 Mat3 R_; 75 double S_; 76 }; 77 78 79 /** 80 * @brief Eigen LM functor to refine Rotation. 81 */ 82 struct lm_RRefine_functor : Functor<double> 83 { 84 /** 85 * @brief Constructor 86 * @param inputs Number of inputs (elements to refine) 87 * @param values Number of samples 88 * @param x1 Input samples for first dataset 89 * @param x2 Input samples for second dataset 90 * @param S Scale 91 * @param R Rotation 92 * @param t Translation 93 */ 94 lm_RRefine_functor( int inputs, int values, 95 const Mat &x1, const Mat &x2, 96 const double &S, const Mat3 & R, const Vec &t ); 97 98 /** 99 * @brief Computes error given a sample 100 * @param x a Sample 101 * @param[out] fvec Error for each values 102 */ 103 int operator()( const Vec &x, Vec &fvec ) const; 104 105 Mat x1_, x2_; 106 Vec3 t_; 107 Mat3 R_; 108 double S_; 109 }; 110 111 /** 3D rigid transformation refinement using LM 112 * Refine the Scale, Rotation and translation rigid transformation 113 * using a Levenberg-Marquardt opimization. 114 * 115 * \param[in] x1 The first 3xN matrix of euclidean points 116 * \param[in] x2 The second 3xN matrix of euclidean points 117 * \param[out] S The initial scale factor 118 * \param[out] t The initial 3x1 translation 119 * \param[out] R The initial 3x3 rotation 120 * 121 * \return none 122 */ 123 void Refine_RTS 124 ( 125 const Mat &x1, 126 const Mat &x2, 127 double * S, 128 Vec3 * t, 129 Mat3 * R 130 ); 131 132 } // namespace geometry 133 } // namespace openMVG 134 135 #endif // OPENMVG_GEOMETRY_RIGID_TRANSFORMATION_3D_SRT_HPP 136