OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
RelativePoseError.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 12, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #ifndef INCLUDE_OKVIS_CERES_RELATIVEPOSEERROR_HPP_
40 #define INCLUDE_OKVIS_CERES_RELATIVEPOSEERROR_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 RelativePoseError : public ::ceres::SizedCostFunction<
55  6 /* number of residuals */,
56  7, /* size of first parameter */
57  7 /* size of second parameter */>, 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<6, 7, 7> base_t;
65 
67  static const int kNumResiduals = 6;
68 
70  typedef Eigen::Matrix<double, 6, 6> information_t;
71 
73  typedef Eigen::Matrix<double, 6, 6> covariance_t;
74 
77 
80  RelativePoseError(const Eigen::Matrix<double, 6, 6> & information);
81 
85  RelativePoseError(double translationVariance, double rotationVariance);
86 
88  virtual ~RelativePoseError() {
89  }
90 
91  // setters
95 
96  // getters
99  const information_t& information() const {
100  return information_;
101  }
102 
105  const information_t& covariance() const {
106  return covariance_;
107  }
108 
109  // error term and Jacobian implementation
117  virtual bool Evaluate(double const* const * parameters, double* residuals,
118  double** jacobians) const;
119 
129  bool EvaluateWithMinimalJacobians(double const* const * parameters,
130  double* residuals, double** jacobians,
131  double** jacobiansMinimal) const;
132 
133  // sizes
135  size_t residualDim() const {
136  return kNumResiduals;
137  }
138 
140  size_t parameterBlocks() const {
141  return parameter_block_sizes().size();
142  }
143 
145  size_t parameterBlockDim(size_t parameterBlockId) const {
146  return base_t::parameter_block_sizes().at(parameterBlockId);
147  }
148 
150  virtual std::string typeInfo() const {
151  return "RelativePoseError";
152  }
153 
154  protected:
155 
156  // weighting related
160 
161 };
162 
163 } // namespace ceres
164 } // namespace okvis
165 
166 #endif /* INCLUDE_OKVIS_CERES_RELATIVEPOSEERROR_HPP_ */
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: RelativePoseError.cpp:84
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: RelativePoseError.hpp:150
static const int kNumResiduals
Number of residuals (6).
Definition: RelativePoseError.hpp:67
size_t parameterBlocks() const
Number of parameter blocks.
Definition: RelativePoseError.hpp:140
Relative error between two poses.
Definition: RelativePoseError.hpp:54
const information_t & covariance() const
Get the covariance matrix.
Definition: RelativePoseError.hpp:105
Simple interface class the errors implemented here should inherit from.
Definition: ErrorInterface.hpp:53
size_t residualDim() const
Residual dimension.
Definition: RelativePoseError.hpp:135
information_t squareRootInformation_
The 6x6 square root information matrix.
Definition: RelativePoseError.hpp:158
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
covariance_t covariance_
The 6x6 covariance matrix.
Definition: RelativePoseError.hpp:159
Eigen::Matrix< double, 6, 6 > information_t
The information matrix type (6x6).
Definition: RelativePoseError.hpp:70
This file contains some useful assert macros.
information_t information_
The 6x6 information matrix.
Definition: RelativePoseError.hpp:157
Eigen::Matrix< double, 6, 6 > covariance_t
The covariance matrix type (same as information).
Definition: RelativePoseError.hpp:73
Header file for the Transformation class.
const information_t & information() const
Get the information matrix.
Definition: RelativePoseError.hpp:99
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
This evaluates the error term and additionally computes the Jacobians.
Definition: RelativePoseError.cpp:77
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef::ceres::SizedCostFunction< 6, 7, 7 > base_t
The base class type.
Definition: RelativePoseError.hpp:64
void setInformation(const information_t &information)
Set the information.
Definition: RelativePoseError.cpp:68
size_t parameterBlockDim(size_t parameterBlockId) const
Dimension of an individual parameter block.
Definition: RelativePoseError.hpp:145