├── CMakeLists.txt
├── README.md
└── include
    ├── Pose.hpp
    ├── Pose_imp.hpp
    └── Quaternion.hpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | set(PROJECT_NAME JPL_SE3_Pose)
 3 | project(${PROJECT_NAME})
 4 | 
 5 | set(CMAKE_CXX_FLAGS "-std=c++11 -O3")
 6 | 
 7 | 
 8 | find_package(Eigen3  REQUIRED)
 9 | include_directories(include 
10 |         ${EIGEN3_INCLUDE_DIR})
11 | 
12 | # test
13 | #add_executable(test test/test.cpp)
14 | 
15 | 
16 | 
17 | 
18 | 
19 | 
20 | 
21 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # JPL_SE3_Pose
2 | SE3 pose lib uses JPL quaternion representing rotation.  Headers only.
3 | 


--------------------------------------------------------------------------------
/include/Pose.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef POSE_H
  2 | #define POSE_H
  3 | #include "Quaternion.hpp"
  4 | 
  5 | 
  6 | 
  7 | 
  8 | /// \brief Implements sin(x)/x for all x in R.
  9 | /// @param[in] x The argument of the sinc function.
 10 | /// \return The result.
 11 | template <typename T>
 12 | T sinc(T x);
 13 | 
 14 | /// \brief Implements the exponential map for quaternions.
 15 | /// @param[in] dAlpha a axis*angle (minimal) input in tangent space.
 16 | /// \return The corresponding Quaternion.
 17 | template <typename T>
 18 | Eigen::Matrix<T,4,1> deltaQ(const Eigen::Matrix<T,3,1>& dAlpha);
 19 | 
 20 | /// \brief Right Jacobian, see Forster et al. RSS 2015 eqn. (8)
 21 | template <typename T>
 22 | Eigen::Matrix<T,3,3> rightJacobian(const  Eigen::Matrix<T,3,1> & PhiVec);
 23 | 
 24 | /// \brief A class that does homogeneous transformations.
 25 | /// This relates a frame A and B: T_AB; it consists of
 26 | ///   translation r_AB (represented in frame A) and
 27 | ///   Quaternion q_AB (as an Eigen Quaternion).
 28 | /// see also the RSS'13 / IJRR'14 paper or the Thesis.
 29 | /// Follows some of the functionality of the SchweizerMesser library by Paul Furgale,
 30 | /// but uses Eigen quaternions underneath.
 31 | /// \warning This means the convention is different to SchweizerMesser
 32 | ///          and the RSS'13 / IJRR'14 paper / the Thesis
 33 | 
 34 | template <typename T>
 35 | class Pose {
 36 | public:
 37 |     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 38 | 
 39 |     /// \brief Default constructor: initialises a unit transformation.
 40 |     Pose();
 41 | 
 42 |     /// \brief Copy constructor: nothing fancy but takes care
 43 |     ///        of not doing bad stuff with internal caching.
 44 |     Pose(const Pose & other);
 45 | 
 46 |     /// \brief Move constructor: nothing fancy but takes care
 47 |     ///        of not doing bad stuff with internal caching.
 48 |     Pose(Pose && other);
 49 | 
 50 |     /// \brief Construct from a translation and quaternion.
 51 |     /// @param[in] r_AB The translation r_AB (represented in frame A).
 52 |     /// @param[in] q_AB The Quaternion q_AB .
 53 |     Pose(const Eigen::Matrix<T,3,1> & r_AB, const Eigen::Matrix<T,4,1>& q_AB);
 54 | 
 55 |     Pose(const Eigen::Matrix<T,3,1> & r_AB, const Eigen::Quaternion<T>& q_AB);
 56 | 
 57 |     /// \brief Construct from a 7x1 vector.
 58 |     /// @param[in] vec The 7x1 vector
 59 |     Pose(const Eigen::Matrix<T,7,1>& vec);
 60 | 
 61 | 
 62 |     /// \brief Construct from a homogeneous transformation matrix.
 63 |     /// @param[in] T_AB The homogeneous transformation matrix.
 64 |     explicit Pose(const Eigen::Matrix<T,4,4> & T_AB);
 65 | 
 66 |     /// \brief Trivial destructor.
 67 |     ~Pose();
 68 | 
 69 |     /// \brief Parameter setting, all 7.
 70 |     /// \tparam Derived_coeffs Deducible matrix type.
 71 |     /// @param[in] coeffs The parameters as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).
 72 |     template<typename Derived_coeffs>
 73 |     bool setCoeffs(const Eigen::MatrixBase<Derived_coeffs> & coeffs);
 74 | 
 75 |     /// \brief Parameter setting, all 7.
 76 |     /// \tparam Derived_coeffs Deducible matrix type.
 77 |     /// @param[in] parameters The parameters as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).
 78 |     template<typename Derived_coeffs>
 79 |     bool setParameters(const Eigen::MatrixBase<Derived_coeffs> & parameters)
 80 |     {
 81 |         return setCoeffs(parameters);
 82 |     }
 83 | 
 84 |     /// \brief The underlying homogeneous transformation matrix.
 85 |     Eigen::Matrix<T,4,4> Transformation() const;
 86 | 
 87 |     /// \brief Returns the rotation matrix (cached).
 88 |     const Eigen::Matrix<T,3,3> & C() const;
 89 | 
 90 |     /// \brief Returns the translation vector r_AB (represented in frame A).
 91 |     const Eigen::Map<Eigen::Matrix<T,3,1>> & r() const;
 92 | 
 93 |     /// \brief Returns the Quaternion q_AB .
 94 |     const Eigen::Map<Eigen::Matrix<T,4,1>> & q() const;
 95 | 
 96 |     /// \brief Get the upper 3x4 part of the homogeneous transformation matrix T_AB.
 97 |     Eigen::Matrix<T, 3, 4> T3x4() const;
 98 | 
 99 |     /// \brief The coefficients (parameters) as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).
100 |     const Eigen::Matrix<T, 7, 1> & coeffs() const
101 |     {
102 |         return parameters_;
103 |     }
104 | 
105 |     /// \brief The parameters (coefficients) as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).
106 |     const Eigen::Matrix<T, 7, 1> & parameters() const
107 |     {
108 |         return parameters_;
109 |     }
110 | 
111 |     /// \brief Get the parameters --- support for ceres.
112 |     /// \warning USE WITH CARE!
113 |     T* parameterPtr()
114 |     {
115 |         return &parameters_[0];
116 |     }
117 |     T* data()
118 |     {
119 |         return &parameters_[0];
120 |     }
121 | 
122 |     /// \brief Set this to a random transformation.
123 |     void setRandom();
124 |     /// \brief Set this to a random transformation with bounded rotation and translation.
125 |     /// @param[in] translationMaxMeters Maximum translation [m].
126 |     /// @param[in] rotationMaxRadians Maximum rotation [rad].
127 |     void setRandom(T translationMaxMeters, T rotationMaxRadians);
128 | 
129 |     /// \brief Set from a homogeneous transformation matrix.
130 |     /// @param[in] T_AB The homogeneous transformation matrix.
131 |     void set(const Eigen::Matrix<T,4,4> & T_AB);
132 | 
133 |     /// \brief Set from a translation and quaternion.
134 |     /// @param[in] r_AB The translation r_AB (represented in frame A).
135 |     /// @param[in] q_AB The Quaternion q_AB .
136 |     void set(const Eigen::Matrix<T,3,1> & r_AB, const Eigen::Matrix<T,4,1>& q_AB);
137 | 
138 |     /// \brief Set this transformation to identity
139 |     void setIdentity();
140 | 
141 |     /// \brief Get an identity transformation
142 |     static Pose Identity();
143 | 
144 |     /// \brief Returns a copy of the transformation inverted.
145 |     Pose inverse() const;
146 | 
147 |     // operator* (group operator)
148 |     /// \brief Multiplication with another transformation object.
149 |     /// @param[in] rhs The right-hand side transformation for this to be multiplied with.
150 |     Pose operator*(const Pose & rhs) const;
151 | 
152 |     /// \brief Transform a direction as v_A = C_AB*v_B (with rhs = hp_B)..
153 |     /// \warning This only applies the rotation!
154 |     /// @param[in] rhs The right-hand side direction for this to be multiplied with.
155 |     Eigen::Matrix<T,3,1> operator*(const Eigen::Matrix<T,3,1> & rhs) const;
156 | 
157 |     /// \brief Transform a homogenous point as hp_B = T_AB*hp_B (with rhs = hp_B).
158 |     /// @param[in] rhs The right-hand side direction for this to be multiplied with.
159 |     Eigen::Matrix<T,4,1> operator*(const Eigen::Matrix<T,4,1> & rhs) const;
160 | 
161 |     /// \brief Assignment -- copy. Takes care of proper caching.
162 |     /// @param[in] rhs The rhs for this to be assigned to.
163 |     Pose& operator=(const Pose & rhs);
164 | 
165 |     /// \brief Apply a small update with delta being 6x1.
166 |     /// \tparam Derived_delta Deducible matrix type.
167 |     /// @param[in] delta The 6x1 minimal update.
168 |     /// \return True on success.
169 |     template<typename Derived_delta>
170 |     bool oplus(const Eigen::MatrixBase<Derived_delta> & delta);
171 | 
172 |     /// \brief Apply a small update with delta being 6x1 --
173 |     ///        the Jacobian is a 7 by 6 matrix.
174 |     /// @param[in] delta The 6x1 minimal update.
175 |     /// @param[out] jacobian The output Jacobian.
176 |     /// \return True on success.
177 |     template<typename Derived_delta, typename Derived_jacobian>
178 |     bool oplus(const Eigen::MatrixBase<Derived_delta> & delta,
179 |                const Eigen::MatrixBase<Derived_jacobian> & jacobian);
180 | 
181 |     /// \brief Get the Jacobian of the oplus operation (a 7 by 6 matrix).
182 |     /// @param[out] jacobian The output Jacobian.
183 |     /// \return True on success.
184 |     template<typename Derived_jacobian>
185 |     bool oplusJacobian(
186 |             const Eigen::MatrixBase<Derived_jacobian> & jacobian) const;
187 | 
188 |     /// \brief Gets the jacobian dx/dChi,
189 |     ///        i.e. lift the minimal Jacobian to a full one (as needed by ceres).
190 |     // @param[out] jacobian The output lift Jacobian (6 by 7 matrix).
191 |     /// \return True on success.
192 |     template<typename Derived_jacobian>
193 |     bool liftJacobian(const Eigen::MatrixBase<Derived_jacobian> & jacobian) const;
194 | 
195 |     Eigen::Matrix<T,4,1> rotation() const;
196 |     Eigen::Matrix<T,3,1> translation() const;
197 | 
198 | protected:
199 |     /// \brief Update the caching of the rotation matrix.
200 |     void updateC();
201 |     Eigen::Matrix<T, 7, 1> parameters_;  ///< Concatenated parameters [r;q].
202 |     Eigen::Map<Eigen::Matrix<T,3,1>> r_;  ///< Translation {_A}r_{B}.
203 |     Eigen::Map<Eigen::Matrix<T,4,1>> q_;  ///< Quaternion q_{AB}.
204 |     Eigen::Matrix<T,3,3> C_; ///< The cached DCM C_{AB}.
205 | };
206 | 
207 | #include "Pose_imp.hpp"
208 | 
209 | #endif
210 | 


