OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
SpeedAndBiasError.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 10, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #ifndef INCLUDE_OKVIS_CERES_SPEEDANDBIASERROR_HPP_
40 #define INCLUDE_OKVIS_CERES_SPEEDANDBIASERROR_HPP_
41 
42 #include <vector>
43 #include <Eigen/Core>
44 #include "ceres/ceres.h"
45 #include <okvis/assert_macros.hpp>
46 #include <okvis/FrameTypedefs.hpp>
48 
50 namespace okvis {
52 namespace ceres {
53 
54 class SpeedAndBiasError : public ::ceres::SizedCostFunction<
55  9 /* number of residuals */,
56  9 /* size of first parameter */>,
57  public ErrorInterface {
58  public:
59 
60  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
61  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
62 
63 
64  typedef ::ceres::SizedCostFunction<9, 9> base_t;
65 
67  static const int kNumResiduals = 9;
68 
70  typedef Eigen::Matrix<double, 9, 9> information_t;
71 
73  typedef Eigen::Matrix<double, 9, 9> covariance_t;
74 
77 
82  const information_t & information);
83 
89  SpeedAndBiasError(const okvis::SpeedAndBiases& measurement,
90  double speedVariance, double gyrBiasVariance,
91  double accBiasVariance);
92 
94  virtual ~SpeedAndBiasError() {
95  }
96 
97  // setters
102  }
103 
107 
108  // getters
112  return measurement_;
113  }
114 
117  const information_t& information() const {
118  return information_;
119  }
120 
123  const covariance_t& covariance() const {
124  return covariance_;
125  }
126 
127  // error term and Jacobian implementation
135  virtual bool Evaluate(double const* const * parameters, double* residuals,
136  double** jacobians) const;
137 
147  virtual bool EvaluateWithMinimalJacobians(double const* const * parameters,
148  double* residuals,
149  double** jacobians,
150  double** jacobiansMinimal) const;
151 
152  // sizes
154  size_t residualDim() const {
155  return kNumResiduals;
156  }
157 
159  size_t parameterBlocks() const {
160  return parameter_block_sizes().size();
161  }
162 
166  size_t parameterBlockDim(size_t parameterBlockId) const {
167  return base_t::parameter_block_sizes().at(parameterBlockId);
168  }
169 
171  virtual std::string typeInfo() const {
172  return "SpeedAndBiasError";
173  }
174 
175  protected:
176 
177  // the measurement
179 
180  // weighting related
184 
185 };
186 
187 } // namespace ceres
188 } // namespace okvis
189 
190 #endif /* INCLUDE_OKVIS_CERES_SPEEDANDBIASERROR_HPP_ */
information_t information_
The 9x9 information matrix.
Definition: SpeedAndBiasError.hpp:181
virtual 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: SpeedAndBiasError.cpp:89
Header file for the ErrorInterface class. A simple interface class that other error classes should in...
virtual std::string typeInfo() const
Residual block type as string.
Definition: SpeedAndBiasError.hpp:171
size_t residualDim() const
Residual dimension.
Definition: SpeedAndBiasError.hpp:154
okvis::SpeedAndBias measurement_
The (9D) measurement.
Definition: SpeedAndBiasError.hpp:178
const okvis::SpeedAndBias & measurement() const
Get the measurement.
Definition: SpeedAndBiasError.hpp:111
Definition: SpeedAndBiasError.hpp:54
Eigen::Matrix< double, 9, 1 > SpeedAndBias
Definition: FrameTypedefs.hpp:234
void setMeasurement(const okvis::SpeedAndBias &measurement)
Set the measurement.
Definition: SpeedAndBiasError.hpp:100
Simple interface class the errors implemented here should inherit from.
Definition: ErrorInterface.hpp:53
static const int kNumResiduals
Number of residuals (9)
Definition: SpeedAndBiasError.hpp:67
void setInformation(const information_t &information)
Set the information.
Definition: SpeedAndBiasError.cpp:73
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
Eigen::Matrix< double, 9, 9 > information_t
The information matrix type (9x9).
Definition: SpeedAndBiasError.hpp:70
This file contains some useful assert macros.
Eigen::Matrix< double, 9, 1 > SpeedAndBias
Definition: SpeedAndBiasParameterBlock.hpp:52
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef::ceres::SizedCostFunction< 9, 9 > base_t
The base class type.
Definition: SpeedAndBiasError.hpp:64
Eigen::Matrix< double, 9, 1 > SpeedAndBiases
Definition: FrameTypedefs.hpp:233
information_t squareRootInformation_
The 9x9 square root information matrix.
Definition: SpeedAndBiasError.hpp:182
size_t parameterBlockDim(size_t parameterBlockId) const
Dimension of an individual parameter block.
Definition: SpeedAndBiasError.hpp:166
covariance_t covariance_
The 9x9 covariance matrix.
Definition: SpeedAndBiasError.hpp:183
size_t parameterBlocks() const
Number of parameter blocks.
Definition: SpeedAndBiasError.hpp:159
Eigen::Matrix< double, 9, 9 > covariance_t
The covariance matrix type (same as information).
Definition: SpeedAndBiasError.hpp:73
const covariance_t & covariance() const
Get the covariance matrix.
Definition: SpeedAndBiasError.hpp:123
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
This evaluates the error term and additionally computes the Jacobians.
Definition: SpeedAndBiasError.cpp:82
const information_t & information() const
Get the information matrix.
Definition: SpeedAndBiasError.hpp:117
This file contains useful typedefs and structs related to frames.