1 // This file is part of OpenMVG, an Open Multiple View Geometry C++ library. 2 3 // Copyright (c) 2015 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_POSE3_HPP 10 #define OPENMVG_GEOMETRY_POSE3_HPP 11 12 #include "openMVG/multiview/projection.hpp" 13 14 namespace openMVG 15 { 16 namespace geometry 17 { 18 19 /** 20 * @brief Defines a pose in 3d space 21 * [R|C] t = -RC 22 */ 23 class Pose3 24 { 25 protected: 26 27 /// Orientation matrix 28 Mat3 rotation_; 29 30 /// Center of rotation 31 Vec3 center_; 32 33 public: 34 35 /** 36 * @brief Constructor 37 * @param r Rotation 38 * @param c Center 39 * @note Default (without args) defines an Identity pose. 40 */ Pose3(const Mat3 & r=std::move (Mat3::Identity ()),const Vec3 & c=std::move (Vec3::Zero ()))41 Pose3 42 ( 43 const Mat3& r = std::move(Mat3::Identity()), 44 const Vec3& c = std::move(Vec3::Zero()) 45 ) 46 : rotation_( r ), center_( c ) {} 47 48 /** 49 * @brief Get Rotation matrix 50 * @return Rotation matrix 51 */ rotation() const52 const Mat3& rotation() const 53 { 54 return rotation_; 55 } 56 57 /** 58 * @brief Get Rotation matrix 59 * @return Rotation matrix 60 */ rotation()61 Mat3& rotation() 62 { 63 return rotation_; 64 } 65 66 /** 67 * @brief Get center of rotation 68 * @return center of rotation 69 */ center() const70 const Vec3& center() const 71 { 72 return center_; 73 } 74 75 /** 76 * @brief Get center of rotation 77 * @return Center of rotation 78 */ center()79 Vec3& center() 80 { 81 return center_; 82 } 83 84 /** 85 * @brief Get translation vector 86 * @return translation vector 87 * @note t = -RC 88 */ translation() const89 inline Vec3 translation() const 90 { 91 return -( rotation_ * center_ ); 92 } 93 94 95 /** 96 * @brief Apply pose 97 * @param p Point 98 * @return transformed point 99 */ 100 template<typename T> operator ()(const T & p) const101 inline typename T::PlainObject operator() (const T& p) const 102 { 103 return rotation_ * ( p.colwise() - center_ ); 104 } 105 /// Specialization for Vec3 operator ()(const Vec3 & p) const106 inline typename Vec3::PlainObject operator() (const Vec3& p) const 107 { 108 return rotation_ * ( p - center_ ); 109 } 110 111 112 /** 113 * @brief Composition of poses 114 * @param P a Pose 115 * @return Composition of current pose and parameter pose 116 */ operator *(const Pose3 & P) const117 Pose3 operator * ( const Pose3& P ) const 118 { 119 return {rotation_ * P.rotation_, 120 P.center_ + P.rotation_.transpose() * center_}; 121 } 122 123 124 /** 125 * @brief Get inverse of the pose 126 * @return Inverse of the pose 127 */ inverse() const128 Pose3 inverse() const 129 { 130 return {rotation_.transpose(), -( rotation_ * center_ )}; 131 } 132 133 /** 134 * @brief Return the pose as a single Mat34 matrix [R|t] 135 * @return The pose as a Mat34 matrix 136 */ asMatrix() const137 inline Mat34 asMatrix() const 138 { 139 return (Mat34() << rotation_, translation()).finished(); 140 } 141 142 /** 143 * Serialization out 144 * @param ar Archive 145 */ 146 template <class Archive> 147 inline void save( Archive & ar ) const; 148 149 /** 150 * @brief Serialization in 151 * @param ar Archive 152 */ 153 template <class Archive> 154 inline void load( Archive & ar ); 155 }; 156 } // namespace geometry 157 } // namespace openMVG 158 159 #endif // OPENMVG_GEOMETRY_POSE3_HPP 160