OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
HomogeneousPointError.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 30, 2014
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #ifndef INCLUDE_OKVIS_CERES_HOMOGENEOUSPOINTERROR_HPP_
40 #define INCLUDE_OKVIS_CERES_HOMOGENEOUSPOINTERROR_HPP_
41 
42 #include <vector>
43 #include <ceres/ceres.h>
44 #include <Eigen/Core>
46 #include <okvis/assert_macros.hpp>
47 
49 namespace okvis {
51 namespace ceres {
52 
54 class HomogeneousPointError : public ::ceres::SizedCostFunction<
55  3 /* number of residuals */, 4 /* size of first parameter */>,
56  public ErrorInterface {
57  public:
58  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
59  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
60 
61 
62  typedef ::ceres::SizedCostFunction<3, 4> base_t;
63 
65  static const int kNumResiduals = 3;
66 
68  typedef Eigen::Matrix<double, 3, 3> information_t;
69 
71  typedef Eigen::Matrix<double, 3, 3> covariance_t;
72 
75 
79  HomogeneousPointError(const Eigen::Vector4d & measurement,
80  const information_t & information);
81 
85  HomogeneousPointError(const Eigen::Vector4d & measurement, double variance);
86 
88  virtual ~HomogeneousPointError() {
89  }
90 
91  // setters
94  void setMeasurement(const Eigen::Vector4d & measurement) {
96  }
97 
101 
102  // getters
105  const Eigen::Vector4d& measurement() const {
106  return measurement_;
107  }
108 
111  const information_t& information() const {
112  return information_;
113  }
114 
117  const information_t& covariance() const {
118  return covariance_;
119  }
120 
128  virtual bool Evaluate(double const* const * parameters, double* residuals,
129  double** jacobians) const;
130 
140  virtual bool EvaluateWithMinimalJacobians(double const* const * parameters,
141  double* residuals,
142  double** jacobians,
143  double** jacobiansMinimal) const;
144 
145  // sizes
147  size_t residualDim() const {
148  return kNumResiduals;
149  }
150 
152  size_t parameterBlocks() const {
153  return base_t::parameter_block_sizes().size();
154  }
155 
159  size_t parameterBlockDim(size_t parameterBlockId) const {
160  return base_t::parameter_block_sizes().at(parameterBlockId);
161  }
162 
164  virtual std::string typeInfo() const {
165  return "HomogeneousPointError";
166  }
167 
168  protected:
169 
170  // the measurement
171  Eigen::Vector4d measurement_;
172 
173  // weighting related
177 
178 };
179 
180 } // namespace ceres
181 } // namespace okvis
182 
183 #endif /* INCLUDE_OKVIS_CERES_HOMOGENEOUSPOINTERROR_HPP_ */
virtual std::string typeInfo() const
Return parameter block type as string.
Definition: HomogeneousPointError.hpp:164
Header file for the ErrorInterface class. A simple interface class that other error classes should in...
virtual bool EvaluateWithMinimalJacobians(double const *const *parameters, double *residuals, double **jacobians, double **jacobiansMinimal) const
EvaluateWithMinimalJacobians This evaluates the error term and additionally computes the Jacobians in...
Definition: HomogeneousPointError.cpp:79
Eigen::Matrix< double, 3, 3 > covariance_t
The covariance matrix type (same as information).
Definition: HomogeneousPointError.hpp:71
Eigen::Vector4d measurement_
The (4D) measurement.
Definition: HomogeneousPointError.hpp:171
information_t _squareRootInformation
The 4x4 square root information matrix.
Definition: HomogeneousPointError.hpp:175
covariance_t covariance_
The 4x4 covariance matrix.
Definition: HomogeneousPointError.hpp:176
Eigen::Matrix< double, 3, 3 > information_t
The information matrix type (3x3).
Definition: HomogeneousPointError.hpp:68
Simple interface class the errors implemented here should inherit from.
Definition: ErrorInterface.hpp:53
const information_t & information() const
Get the information matrix.
Definition: HomogeneousPointError.hpp:111
size_t parameterBlockDim(size_t parameterBlockId) const
Dimension of an individual parameter block.
Definition: HomogeneousPointError.hpp:159
size_t parameterBlocks() const
Number of parameter blocks.
Definition: HomogeneousPointError.hpp:152
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
const Eigen::Vector4d & measurement() const
Get the measurement.
Definition: HomogeneousPointError.hpp:105
This file contains some useful assert macros.
size_t residualDim() const
Residual dimension.
Definition: HomogeneousPointError.hpp:147
information_t information_
The 4x4 information matrix.
Definition: HomogeneousPointError.hpp:174
void setMeasurement(const Eigen::Vector4d &measurement)
Set the measurement.
Definition: HomogeneousPointError.hpp:94
void setInformation(const information_t &information)
Set the information.
Definition: HomogeneousPointError.cpp:62
Absolute error of a homogeneous point (landmark).
Definition: HomogeneousPointError.hpp:54
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef::ceres::SizedCostFunction< 3, 4 > base_t
The base class type.
Definition: HomogeneousPointError.hpp:62
const information_t & covariance() const
Get the covariance matrix.
Definition: HomogeneousPointError.hpp:117
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
This evaluates the error term and additionally computes the Jacobians.
Definition: HomogeneousPointError.cpp:71
static const int kNumResiduals
Number of residuals (3)
Definition: HomogeneousPointError.hpp:65