OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
PoseError.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_POSEERROR_HPP_
40 #define INCLUDE_OKVIS_CERES_POSEERROR_HPP_
41 
42 #include <vector>
43 #include "ceres/ceres.h"
45 #include <okvis/assert_macros.hpp>
47 
49 namespace okvis {
51 namespace ceres {
52 
54 class PoseError : public ::ceres::SizedCostFunction<6 /* number of residuals */,
55  7 /* size of first parameter */>, public ErrorInterface {
56  public:
57 
58  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
60 
61 
62  typedef ::ceres::SizedCostFunction<6, 7> base_t;
63 
65  static const int kNumResiduals = 6;
66 
68  typedef Eigen::Matrix<double, 6, 6> information_t;
69 
71  typedef Eigen::Matrix<double, 6, 6> covariance_t;
72 
74  PoseError() {}
75 
80  const Eigen::Matrix<double, 6, 6> & information);
81 
87  double translationVariance, double rotationVariance);
88 
90  virtual ~PoseError() {
91  }
92 
95 
100  }
101 
105 
109 
113  return measurement_;
114  }
115 
118  const information_t& information() const {
119  return information_;
120  }
121 
124  const information_t& covariance() const {
125  return covariance_;
126  }
127 
129 
130  // error term and Jacobian implementation
138  virtual bool Evaluate(double const* const * parameters, double* residuals,
139  double** jacobians) const;
140 
150  virtual bool EvaluateWithMinimalJacobians(double const* const * parameters,
151  double* residuals,
152  double** jacobians,
153  double** jacobiansMinimal) const;
154 
155  // sizes
157  size_t residualDim() const {
158  return kNumResiduals;
159  }
160 
162  size_t parameterBlocks() const {
163  return base_t::parameter_block_sizes().size();
164  }
165 
167  size_t parameterBlockDim(size_t parameterBlockId) const {
168  return base_t::parameter_block_sizes().at(parameterBlockId);
169  }
170 
172  virtual std::string typeInfo() const {
173  return "PoseError";
174  }
175 
176  protected:
177 
178  // the measurement
180 
181  // weighting related
185 
186 };
187 
188 } // namespace ceres
189 } // namespace okvis
190 
191 #endif /* INCLUDE_OKVIS_CERES_POSEERROR_HPP_ */
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
This evaluates the error term and additionally computes the Jacobians.
Definition: PoseError.cpp:79
void setInformation(const information_t &information)
Set the information.
Definition: PoseError.cpp:70
virtual std::string typeInfo() const
Return parameter block type as string.
Definition: PoseError.hpp:172
Header file for the ErrorInterface class. A simple interface class that other error classes should in...
virtual ~PoseError()
Trivial destructor.
Definition: PoseError.hpp:90
Eigen::Matrix< double, 6, 6 > information_t
The information matrix type (6x6).
Definition: PoseError.hpp:68
information_t information_
The 6x6 information matrix.
Definition: PoseError.hpp:182
void setMeasurement(const okvis::kinematics::Transformation &measurement)
Set the measurement.
Definition: PoseError.hpp:98
size_t parameterBlocks() const
Number of parameter blocks.
Definition: PoseError.hpp:162
const information_t & covariance() const
Get the covariance matrix.
Definition: PoseError.hpp:124
A class that does homogeneous transformations. This relates a frame A and B: T_AB; it consists of tra...
Definition: Transformation.hpp:76
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef::ceres::SizedCostFunction< 6, 7 > base_t
The base class type.
Definition: PoseError.hpp:62
Simple interface class the errors implemented here should inherit from.
Definition: ErrorInterface.hpp:53
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
This file contains some useful assert macros.
PoseError()
Default constructor.
Definition: PoseError.hpp:74
Absolute error of a pose.
Definition: PoseError.hpp:54
static const int kNumResiduals
The number of residuals (6).
Definition: PoseError.hpp:65
const information_t & information() const
Get the information matrix.
Definition: PoseError.hpp:118
information_t squareRootInformation_
The 6x6 square root information matrix.
Definition: PoseError.hpp:183
Header file for the Transformation class.
okvis::kinematics::Transformation measurement_
The pose measurement.
Definition: PoseError.hpp:179
size_t residualDim() const
Residual dimension.
Definition: PoseError.hpp:157
const okvis::kinematics::Transformation & measurement() const
Get the measurement.
Definition: PoseError.hpp:112
covariance_t covariance_
The 6x6 covariance matrix.
Definition: PoseError.hpp:184
Eigen::Matrix< double, 6, 6 > covariance_t
The covariance matrix type (same as information).
Definition: PoseError.hpp:71
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: PoseError.cpp:86
size_t parameterBlockDim(size_t parameterBlockId) const
Dimension of an individual parameter block.
Definition: PoseError.hpp:167