--------------------------------------------------------------------------------
/include/Pose_imp.hpp:
--------------------------------------------------------------------------------
  1 | 
  2 | /*********************************************************************************
  3 |  *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
  4 |  *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
  5 |  *
  6 |  *  Redistribution and use in source and binary forms, with or without
  7 |  *  modification, are permitted provided that the following conditions are met:
  8 |  *
  9 |  *   * Redistributions of source code must retain the above copyright notice,
 10 |  *     this list of conditions and the following disclaimer.
 11 |  *   * Redistributions in binary form must reproduce the above copyright notice,
 12 |  *     this list of conditions and the following disclaimer in the documentation
 13 |  *     and/or other materials provided with the distribution.
 14 |  *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 15 |  *     its contributors may be used to endorse or promote products derived from
 16 |  *     this software without specific prior written permission.
 17 |  *
 18 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 19 |  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 20 |  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 21 |  *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 22 |  *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 23 |  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 24 |  *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 25 |  *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 26 |  *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 27 |  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 28 |  *  POSSIBILITY OF SUCH DAMAGE.
 29 |  *
 30 |  *  Created on: Dec 2, 2014
 31 |  *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 32 |  *********************************************************************************/
 33 | 
 34 | /**
 35 |  * @file kinematics/Transformation.hpp
 36 |  * @brief Header file for the Transformation class.
 37 |  * @author Stefan Leutenegger
 38 |  */
 39 | 
 40 | /**
 41 |  * @file Pose.hpp
 42 |  * @brief Header file for the Pose class.
 43 |  * @author Modified By Pang Fumin
 44 |  */
 45 | template <typename T>
 46 | __inline__ T sinc(T x) {
 47 |     if (fabs(x) > 1e-6) {
 48 |         return sin(x) / x;
 49 |     } else {
 50 |         static const T c_2 = T(1.0 / 6.0);
 51 |         static const T c_4 = T(1.0 / 120.0);
 52 |         static const T c_6 = T(1.0 / 5040.0);
 53 |         const T x_2 = x * x;
 54 |         const T x_4 = x_2 * x_2;
 55 |         const T x_6 = x_2 * x_2 * x_2;
 56 |         return T(1.0) - c_2 * x_2 + c_4 * x_4 - c_6 * x_6;
 57 |     }
 58 | }
 59 | template <typename T>
 60 | __inline__ Eigen::Matrix<T,4,1> deltaQ(const Eigen::Matrix<T,3,1>& dAlpha)
 61 | {
 62 |     Eigen::Matrix<T,4,1> dq;
 63 |     T halfnorm = T(0.5) * dAlpha.template tail<3>().norm();
 64 |     dq.template head<3>() = sinc(halfnorm) * 0.5 * dAlpha.template tail<3>();
 65 |     dq[3] = cos(halfnorm);
 66 |     return dq;
 67 | }
 68 | 
 69 | // Right Jacobian, see Forster et al. RSS 2015 eqn. (8)
 70 | template <typename T>
 71 | __inline__ Eigen::Matrix<T,3,3> rightJacobian(const Eigen::Matrix<T,3,1> & PhiVec) {
 72 |     const T Phi = PhiVec.norm();
 73 |     Eigen::Matrix<T,3,3> retMat = Eigen::Matrix<T,3,3>::Identity();
 74 |     const  Eigen::Matrix<T,3,3> Phi_x = crossMat(PhiVec);
 75 |     const  Eigen::Matrix<T,3,3> Phi_x2 = Phi_x*Phi_x;
 76 |     if(Phi < T(1.0e-4)) {
 77 |         retMat += T(-0.5)*Phi_x + T(1.0/6.0)*Phi_x2;
 78 |     } else {
 79 |         const T Phi2 = Phi*Phi;
 80 |         const T Phi3 = Phi2*Phi;
 81 |         retMat += -(T(1.0)-cos(Phi))/(Phi2)*Phi_x + (Phi-sin(Phi))/Phi3*Phi_x2;
 82 |     }
 83 |     return retMat;
 84 | }
 85 | 
 86 | template <typename T>
 87 | inline Pose<T>::Pose(const Pose & other)
 88 |         : parameters_(other.parameters_),
 89 |           r_(&parameters_[0]),
 90 |           q_(&parameters_[3]),
 91 |           C_(other.C_) {
 92 | 
 93 | }
 94 | template <typename T>
 95 | inline Pose<T>::Pose(Pose && other)
 96 |         : parameters_(std::move(other.parameters_)),
 97 |           r_(&parameters_[0]),
 98 |           q_(&parameters_[3]),
 99 |           C_(std::move(other.C_)) {
100 | 
101 | }
102 | 
103 | template <typename T>
104 | inline Pose<T>::Pose()
105 |         : r_(&parameters_[0]),
106 |           q_(&parameters_[3]),
107 |           C_(Eigen::Matrix<T,3,3>::Identity()) {
108 |     r_ = Eigen::Matrix<T,3,1>(0.0, 0.0, 0.0);
109 |     q_ = unitQuat<T>();
110 | }
111 | 
112 | template <typename T>
113 | inline Pose<T>::Pose(const Eigen::Matrix<T,3,1> & r_AB,
114 |                                       const Eigen::Matrix<T,4,1>& q_AB)
115 |         : r_(&parameters_[0]),
116 |           q_(&parameters_[3]) {
117 |     r_ = r_AB;
118 |     q_ = q_AB.normalized();
119 |     updateC();
120 | }
121 | template <typename T>
122 | inline Pose<T>::Pose(const Eigen::Matrix<T,3,1> & r_AB, const Eigen::Quaternion<T>& q_AB) 
123 |         : r_(&parameters_[0]),
124 |           q_(&parameters_[3]) {
125 |     r_ = r_AB;
126 |     q_ = q_AB.coeffs();
127 |     updateC();
128 | }
129 | 
130 | template <typename T>
131 | inline Pose<T>::Pose(const Eigen::Matrix<T,7,1>& vec): r_(&parameters_[0]),
132 |                                                          q_(&parameters_[3]) {
133 |     r_ = vec.template head<3>();
134 |     q_ = vec.template tail<4>();
135 |     q_ = quatNorm<T>(q_);
136 |     updateC();
137 | 
138 | }
139 | template <typename T>
140 | inline Pose<T>::Pose(const Eigen::Matrix<T,4,4> & T_AB)
141 |         : r_(&parameters_[0]),
142 |           q_(&parameters_[3]),
143 |           C_(T_AB.template topLeftCorner<3, 3>()) {
144 |     r_ = (T_AB.template topRightCorner<3, 1>());
145 |     q_ = rotMatToQuat<T>(C_);
146 | //    assert(fabs(T_AB(3, 0)) < 1.0e-12);
147 | //    assert(fabs(T_AB(3, 1)) < 1.0e-12);
148 | //    assert(fabs(T_AB(3, 2)) < 1.0e-12);
149 | //    assert(fabs(T_AB(3, 3) - 1.0) < 1.0e-12);
150 | }
151 | template <typename T>
152 | inline Pose<T>::~Pose() {
153 | 
154 | }
155 | template<typename T>
156 | template<typename Derived_coeffs>
157 | inline bool Pose<T>::setCoeffs(
158 |         const Eigen::MatrixBase<Derived_coeffs> & coeffs) {
159 |     EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_coeffs, 7);
160 |     parameters_ = coeffs;
161 |     updateC();
162 |     return true;
163 | }
164 | 
165 | // The underlying transformation
166 | template <typename T>
167 | inline Eigen::Matrix<T,4,4> Pose<T>::Transformation() const {
168 |     Eigen::Matrix<T,4,4> T_ret;
169 |     T_ret.template topLeftCorner<3, 3>() = C_;
170 |     T_ret.template topRightCorner<3, 1>() = r_;
171 |     T_ret.template bottomLeftCorner<1, 3>().setZero();
172 |     T_ret(3, 3) = T(1.0);
173 |     return T_ret;
174 | }
175 | 
176 | // return the rotation matrix
177 | template <typename T>
178 | inline const Eigen::Matrix<T,3,3> & Pose<T>::C() const {
179 |     return C_;
180 | }
181 | 
182 | // return the translation vector
183 | template <typename T>
184 | inline const Eigen::Map<Eigen::Matrix<T,3,1>> & Pose<T>::r() const {
185 |     return r_;
186 | }
187 | template <typename T>
188 | inline const Eigen::Map<Eigen::Matrix<T,4,1>> & Pose<T>::q() const {
189 |     return q_;
190 | }
191 | 
192 | template <typename T>
193 | inline Eigen::Matrix<T, 3, 4> Pose<T>::T3x4() const {
194 |     Eigen::Matrix<T, 3, 4> T3x4_ret;
195 |     T3x4_ret.template topLeftCorner<3, 3>() = C_;
196 |     T3x4_ret.template topRightCorner<3, 1>() = r_;
197 |     return T3x4_ret;
198 | }
199 | // Return a copy of the transformation inverted.
200 | template <typename T>
201 | inline Pose<T> Pose<T>::inverse() const {
202 |     return Pose<T>(-(C_.transpose() * r_), quatInv<T>(q_));
203 | }
204 | 
205 | // Set this to a random transformation.
206 | template <typename T>
207 | inline void Pose<T>::setRandom() {
208 |     setRandom(T(1.0), T(M_PI));
209 | }
210 | // Set this to a random transformation with bounded rotation and translation.
211 | template <typename T>
212 | inline void Pose<T>::setRandom(T translationMaxMeters,
213 |                                       T rotationMaxRadians) {
214 |     // Create a random unit-length axis.
215 |     Eigen::Matrix<T,3,1> axis = rotationMaxRadians * Eigen::Matrix<T,3,1>::Random();
216 |     // Create a random rotation angle in radians.
217 |     Eigen::Matrix<T,3,1> r = translationMaxMeters * Eigen::Matrix<T,3,1>::Random();
218 |     r_ = r;
219 |     C_ = Eigen::AngleAxis<T>(axis.norm(), axis.normalized());
220 |     q_ = rotMatToQuat<T>(C_);
221 | }
222 | 
223 | // Setters
224 | template <typename T>
225 | inline void Pose<T>::set(const Eigen::Matrix<T,4,4> & T_AB) {
226 |     r_ = (T_AB.template topRightCorner<3, 1>());
227 |     C_ = (T_AB.template topLeftCorner<3, 3>());
228 |     q_ = rotMatToQuat(C_);
229 | }
230 | template <typename T>
231 | 
232 | inline void Pose<T>::set(const Eigen::Matrix<T,3,1> & r_AB,
233 |                                 const Eigen::Matrix<T,4,1>& q_AB) {
234 |     r_ = r_AB;
235 |     q_ = quatNorm<T>(q_AB);
236 | 
237 |     updateC();
238 | }
239 | // Set this transformation to identity
240 | template <typename T>
241 | inline void Pose<T>::setIdentity() {
242 |     q_ = unitQuat<T>();
243 |     r_.setZero();
244 |     C_.setIdentity();
245 | }
246 | 
247 | template <typename T>
248 | inline Pose<T> Pose<T>::Identity() {
249 |     return Pose<T>();
250 | }
251 | 
252 | // operator*
253 | template <typename T>
254 | inline Pose<T> Pose<T>::operator*(
255 |         const Pose & rhs) const {
256 |     return Pose(C_ * rhs.r_ + r_, quatLeftComp<T>(q_) * rhs.q_);
257 | }
258 | template <typename T>
259 | inline Eigen::Matrix<T,3,1> Pose<T>::operator*(
260 |         const Eigen::Matrix<T,3,1> & rhs) const {
261 |     return C_ * rhs;
262 | }
263 | template <typename T>
264 | inline Eigen::Matrix<T,4,1> Pose<T>::operator*(
265 |         const Eigen::Matrix<T,4,1> & rhs) const {
266 |     const T s = rhs[3];
267 |     Eigen::Matrix<T,4,1> retVec;
268 |     retVec.template head<3>() = C_ * rhs.template head<3>() + r_ * s;
269 |     retVec[3] = s;
270 |     return retVec;
271 | }
272 | 
273 | template <typename T>
274 | inline Pose<T>& Pose<T>::operator=(const Pose<T> & rhs) {
275 |     parameters_ = rhs.parameters_;
276 |     C_ = rhs.C_;
277 |     r_ = Eigen::Map<Eigen::Matrix<T,3,1>>(&parameters_[0]);
278 |     q_ = Eigen::Map<Eigen::Matrix<T,4,1>>(&parameters_[3]);
279 |     return *this;
280 | }
281 | 
282 | 
283 | template <typename T>
284 | inline void Pose<T>::updateC() {
285 |     C_ = quatToRotMat<T>(q_);
286 | }
287 | 
288 | 
289 | // apply small update:
290 | template <typename T>
291 | template<typename Derived_delta>
292 | inline bool Pose<T>::oplus(
293 |         const Eigen::MatrixBase<Derived_delta> & delta) {
294 |     EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_delta, 6);
295 |     r_ += delta.template head<3>();
296 |     Eigen::Matrix<T,4,1> dq;
297 |     T halfnorm = T(0.5) * delta.template tail<3>().norm();
298 |     dq.template head<3>() = sinc(halfnorm) * T(0.5) * delta.template tail<3>();
299 |     dq[3] = cos(halfnorm);
300 |     q_ = (quatLeftComp(dq) * q_);
301 |     q_.normalize();
302 |     updateC();
303 |     return true;
304 | }
305 | 
306 | template <typename T>
307 | template<typename Derived_delta, typename Derived_jacobian>
308 | inline bool Pose<T>::oplus(
309 |         const Eigen::MatrixBase<Derived_delta> & delta,
310 |         const Eigen::MatrixBase<Derived_jacobian> & jacobian) {
311 |     EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_delta, 6);
312 |     EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 7, 6);
313 |     if (!oplus(delta)) {
314 |         return false;
315 |     }
316 |     return oplusJacobian(jacobian);
317 | }
318 | template <typename T>
319 | template<typename Derived_jacobian>
320 | inline bool Pose<T>::oplusJacobian(
321 |         const Eigen::MatrixBase<Derived_jacobian> & jacobian) const {
322 |     EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 7, 6);
323 |     Eigen::Matrix<double, 4, 3> S = Eigen::Matrix<double, 4, 3>::Zero();
324 |     const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).setZero();
325 |     const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian)
326 |             .template topLeftCorner<3, 3>().setIdentity();
327 |     S(0, 0) = T(0.5);
328 |     S(1, 1) = T(0.5);
329 |     S(2, 2) = T(0.5);
330 |     const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian)
331 |             .template bottomRightCorner<4, 3>() = quatRightComp<double>(q_) * S;
332 |     return true;
333 | }
334 | 
335 | //use right multiplication to compute $\frac{d\alpha}{d\Delta q}$ where \alpha is defined in leutenegger
336 | // ijrr 15, and q+\Delta q = \delta q(\alpha)\otimes q,
337 | // so (q+\Delta q)\otimes q^{-1} = \delta q(\alpha) = [0.5\alpha; 1]^T
338 | template <typename T>
339 | template <typename Derived_jacobian>
340 | inline bool Pose<T>::liftJacobian(const Eigen::MatrixBase<Derived_jacobian> & jacobian) const
341 | {
342 |     EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 6, 7);
343 |     const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).setZero();
344 |     const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).template topLeftCorner<3,3>()
345 |             = Eigen::Matrix3d::Identity();
346 |     const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).template bottomRightCorner<3,4>()
347 |             = 2*quatRightComp<T>(quatInv<T>(q_)).template topLeftCorner<3,4>();
348 |     return true;
349 | }
350 | template <typename T>
351 | inline  Eigen::Matrix<T,4,1> Pose<T>::rotation() const {
352 |     return q_;
353 | }
354 | template <typename T>
355 | 
356 | inline  Eigen::Matrix<T,3,1> Pose<T>::translation() const {
357 |     return r_;
358 | }
359 | 
360 | 


