OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Transformation.hpp
Go to the documentation of this file.
1 /*********************************************************************************
2  * OKVIS - Open Keyframe-based Visual-Inertial SLAM
3  * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
14  * its contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Created on: Dec 2, 2014
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 namespace okvis {
41 
43 namespace kinematics {
44 
45 __inline__ double sinc(double x) {
46  if (fabs(x) > 1e-6) {
47  return sin(x) / x;
48  } else {
49  static const double c_2 = 1.0 / 6.0;
50  static const double c_4 = 1.0 / 120.0;
51  static const double c_6 = 1.0 / 5040.0;
52  const double x_2 = x * x;
53  const double x_4 = x_2 * x_2;
54  const double x_6 = x_2 * x_2 * x_2;
55  return 1.0 - c_2 * x_2 + c_4 * x_4 - c_6 * x_6;
56  }
57 }
58 
59 __inline__ Eigen::Quaterniond deltaQ(const Eigen::Vector3d& dAlpha)
60 {
61  Eigen::Vector4d dq;
62  double halfnorm = 0.5 * dAlpha.template tail<3>().norm();
63  dq.template head<3>() = sinc(halfnorm) * 0.5 * dAlpha.template tail<3>();
64  dq[3] = cos(halfnorm);
65  return Eigen::Quaterniond(dq);
66 }
67 
68 // Right Jacobian, see Forster et al. RSS 2015 eqn. (8)
69 __inline__ Eigen::Matrix3d rightJacobian(const Eigen::Vector3d & PhiVec) {
70  const double Phi = PhiVec.norm();
71  Eigen::Matrix3d retMat = Eigen::Matrix3d::Identity();
72  const Eigen::Matrix3d Phi_x = okvis::kinematics::crossMx(PhiVec);
73  const Eigen::Matrix3d Phi_x2 = Phi_x*Phi_x;
74  if(Phi < 1.0e-4) {
75  retMat += -0.5*Phi_x + 1.0/6.0*Phi_x2;
76  } else {
77  const double Phi2 = Phi*Phi;
78  const double Phi3 = Phi2*Phi;
79  retMat += -(1.0-cos(Phi))/(Phi2)*Phi_x + (Phi-sin(Phi))/Phi3*Phi_x2;
80  }
81  return retMat;
82 }
83 
85  : parameters_(other.parameters_),
86  r_(&parameters_[0]),
87  q_(&parameters_[3]),
88  C_(other.C_) {
89 
90 }
91 
93  : parameters_(std::move(other.parameters_)),
94  r_(&parameters_[0]),
95  q_(&parameters_[3]),
96  C_(std::move(other.C_)) {
97 
98 }
99 
101  : r_(&parameters_[0]),
102  q_(&parameters_[3]),
103  C_(Eigen::Matrix3d::Identity()) {
104  r_ = Eigen::Vector3d(0.0, 0.0, 0.0);
105  q_ = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
106 }
107 
108 inline Transformation::Transformation(const Eigen::Vector3d & r_AB,
109  const Eigen::Quaterniond& q_AB)
110  : r_(&parameters_[0]),
111  q_(&parameters_[3]) {
112  r_ = r_AB;
113  q_ = q_AB.normalized();
114  updateC();
115 }
116 inline Transformation::Transformation(const Eigen::Matrix4d & T_AB)
117  : r_(&parameters_[0]),
118  q_(&parameters_[3]),
119  C_(T_AB.topLeftCorner<3, 3>()) {
120  r_ = (T_AB.topRightCorner<3, 1>());
121  q_ = (T_AB.topLeftCorner<3, 3>());
122  assert(fabs(T_AB(3, 0)) < 1.0e-12);
123  assert(fabs(T_AB(3, 1)) < 1.0e-12);
124  assert(fabs(T_AB(3, 2)) < 1.0e-12);
125  assert(fabs(T_AB(3, 3) - 1.0) < 1.0e-12);
126 }
128 
129 }
130 
131 template<typename Derived_coeffs>
133  const Eigen::MatrixBase<Derived_coeffs> & coeffs) {
134  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_coeffs, 7);
136  updateC();
137  return true;
138 }
139 
140 // The underlying transformation
141 inline Eigen::Matrix4d Transformation::T() const {
142  Eigen::Matrix4d T_ret;
143  T_ret.topLeftCorner<3, 3>() = C_;
144  T_ret.topRightCorner<3, 1>() = r_;
145  T_ret.bottomLeftCorner<1, 3>().setZero();
146  T_ret(3, 3) = 1.0;
147  return T_ret;
148 }
149 
150 // return the rotation matrix
151 inline const Eigen::Matrix3d & Transformation::C() const {
152  return C_;
153 }
154 
155 // return the translation vector
156 inline const Eigen::Map<Eigen::Vector3d> & Transformation::r() const {
157  return r_;
158 }
159 
160 inline const Eigen::Map<Eigen::Quaterniond> & Transformation::q() const {
161  return q_;
162 }
163 
164 inline Eigen::Matrix<double, 3, 4> Transformation::T3x4() const {
165  Eigen::Matrix<double, 3, 4> T3x4_ret;
166  T3x4_ret.topLeftCorner<3, 3>() = C_;
167  T3x4_ret.topRightCorner<3, 1>() = r_;
168  return T3x4_ret;
169 }
170 // Return a copy of the transformation inverted.
172  return Transformation(-(C_.transpose() * r_), q_.inverse());
173 }
174 
175 // Set this to a random transformation.
177  setRandom(1.0, M_PI);
178 }
179 // Set this to a random transformation with bounded rotation and translation.
180 inline void Transformation::setRandom(double translationMaxMeters,
181  double rotationMaxRadians) {
182  // Create a random unit-length axis.
183  Eigen::Vector3d axis = rotationMaxRadians * Eigen::Vector3d::Random();
184  // Create a random rotation angle in radians.
185  Eigen::Vector3d r = translationMaxMeters * Eigen::Vector3d::Random();
186  r_ = r;
187  q_ = Eigen::AngleAxisd(axis.norm(), axis.normalized());
188  updateC();
189 }
190 
191 // Setters
192 inline void Transformation::set(const Eigen::Matrix4d & T_AB) {
193  r_ = (T_AB.topRightCorner<3, 1>());
194  q_ = (T_AB.topLeftCorner<3, 3>());
195  updateC();
196 }
197 inline void Transformation::set(const Eigen::Vector3d & r_AB,
198  const Eigen::Quaternion<double> & q_AB) {
199  r_ = r_AB;
200  q_ = q_AB.normalized();
201  updateC();
202 }
203 // Set this transformation to identity
205  q_.setIdentity();
206  r_.setZero();
207  C_.setIdentity();
208 }
209 
211  return Transformation();
212 }
213 
214 // operator*
216  const Transformation & rhs) const {
217  return Transformation(C_ * rhs.r_ + r_, q_ * rhs.q_);
218 }
219 inline Eigen::Vector3d Transformation::operator*(
220  const Eigen::Vector3d & rhs) const {
221  return C_ * rhs;
222 }
223 inline Eigen::Vector4d Transformation::operator*(
224  const Eigen::Vector4d & rhs) const {
225  const double s = rhs[3];
226  Eigen::Vector4d retVec;
227  retVec.head<3>() = C_ * rhs.head<3>() + r_ * s;
228  retVec[3] = s;
229  return retVec;
230 }
231 
233  parameters_ = rhs.parameters_;
234  C_ = rhs.C_;
235  r_ = Eigen::Map<Eigen::Vector3d>(&parameters_[0]);
236  q_ = Eigen::Map<Eigen::Quaterniond>(&parameters_[3]);
237  return *this;
238 }
239 
240 inline void Transformation::updateC() {
241  C_ = q_.toRotationMatrix();
242 }
243 
244 // apply small update:
245 template<typename Derived_delta>
247  const Eigen::MatrixBase<Derived_delta> & delta) {
248  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_delta, 6);
249  r_ += delta.template head<3>();
250  Eigen::Vector4d dq;
251  double halfnorm = 0.5 * delta.template tail<3>().norm();
252  dq.template head<3>() = sinc(halfnorm) * 0.5 * delta.template tail<3>();
253  dq[3] = cos(halfnorm);
254  q_ = (Eigen::Quaterniond(dq) * q_);
255  q_.normalize();
256  updateC();
257  return true;
258 }
259 
260 template<typename Derived_delta, typename Derived_jacobian>
262  const Eigen::MatrixBase<Derived_delta> & delta,
263  const Eigen::MatrixBase<Derived_jacobian> & jacobian) {
264  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived_delta, 6);
265  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 7, 6);
266  if (!oplus(delta)) {
267  return false;
268  }
269  return oplusJacobian(jacobian);
270 }
271 
272 template<typename Derived_jacobian>
274  const Eigen::MatrixBase<Derived_jacobian> & jacobian) const {
275  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 7, 6);
276  Eigen::Matrix<double, 4, 3> S = Eigen::Matrix<double, 4, 3>::Zero();
277  const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).setZero();
278  const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian)
279  .template topLeftCorner<3, 3>().setIdentity();
280  S(0, 0) = 0.5;
281  S(1, 1) = 0.5;
282  S(2, 2) = 0.5;
283  const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian)
284  .template bottomRightCorner<4, 3>() = okvis::kinematics::oplus(q_) * S;
285  return true;
286 }
287 
288 template <typename Derived_jacobian>
289 inline bool Transformation::liftJacobian(const Eigen::MatrixBase<Derived_jacobian> & jacobian) const
290 {
291  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived_jacobian, 6, 7);
292  const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).setZero();
293  const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).template topLeftCorner<3,3>()
294  = Eigen::Matrix3d::Identity();
295  const_cast<Eigen::MatrixBase<Derived_jacobian>&>(jacobian).template bottomRightCorner<3,4>()
296  = 2*okvis::kinematics::oplus(q_.inverse()).template topLeftCorner<3,4>();
297  return true;
298 }
299 
300 } // namespace kinematics
301 } // namespace okvis
Eigen::Matrix< Scalar_T, 3, 3 > crossMx(Scalar_T x, Scalar_T y, Scalar_T z)
Cross matrix of a vector [x,y,z]. Adopted from Schweizer-Messer by Paul Furgale.
Definition: operators.hpp:62
const Eigen::Map< Eigen::Vector3d > & r() const
Returns the translation vector r_AB (represented in frame A).
Definition: Transformation.hpp:156
Transformation & operator=(const Transformation &rhs)
Assignment – copy. Takes care of proper caching.
Definition: Transformation.hpp:232
~Transformation()
Trivial destructor.
Definition: Transformation.hpp:127
void setIdentity()
Set this transformation to identity.
Definition: Transformation.hpp:204
bool oplusJacobian(const Eigen::MatrixBase< Derived_jacobian > &jacobian) const
Get the Jacobian of the oplus operation (a 7 by 6 matrix).
Definition: Transformation.hpp:273
Transformation inverse() const
Returns a copy of the transformation inverted.
Definition: Transformation.hpp:171
A class that does homogeneous transformations. This relates a frame A and B: T_AB; it consists of tra...
Definition: Transformation.hpp:76
Eigen::Map< Eigen::Vector3d > r_
Translation {A}r{B}.
Definition: Transformation.hpp:230
Eigen::Matrix< double, 7, 1 > parameters_
Concatenated parameters [r;q].
Definition: Transformation.hpp:229
Eigen::Map< Eigen::Quaterniond > q_
Quaternion q_{AB}.
Definition: Transformation.hpp:231
Eigen::Matrix< double, 3, 4 > T3x4() const
Get the upper 3x4 part of the homogeneous transformation matrix T_AB.
Definition: Transformation.hpp:164
Eigen::Matrix4d oplus(const Eigen::Quaterniond &q_BC)
Oplus matrix of a quaternion, i.e. q_AB*q_BC = oplus(q_BC)*q_AB.coeffs().
Definition: operators.hpp:104
__inline__ double sinc(double x)
Implements sin(x)/x for all x in R.
Definition: Transformation.hpp:45
const Eigen::Map< Eigen::Quaterniond > & q() const
Returns the Quaternion q_AB (as an Eigen Quaternion).
Definition: Transformation.hpp:160
bool setCoeffs(const Eigen::MatrixBase< Derived_coeffs > &coeffs)
Parameter setting, all 7.
Definition: Transformation.hpp:132
void updateC()
Update the caching of the rotation matrix.
Definition: Transformation.hpp:240
__inline__ Eigen::Matrix3d rightJacobian(const Eigen::Vector3d &PhiVec)
Right Jacobian, see Forster et al. RSS 2015 eqn. (8)
Definition: Transformation.hpp:69
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Transformation()
Default constructor: initialises a unit transformation.
Definition: Transformation.hpp:100
void set(const Eigen::Matrix4d &T_AB)
Set from a homogeneous transformation matrix.
Definition: Transformation.hpp:192
Eigen::Matrix3d C_
The cached DCM C_{AB}.
Definition: Transformation.hpp:232
static Transformation Identity()
Get an identity transformation.
Definition: Transformation.hpp:210
bool liftJacobian(const Eigen::MatrixBase< Derived_jacobian > &jacobian) const
Gets the jacobian dx/dChi, i.e. lift the minimal Jacobian to a full one (as needed by ceres)...
Definition: Transformation.hpp:289
Transformation operator*(const Transformation &rhs) const
Multiplication with another transformation object.
Definition: Transformation.hpp:215
const Eigen::Matrix< double, 7, 1 > & coeffs() const
The coefficients (parameters) as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).
Definition: Transformation.hpp:135
void setRandom()
Set this to a random transformation.
Definition: Transformation.hpp:176
bool oplus(const Eigen::MatrixBase< Derived_delta > &delta)
Apply a small update with delta being 6x1.
Definition: Transformation.hpp:246
__inline__ Eigen::Quaterniond deltaQ(const Eigen::Vector3d &dAlpha)
Implements the exponential map for quaternions.
Definition: Transformation.hpp:59
const Eigen::Matrix3d & C() const
Returns the rotation matrix (cached).
Definition: Transformation.hpp:151
Eigen::Matrix4d T() const
The underlying homogeneous transformation matrix.
Definition: Transformation.hpp:141