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