OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
ImuError.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: Sep 3, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #ifndef INCLUDE_OKVIS_CERES_IMUERROR_HPP_
40 #define INCLUDE_OKVIS_CERES_IMUERROR_HPP_
41 
42 #include <vector>
43 #include <mutex>
44 #include "ceres/ceres.h"
45 #include <okvis/FrameTypedefs.hpp>
46 #include <okvis/Time.hpp>
47 #include <okvis/assert_macros.hpp>
48 #include <okvis/Measurements.hpp>
49 #include <okvis/Variables.hpp>
50 #include <okvis/Parameters.hpp>
52 
54 namespace okvis {
56 namespace ceres {
57 
59 class ImuError :
60  public ::ceres::SizedCostFunction<15 /* number of residuals */,
61  7 /* size of first parameter (PoseParameterBlock k) */,
62  9 /* size of second parameter (SpeedAndBiasParameterBlock k) */,
63  7 /* size of third parameter (PoseParameterBlock k+1) */,
64  9 /* size of fourth parameter (SpeedAndBiasParameterBlock k+1) */>,
65  public ErrorInterface {
66  public:
67 
68  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
70 
71 
72  typedef ::ceres::SizedCostFunction<15, 7, 9, 7, 9> base_t;
73 
75  static const int kNumResiduals = 15;
76 
78  typedef Eigen::Matrix<double, 15, 15> covariance_t;
79 
81  typedef covariance_t information_t;
82 
84  typedef Eigen::Matrix<double, 15, 15> jacobian_t;
85 
88  typedef Eigen::Matrix<double, 15, 7> jacobian0_t;
89 
91  typedef Eigen::Matrix<double, 15, 9> jacobian1_t;
92 
95  }
96 
98  virtual ~ImuError() {
99  }
100 
107  const okvis::ImuParameters & imuParameters, const okvis::Time& t_0,
108  const okvis::Time& t_1);
109 
124  const okvis::ImuParameters & imuParams,
126  okvis::SpeedAndBias & speedAndBiases,
127  const okvis::Time& t_start, const okvis::Time& t_end,
128  covariance_t* covariance = 0,
129  jacobian_t* jacobian = 0);
130 
139  const okvis::SpeedAndBias & speedAndBiases) const;
140 
141  // setters
142 
147  }
148 
153  }
154 
157  void setT0(const okvis::Time& t_0) {
158  t0_ = t_0;
159  }
160 
163  void setT1(const okvis::Time& t_1) {
164  t1_ = t_1;
165  }
166 
167  // getters
168 
172  return imuParameters_;
173  }
174 
177  return imuMeasurements_;
178  }
179 
181  okvis::Time t0() const {
182  return t0_;
183  }
184 
186  okvis::Time t1() const {
187  return t1_;
188  }
189 
190  // error term and Jacobian implementation
198  virtual bool Evaluate(double const* const * parameters, double* residuals,
199  double** jacobians) const;
200 
210  bool EvaluateWithMinimalJacobians(double const* const * parameters,
211  double* residuals, double** jacobians,
212  double** jacobiansMinimal) const;
213 
214  // sizes
216  size_t residualDim() const {
217  return kNumResiduals;
218  }
219 
221  virtual size_t parameterBlocks() const {
222  return parameter_block_sizes().size();
223  }
224 
228  size_t parameterBlockDim(size_t parameterBlockId) const {
229  return base_t::parameter_block_sizes().at(parameterBlockId);
230  }
231 
233  virtual std::string typeInfo() const {
234  return "ImuError";
235  }
236 
237  protected:
238  // parameters
240 
241  // measurements
243 
244  // times
247 
248  // preintegration stuff. the mutable is a TERRIBLE HACK, but what can I do.
249  mutable std::mutex preintegrationMutex_; //< Protect access of intermediate results.
250  // increments (initialise with identity)
251  mutable Eigen::Quaterniond Delta_q_ = Eigen::Quaterniond(1,0,0,0);
252  mutable Eigen::Matrix3d C_integral_ = Eigen::Matrix3d::Zero();
253  mutable Eigen::Matrix3d C_doubleintegral_ = Eigen::Matrix3d::Zero();
254  mutable Eigen::Vector3d acc_integral_ = Eigen::Vector3d::Zero();
255  mutable Eigen::Vector3d acc_doubleintegral_ = Eigen::Vector3d::Zero();
256 
257  // cross matrix accumulatrion
258  mutable Eigen::Matrix3d cross_ = Eigen::Matrix3d::Zero();
259 
260  // sub-Jacobians
261  mutable Eigen::Matrix3d dalpha_db_g_ = Eigen::Matrix3d::Zero();
262  mutable Eigen::Matrix3d dv_db_g_ = Eigen::Matrix3d::Zero();
263  mutable Eigen::Matrix3d dp_db_g_ = Eigen::Matrix3d::Zero();
264 
266  mutable Eigen::Matrix<double,15,15> P_delta_ = Eigen::Matrix<double,15,15>::Zero();
267 
269  mutable SpeedAndBiases speedAndBiases_ref_ = SpeedAndBiases::Zero();
270 
271  mutable bool redo_ = true;
272  mutable int redoCounter_ = 0;
273 
274  // information matrix and its square root
277 
278 };
279 
280 } // namespace ceres
281 } // namespace okvis
282 
283 #endif /* INCLUDE_OKVIS_CERES_IMUERROR_HPP_ */
Eigen::Matrix3d cross_
Intermediate result.
Definition: ImuError.hpp:258
virtual ~ImuError()
Trivial destructor.
Definition: ImuError.hpp:98
const okvis::ImuMeasurementDeque & imuMeasurements() const
Get the IMU measurements.
Definition: ImuError.hpp:176
Eigen::Vector3d acc_integral_
Intermediate result.
Definition: ImuError.hpp:254
Header file for the ErrorInterface class. A simple interface class that other error classes should in...
Eigen::Matrix< double, 15, 7 > jacobian0_t
The type of the Jacobian w.r.t. poses –.
Definition: ImuError.hpp:88
Eigen::Matrix3d C_integral_
Intermediate result.
Definition: ImuError.hpp:252
Eigen::Matrix< double, 15, 15 > P_delta_
The Jacobian of the increment (w/o biases).
Definition: ImuError.hpp:266
okvis::Time t0_
The start time (i.e. time of the first set of states).
Definition: ImuError.hpp:245
information_t information_
The information matrix for this error term.
Definition: ImuError.hpp:275
okvis::Time t0() const
Get the start time.
Definition: ImuError.hpp:181
bool EvaluateWithMinimalJacobians(double const *const *parameters, double *residuals, double **jacobians, double **jacobiansMinimal) const
This evaluates the error term and additionally computes the Jacobians in the minimal internal represe...
Definition: ImuError.cpp:514
std::deque< ImuMeasurement, Eigen::aligned_allocator< ImuMeasurement > > ImuMeasurementDeque
Definition: Measurements.hpp:163
void setT1(const okvis::Time &t_1)
(Re)set the start time. @param[in] t_1 End time.
Definition: ImuError.hpp:163
static int propagation(const okvis::ImuMeasurementDeque &imuMeasurements, const okvis::ImuParameters &imuParams, okvis::kinematics::Transformation &T_WS, okvis::SpeedAndBias &speedAndBiases, const okvis::Time &t_start, const okvis::Time &t_end, covariance_t *covariance=0, jacobian_t *jacobian=0)
Propagates pose, speeds and biases with given IMU measurements.
Definition: ImuError.cpp:287
okvis::ImuParameters imuParameters_
The IMU parameters.
Definition: ImuError.hpp:239
covariance_t information_t
The type of the information (same matrix dimension as covariance).
Definition: ImuError.hpp:81
static const int kNumResiduals
The number of residuals.
Definition: ImuError.hpp:75
void setImuParameters(const okvis::ImuParameters &imuParameters)
(Re)set the parameters. @param[in] imuParameters The parameters to be used.
Definition: ImuError.hpp:145
SpeedAndBiases speedAndBiases_ref_
Reference biases that are updated when called redoPreintegration.
Definition: ImuError.hpp:269
Eigen::Matrix< double, 9, 1 > SpeedAndBias
Definition: FrameTypedefs.hpp:234
Eigen::Matrix< double, 15, 9 > jacobian1_t
The type of Jacobian w.r.t. Speed and biases.
Definition: ImuError.hpp:91
This file contains the templated measurement structs, structs encapsulating Sensor data and related t...
A class that does homogeneous transformations. This relates a frame A and B: T_AB; it consists of tra...
Definition: Transformation.hpp:76
Header file for the TimeBase, Time and WallTime class.
Simple interface class the errors implemented here should inherit from.
Definition: ErrorInterface.hpp:53
size_t residualDim() const
Residual dimension.
Definition: ImuError.hpp:216
std::mutex preintegrationMutex_
Definition: ImuError.hpp:249
Eigen::Matrix< double, 15, 15 > covariance_t
The type of the covariance.
Definition: ImuError.hpp:78
Eigen::Matrix3d dp_db_g_
Intermediate result.
Definition: ImuError.hpp:263
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
void setImuMeasurements(const okvis::ImuMeasurementDeque &imuMeasurements)
(Re)set the measurements @param[in] imuMeasurements All the IMU measurements.
Definition: ImuError.hpp:151
This file contains some useful assert macros.
Eigen::Matrix3d C_doubleintegral_
Intermediate result.
Definition: ImuError.hpp:253
int redoCounter_
Counts the number of preintegrations for statistics.
Definition: ImuError.hpp:272
const okvis::ImuParameters & imuParameters() const
Get the IMU Parameters.
Definition: ImuError.hpp:171
okvis::Time t1_
The end time (i.e. time of the sedond set of states).
Definition: ImuError.hpp:246
bool redo_
Keeps track of whether or not this redoPreintegration() needs to be called.
Definition: ImuError.hpp:271
int redoPreintegration(const okvis::kinematics::Transformation &T_WS, const okvis::SpeedAndBias &speedAndBiases) const
Propagates pose, speeds and biases with given IMU measurements.
Definition: ImuError.cpp:76
okvis::ImuMeasurementDeque imuMeasurements_
The IMU measurements used. Must be spanning t0_ - t1_.
Definition: ImuError.hpp:242
This file contains some typedefs related to state variables.
okvis::Time t1() const
Get the end time.
Definition: ImuError.hpp:186
Eigen::Matrix< double, 9, 1 > SpeedAndBiases
Definition: FrameTypedefs.hpp:233
Eigen::Matrix3d dalpha_db_g_
Intermediate result.
Definition: ImuError.hpp:261
virtual std::string typeInfo() const
Return parameter block type as string.
Definition: ImuError.hpp:233
ImuError()
Default constructor – assumes information recomputation.
Definition: ImuError.hpp:94
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef::ceres::SizedCostFunction< 15, 7, 9, 7, 9 > base_t
The base in ceres we derive from.
Definition: ImuError.hpp:72
IMU parameters.
Definition: Parameters.hpp:105
Eigen::Matrix< double, 15, 15 > jacobian_t
The type of hte overall Jacobian.
Definition: ImuError.hpp:84
Eigen::Quaterniond Delta_q_
Intermediate result.
Definition: ImuError.hpp:251
Time representation. May either represent wall clock time or ROS clock time.
Definition: Time.hpp:187
Implements a nonlinear IMU factor.
Definition: ImuError.hpp:59
Eigen::Vector3d acc_doubleintegral_
Intermediate result.
Definition: ImuError.hpp:255
This file contains struct definitions that encapsulate parameters and settings.
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
This evaluates the error term and additionally computes the Jacobians.
Definition: ImuError.cpp:507
Eigen::Matrix3d dv_db_g_
Intermediate result.
Definition: ImuError.hpp:262
information_t squareRootInformation_
The square root information matrix for this error term.
Definition: ImuError.hpp:276
virtual size_t parameterBlocks() const
Number of parameter blocks.
Definition: ImuError.hpp:221
size_t parameterBlockDim(size_t parameterBlockId) const
Dimension of an individual parameter block.
Definition: ImuError.hpp:228
This file contains useful typedefs and structs related to frames.
void setT0(const okvis::Time &t_0)
(Re)set the start time. @param[in] t_0 Start time.
Definition: ImuError.hpp:157