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 #ifndef INCLUDE_OKVIS_TRANSFORMATION_HPP_
40 #define INCLUDE_OKVIS_TRANSFORMATION_HPP_
41 
42 #include <stdint.h>
43 #include <vector>
44 #include <Eigen/Core>
45 #include <Eigen/Geometry>
47 
49 namespace okvis {
50 
52 namespace kinematics {
53 
57 double sinc(double x);
58 
62 Eigen::Quaterniond deltaQ(const Eigen::Vector3d& dAlpha);
63 
65 Eigen::Matrix3d rightJacobian(const Eigen::Vector3d & PhiVec);
66 
77 {
78  public:
79  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
80 
83 
86  Transformation(const Transformation & other);
87 
90  Transformation(Transformation && other);
91 
95  Transformation(const Eigen::Vector3d & r_AB, const Eigen::Quaterniond& q_AB);
96 
99  explicit Transformation(const Eigen::Matrix4d & T_AB);
100 
102  ~Transformation();
103 
107  template<typename Derived_coeffs>
108  bool setCoeffs(const Eigen::MatrixBase<Derived_coeffs> & coeffs);
109 
113  template<typename Derived_coeffs>
114  bool setParameters(const Eigen::MatrixBase<Derived_coeffs> & parameters)
115  {
116  return setCoeffs(parameters);
117  }
118 
120  Eigen::Matrix4d T() const;
121 
123  const Eigen::Matrix3d & C() const;
124 
126  const Eigen::Map<Eigen::Vector3d> & r() const;
127 
129  const Eigen::Map<Eigen::Quaterniond> & q() const;
130 
132  Eigen::Matrix<double, 3, 4> T3x4() const;
133 
135  const Eigen::Matrix<double, 7, 1> & coeffs() const
136  {
137  return parameters_;
138  }
139 
141  const Eigen::Matrix<double, 7, 1> & parameters() const
142  {
143  return parameters_;
144  }
145 
148  const double* parameterPtr() const
149  {
150  return &parameters_[0];
151  }
152 
154  void setRandom();
158  void setRandom(double translationMaxMeters, double rotationMaxRadians);
159 
162  void set(const Eigen::Matrix4d & T_AB);
163 
167  void set(const Eigen::Vector3d & r_AB, const Eigen::Quaternion<double>& q_AB);
168 
170  void setIdentity();
171 
173  static Transformation Identity();
174 
176  Transformation inverse() const;
177 
178  // operator* (group operator)
181  Transformation operator*(const Transformation & rhs) const;
182 
186  Eigen::Vector3d operator*(const Eigen::Vector3d & rhs) const;
187 
190  Eigen::Vector4d operator*(const Eigen::Vector4d & rhs) const;
191 
194  Transformation& operator=(const Transformation & rhs);
195 
200  template<typename Derived_delta>
201  bool oplus(const Eigen::MatrixBase<Derived_delta> & delta);
202 
208  template<typename Derived_delta, typename Derived_jacobian>
209  bool oplus(const Eigen::MatrixBase<Derived_delta> & delta,
210  const Eigen::MatrixBase<Derived_jacobian> & jacobian);
211 
215  template<typename Derived_jacobian>
216  bool oplusJacobian(
217  const Eigen::MatrixBase<Derived_jacobian> & jacobian) const;
218 
221  // @param[out] jacobian The output lift Jacobian (6 by 7 matrix).
223  template<typename Derived_jacobian>
224  bool liftJacobian(const Eigen::MatrixBase<Derived_jacobian> & jacobian) const;
225 
226  protected:
228  void updateC();
229  Eigen::Matrix<double, 7, 1> parameters_;
230  Eigen::Map<Eigen::Vector3d> r_;
231  Eigen::Map<Eigen::Quaterniond> q_;
232  Eigen::Matrix3d C_;
233 };
234 
235 } // namespace kinematics
236 } // namespace okvis
237 
239 
240 #endif /* INCLUDE_OKVIS_TRANSFORMATION_HPP_ */
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
const Eigen::Matrix< double, 7, 1 > & parameters() const
The parameters (coefficients) as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).
Definition: Transformation.hpp:141
~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
Header implementation file for the Transformation class.
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
__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
const double * parameterPtr() const
Get the parameters — support for ceres.
Definition: Transformation.hpp:148
void setRandom()
Set this to a random transformation.
Definition: Transformation.hpp:176
File with some helper functions related to matrix/vector operations.
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
bool setParameters(const Eigen::MatrixBase< Derived_coeffs > &parameters)
Parameter setting, all 7.
Definition: Transformation.hpp:114