--------------------------------------------------------------------------------
/include/Quaternion.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef QUATERNION_H
  2 | #define QUATERNION_H
  3 | 
  4 | 
  5 | #include <map>
  6 | #include <iostream>
  7 | #include <Eigen/Core>
  8 | #include <Eigen/Geometry>
  9 | #include <eigen3/Eigen/Dense>
 10 | /*
 11 |  * R.f.:
 12 |  * [1] Pose estimation using linearized rotations and quaternion algebra.
 13 |  * [2] Indirect Kalman filter for 3D attitude estimation.
 14 |  *
 15 |  */
 16 | 
 17 | typedef  Eigen::Matrix<double,4,1> Quaternion;
 18 | typedef  Quaternion* QuatPtr;
 19 | typedef  Eigen::Map<Quaternion> QuaternionMap;
 20 | typedef  Eigen::Matrix<double,3,3> RotMat;
 21 | typedef  double real_t;
 22 | 
 23 | 
 24 | 
 25 | template<typename T>
 26 | Eigen::Matrix<T,4,1> unitQuat(){
 27 |     return (Eigen::Matrix<T,4,1>() 
 28 |         << T(0.0),T(0.0),T(0.0),T(1.0)).finished();
 29 | };
 30 | 
 31 | template<typename T>
 32 | Eigen::Matrix<T,4,1> quatMap(T* ptr){
 33 |     Eigen::Map<Eigen::Matrix<T,4,1>> j(ptr);
 34 |     Eigen::Matrix<T,4,1> jacobian = j;
 35 |     return jacobian;
 36 |  };
 37 | 
 38 | template<typename T>
 39 | inline Eigen::Matrix<T,3,3> crossMat(const Eigen::Matrix<T,3,1>& vec)
 40 | {
 41 |     return (Eigen::Matrix<T,3,3>()<< 0,       -vec[2], vec[1],
 42 |                                     vec[2],  0,       -vec[0],
 43 |                                     -vec[1], vec[0],  0).finished();
 44 | }
 45 | 
 46 | 
 47 | template<typename T>
 48 | bool isLessThenEpsilons4thRoot(T x){
 49 |     static const T epsilon4thRoot = pow(std::numeric_limits<T>::epsilon(), T(1.0/4.0));
 50 |     return x < epsilon4thRoot;
 51 | }
 52 | 
 53 | 
 54 | template<typename T>
 55 | Eigen::Matrix<T,4,1> quatExp(const Eigen::Matrix<T,3,1>& dx) {
 56 |     // Method of implementing this function that is accurate to numerical precision from
 57 |     // Grassia, F. S. (1998). Practical parameterization of rotations using the exponential map.
 58 |     // journal of graphics, gpu, and game tools, 3(3):29–48.
 59 | 
 60 |     T theta = dx.norm();
 61 |     // na is 1/theta sin(theta/2)
 62 |     T na;
 63 |     if(isLessThenEpsilons4thRoot(theta)){
 64 |         static const T one_over_48 = T(1.0/48.0);
 65 |         na = T(0.5) + (theta * theta) * one_over_48;
 66 |     } else {
 67 |         na = sin(theta*T(0.5)) / theta;
 68 |     }
 69 |     T ct = cos(theta*T(0.5));
 70 |     return Eigen::Matrix<T,4,1>(dx[0]*na,
 71 |                       dx[1]*na,
 72 |                       dx[2]*na,
 73 |                       ct);
 74 | }
 75 | 
 76 | template<typename T>
 77 | T fabsT(const T scale){
 78 |     if( scale > T(0)) return scale;
 79 |     else return -scale;
 80 | }
 81 | template<typename T>
 82 | T arcSinXOverX(T x) {
 83 |     if(isLessThenEpsilons4thRoot(fabsT(x))){
 84 |         return T(1.0) + x * x * T(1.0/6.0);
 85 |     }
 86 |     return asin(x) / x;
 87 | }
 88 | 
 89 | 
 90 | 
 91 | template<typename T>
 92 | Eigen::Matrix<T,3,1> quatLog(Eigen::Matrix<T,4,1> & q){
 93 | 
 94 | 
 95 |     const Eigen::Matrix<T, 3, 1> a = q.head(3);
 96 |     const T na = a.norm();
 97 |     const T eta = q[3];
 98 |     T scale;
 99 |     if(fabsT(eta) < na){ // use eta because it is more precise than na to calculate the scale. No singularities here.
100 |         // check sign of eta so that we can be sure that log(-q) = log(q)
101 |         if (eta >= T(0)) {
102 |             scale = acos(eta) / na;
103 |         } else {
104 |             scale = -acos(-eta) / na;
105 |         }
106 |     } else {
107 |         /*
108 |          * In this case more precision is in na than in eta so lets use na only to calculate the scale:
109 |          *
110 |          * assume first eta > 0 and 1 > na > 0.
111 |          *               u = asin (na) / na  (this implies u in [1, pi/2], because na i in [0, 1]
112 |          *    sin (u * na) = na
113 |          *  sin^2 (u * na) = na^2
114 |          *  cos^2 (u * na) = 1 - na^2
115 |          *                              (1 = ||q|| = eta^2 + na^2)
116 |          *    cos^2 (u * na) = eta^2
117 |          *                              (eta > 0,  u * na = asin(na) in [0, pi/2] => cos(u * na) >= 0 )
118 |          *      cos (u * na) = eta
119 |          *                              (u * na in [ 0, pi/2] )
120 |          *                 u = acos (eta) / na
121 |          *
122 |          * So the for eta > 0 it is acos(eta) / na == asin(na) / na.
123 |          * From some geometric considerations (mirror the setting at the hyper plane q==0)
124 |          * it follows for eta < 0 that (pi - asin(na)) / na = acos(eta) / na.
125 |          */
126 |         if(eta > T(0)) {
127 |             // For asin(na)/ na the singularity na == 0 can be removed. We can ask (e.g. Wolfram alpha)
128 |             // for its series expansion at na = 0. And that is done in the following function.
129 |             scale = arcSinXOverX(na);
130 |         } else {
131 |             // the negative is here so that log(-q) == log(q)
132 |             scale = -arcSinXOverX(na);
133 |         }
134 |     }
135 |     return a * ((T(2.0)) * scale);
136 | }
137 | 
138 | template<typename T>
139 | Eigen::Matrix<T,3,3> quatL(Eigen::Matrix<T,4,1> &q){
140 |     Eigen::Matrix<T,3,3> Jac_log;
141 |     Jac_log.setIdentity();
142 |     if(std::abs(q(3) - T(1)) < T(1e-5)){
143 |         return Jac_log;
144 |     }else{
145 |         Eigen::Matrix<T,3,1> phi = quatLog(q);
146 |         Eigen::Matrix<T,3,3> I;
147 |         I.setIdentity();
148 |         T nphi = phi.norm();
149 |         Eigen::Matrix<T,3,1> a = phi/nphi;
150 |         Eigen::Matrix<T,3,3> squareA= crossMat(a)*crossMat(a);
151 | 
152 | 
153 |         Jac_log = I + T(0.5)*crossMat<T>(phi)
154 |                   + (T(1) - nphi/(T(2)*tan(T(0.5)*nphi)))*squareA;
155 |         return Jac_log;
156 |     }
157 | }
158 | template<typename T>
159 | Eigen::Matrix<T,3,3> quatS(Eigen::Matrix<T,3,1> &phi){
160 |     Eigen::Matrix<T,3,3> Jac_exp;
161 |     Jac_exp.setIdentity();
162 |     T nphi = phi.norm();
163 |     if(nphi < T(1e-5)){
164 |         return Jac_exp;
165 |     }else{
166 |         Eigen::Matrix<T,3,3> I;
167 |         I.setIdentity();
168 |         Eigen::Matrix<T,3,1> a = phi/nphi;
169 |         T squareSin = sin(T(0.5)*nphi)*sin((0.5)*nphi);
170 |         Eigen::Matrix<T,3,3> squareA= crossMat(a)*crossMat(a);
171 |         Jac_exp = I - T(2.0)/nphi*squareSin*crossMat(a)
172 |                   + (T(1) - T(1.0)/(nphi)*sin(nphi))*squareA;
173 |         return Jac_exp;
174 | 
175 |     }
176 | }
177 | 
178 | template<typename T>
179 | Eigen::Matrix<T,4,1> QuatFromEuler(T* euler)
180 | {
181 |     Eigen::Matrix<T,4,1> quat;
182 |     T cr2 = cos(euler[0]*T(0.5));
183 |     T cp2 = cos(euler[1]*T(0.5));
184 |     T cy2 = cos(euler[2]*T(0.5));
185 |     T sr2 = sin(euler[0]*T(0.5));
186 |     T sp2 = sin(euler[1]*T(0.5));
187 |     T sy2 = sin(euler[2]*T(0.5));
188 | 
189 | 
190 |     quat[0] = sr2*cp2*cy2 - cr2*sp2*sy2;
191 |     quat[1] = cr2*sp2*cy2 + sr2*cp2*sy2;
192 |     quat[2] = cr2*cp2*sy2 - sr2*sp2*cy2;
193 |     quat[3] = cr2*cp2*cy2 + sr2*sp2*sy2;
194 |     return quat;
195 | }
196 | 
197 | 
198 | template<typename T>
199 | Eigen::Matrix<T,4,1> deltaQuat( Eigen::Matrix<T,3,1> &deltaTheta )
200 | {
201 |     Eigen::Matrix<T,4,1>  r;
202 |     Eigen::Matrix<T,3,1> deltaq = T(0.5) * deltaTheta;
203 | 
204 |     r.head(3) = deltaq;
205 |     r(3) = 1.0;
206 | 
207 |     return r;
208 | }
209 | 
210 | 
211 | template<typename T>
212 | Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>
213 | null(const Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>& A)
214 | {
215 |     int r = 0;
216 |     Eigen::JacobiSVD<Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>>
217 |             svd(A.transpose(), Eigen::ComputeFullV);
218 | 
219 |     /* Get the V matrix */
220 |     Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>
221 |             V((int)svd.matrixV().rows(), (int)svd.matrixV().cols());
222 |     V = svd.matrixV();
223 |     Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> S = svd.singularValues();
224 | 
225 |     T tol = std::max(A.rows(), A.cols()) * S.maxCoeff() * T(2.2204e-016);
226 |     for (int i = 0; i < S.size(); i++)
227 |     {
228 |         if (S.coeff(i) > tol)
229 |         {
230 |             r++;
231 |         }
232 |     }
233 |     Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>
234 |             Z = V.block(0, r, V.rows(), V.cols()-r);
235 |     return Z;
236 | }
237 | 
238 | 
239 | template<typename T>
240 | Eigen::Matrix<T,4,1> quatInv(const Eigen::Matrix<T,4,1> q)
241 | {
242 |     //assert( abs( q.norm() - 1.0) <= std::numeric_limits<double>::epsilon() );
243 |     Eigen::Matrix<T,4,1> qinv =  q/q.norm();
244 |     qinv(0,0) = -qinv(0,0);
245 |     qinv(1,0) = -qinv(1,0);
246 |     qinv(2,0) = -qinv(2,0);
247 |     qinv(3,0) = qinv(3,0);
248 | 
249 |     return qinv;
250 | }
251 | 
252 | template<typename T>
253 | inline Eigen::Matrix<T,4,1> quatNorm( Eigen::Matrix<T,4,1> q){
254 |     return q/q.norm();
255 | }
256 | 
257 | 
258 | template<typename T>
259 | inline Eigen::Matrix<T,4,4> quatLeftComp( const Eigen::Matrix<T,4,1> q )
260 | {
261 |     // [  q3,  q2, -q1, q0]
262 |     // [ -q2,  q3,  q0, q1]
263 |     // [  q1, -q0,  q3, q2]
264 |     // [ -q0, -q1, -q2, q3]
265 |     Eigen::Matrix<T,4,4> Q;
266 |     Q(0,0) =  q[3]; Q(0,1) =  q[2]; Q(0,2) = -q[1]; Q(0,3) =  q[0];
267 |     Q(1,0) = -q[2]; Q(1,1) =  q[3]; Q(1,2) =  q[0]; Q(1,3) =  q[1];
268 |     Q(2,0) =  q[1]; Q(2,1) = -q[0]; Q(2,2) =  q[3]; Q(2,3) =  q[2];
269 |     Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) =  q[3];
270 | 
271 |     return Q;
272 | }
273 | 
274 | template<typename T>
275 | inline Eigen::Matrix<T,4,4> quatRightComp( const Eigen::Matrix<T,4,1> q )
276 | {
277 |     // [  q3, -q2,  q1, q0]
278 |     // [  q2,  q3, -q0, q1]
279 |     // [ -q1,  q0,  q3, q2]
280 |     // [ -q0, -q1, -q2, q3]
281 | 
282 |     Eigen::Matrix<T,4,4> Q;
283 |     Q(0,0) =  q[3]; Q(0,1) = -q[2]; Q(0,2) =  q[1]; Q(0,3) =  q[0];
284 |     Q(1,0) =  q[2]; Q(1,1) =  q[3]; Q(1,2) = -q[0]; Q(1,3) =  q[1];
285 |     Q(2,0) = -q[1]; Q(2,1) =  q[0]; Q(2,2) =  q[3]; Q(2,3) =  q[2];
286 |     Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) =  q[3];
287 | 
288 |     return Q;
289 | }
290 | 
291 | 
292 | 
293 | template<typename T>
294 | Eigen::Matrix<T,4,1> quatMult( const Eigen::Matrix<T,4,1> q1,const Eigen::Matrix<T,4,1> q2 )
295 | {
296 |     Eigen::Matrix<T,4,1> r;
297 |     r = quatLeftComp(q1) * q2;
298 |     return r;
299 | }
300 | 
301 | 
302 | template<typename T>
303 | Eigen::Matrix<T,3,3> renormalizeRotMat( Eigen::Matrix<T,3,3> &m )
304 | {
305 |     Eigen::JacobiSVD<Eigen::Matrix<T,3,3> > jsvd(m, Eigen::ComputeFullU | Eigen::ComputeFullV );
306 | 
307 |     Eigen::Matrix<T,3,3> VT = jsvd.matrixV().transpose();
308 | 
309 |     return jsvd.matrixU() * Eigen::Matrix<T,3,3>::Identity() * VT;
310 | }
311 | 
312 | template<typename T>
313 | Eigen::Matrix<T,3,3> quatToRotMat( Eigen::Matrix<T,4,1> q )
314 | {
315 |     q = q / q.norm();
316 | 
317 |     Eigen::Matrix<T,4,4> R = quatRightComp(q).transpose() * quatLeftComp(q);
318 |     Eigen::Matrix<T,3,3> tmp = R.topLeftCorner(3,3);
319 |     tmp = renormalizeRotMat( tmp );
320 | 
321 |     return tmp;
322 | }
323 | 
324 | 
325 | 
326 | template<typename T>
327 | Eigen::Matrix<T,4,1> rotMatToQuat( Eigen::Matrix<T,3,3> &R )
328 | {
329 |     Eigen::Matrix<T,4,1> q;
330 |     Eigen::Matrix<T,3,3> R_T = R.transpose();
331 | 
332 |     T Rxx = R_T(0,0); T Rxy = R_T(0,1); T Rxz = R_T(0,2);
333 |     T Ryx = R_T(1,0); T Ryy = R_T(1,1); T Ryz = R_T(1,2);
334 |     T Rzx = R_T(2,0); T Rzy = R_T(2,1); T Rzz = R_T(2,2);
335 | 
336 |     T w = sqrt( R.trace() + T(1.0) ) / T(2.0);
337 |     T x = sqrt( T(1.0) + Rxx - Ryy - Rzz ) / T(2.0);
338 |     T y = sqrt( T(1.0) + Ryy - Rxx - Rzz ) / T(2.0);
339 |     T z = sqrt( T(1.0) + Rzz - Ryy - Rxx ) / T(2.0);
340 | 
341 |     T i = std::max( std::max(w,x), std::max(y,z) );
342 |     if( i == w )
343 |     {
344 |         x = ( Rzy - Ryz ) / (T(4.0)*w);
345 |         y = ( Rxz - Rzx ) / (T(4.0)*w);
346 |         z = ( Ryx - Rxy ) / (T(4.0)*w);
347 |     }
348 |     else if( i == x )
349 |     {
350 |         w = ( Rzy - Ryz ) / (T(4.0)*x);
351 |         y = ( Rxy + Ryx ) / (T(4.0)*x);
352 |         z = ( Rzx + Rxz ) / (T(4.0)*x);
353 |     }
354 |     else if( i == y )
355 |     {
356 |         w = ( Rxz - Rzx ) / (T(4.0)*y);
357 |         x = ( Rxy + Ryx ) / (T(4.0)*y);
358 |         z = ( Ryz + Rzy ) / (T(4.0)*y);
359 |     }
360 |     else if( i == z )
361 |     {
362 |         w = ( Ryx - Rxy ) / (T(4.0)*z);
363 |         x = ( Rzx + Rxz ) / (T(4.0)*z);
364 |         y = ( Ryz + Rzy ) / (T(4.0)*z);
365 |     }
366 | 
367 |     q(0) = x;
368 |     q(1) = y;
369 |     q(2) = z;
370 |     q(3) = w;
371 | 
372 |     return q;
373 | }
374 | 
375 | 
376 | 
377 | template<typename T>
378 | Eigen::Matrix<T,3,3> axisAngleToRotMat(Eigen::Matrix<T,3,1> aa){
379 |     T phi = aa.norm();
380 |     Eigen::Matrix<T,3,1> a = aa/phi;
381 |     Eigen::Matrix<T,3,3> I;
382 |     I.setIdentity();
383 |     T cos_phi = cos(phi);
384 | 
385 |     return cos_phi*I + (1 - cos_phi)*a*a.transpose() - sin(phi)*crossMat<T>(a);
386 | 
387 | }
388 | 
389 | template<typename T>
390 | Eigen::Matrix<T,4,1> randomQuat() {
391 |     // Create a random unit-length axis.
392 |     Eigen::Matrix<T, 3, 1> axis = T(M_PI) * Eigen::Matrix<T, 3, 1>::Random();
393 | 
394 |     Eigen::Matrix<T,3,3> C_
395 |             = Eigen::AngleAxis<T>(axis.norm(), axis.normalized()).toRotationMatrix();
396 |     Eigen::Matrix<T,4,1> q_ = rotMatToQuat<T>(C_);
397 |     return q_;
398 | }
399 | 
400 | 
401 | 
402 | /*
403 |  * From Barfoot's book.
404 |  */
405 | 
406 | 
407 | template<typename T>
408 | Eigen::Matrix<T,3,3> rotX(T x){
409 |     Eigen::Matrix<T,3,3> Rx ;
410 |     Rx<<   T(1),      T(0),     T(0),
411 |             T(0), cos(x),sin(x),
412 |             T(0),-sin(x),cos(x);
413 |     return Rx;
414 | }
415 | 
416 | 
417 | template<typename T>
418 | Eigen::Matrix<T,3,3> rotY(T y){
419 |     Eigen::Matrix<T,3,3> Ry ;
420 |     Ry<<  cos(y),   T(0),   -sin(y),
421 |             T(0),   T(1),         T(0),
422 |             sin(y),  T(0),    cos(y);
423 |     return Ry;
424 | 
425 | }
426 | 
427 | 
428 | template<typename T>
429 | Eigen::Matrix<T,3,3> rotZ(double z){
430 |     Eigen::Matrix<T,3,3> Rz ;
431 |     Rz<<  cos(z),sin(z),T(0),
432 |             -sin(z),cos(z),T(0),
433 |             T(0),     T(0),   T(1);
434 |     return Rz;
435 | 
436 | }
437 | 
438 | /*
439 |  * Kinematics
440 |  */
441 | 
442 | 
443 | template<typename T>
444 | Eigen::Matrix<T,4,4> OmegaMat(const Eigen::Matrix<T,3,1>& vec){
445 |     return (Eigen::Matrix<T,4,4>()<< 0, vec[2], -vec[1], vec[0],
446 |             -vec[2], 0, vec[0], vec[1],
447 |             vec[1], -vec[0], 0, vec[2],
448 |             -vec[0], -vec[1], -vec[2], 0).finished();
449 | };
450 | 
451 | 
452 | template<typename  T>
453 | inline Eigen::Matrix<T,3,1> errorRotationPropagate(Eigen::Matrix<T,3,1>& omega,
454 |                                             Eigen::Matrix<T,3,1>& deltaTheta0 ){
455 |    return (Eigen::Matrix<T,3,3>::setIdentity() - crossMat(omega))*deltaTheta0;
456 | };
457 | 
458 | 
459 | /*
460 |  * q2 = Exp(omega*dt)*q1
461 |  */
462 | template <typename T>
463 | Eigen::Matrix<T,3,1> getOmegaFromTwoQuaternion(const Eigen::Matrix<T,4,1>& q1,
464 |                                                const Eigen::Matrix<T,4,1>& q2,
465 |                                                T dt){
466 |     Eigen::Matrix<T,4,1> temp = quatLeftComp<T>(q2)*quatInv<T>(q1);
467 |     return   quatLog<T>(temp)/dt;
468 | };
469 | 
470 | #endif


--------------------------------------------------------------------------------