OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
ReprojectionError.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: Aug 30, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #ifndef INCLUDE_OKVIS_CERES_REPROJECTIONERROR_HPP_
40 #define INCLUDE_OKVIS_CERES_REPROJECTIONERROR_HPP_
41 
42 #include <vector>
43 #include <memory>
44 #include <ceres/ceres.h>
45 #include <okvis/assert_macros.hpp>
49 
50 namespace okvis {
51 namespace ceres {
52 
55 template<class GEOMETRY_TYPE>
57 {
58  public:
59 
60  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
61  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
62 
63 
64  typedef GEOMETRY_TYPE camera_geometry_t;
65 
67  typedef ::ceres::SizedCostFunction<2, 7, 4, 7> base_t;
68 
70  static const int kNumResiduals = 2;
71 
73  typedef Eigen::Vector2d keypoint_t;
74 
77 
83  ReprojectionError(std::shared_ptr<const camera_geometry_t> cameraGeometry,
84  uint64_t cameraId, const measurement_t & measurement,
85  const covariance_t & information);
86 
88  virtual ~ReprojectionError()
89  {
90  }
91 
92  // setters
96  {
98  }
99 
103  std::shared_ptr<const camera_geometry_t> cameraGeometry)
104  {
105  cameraGeometry_ = cameraGeometry;
106  }
107 
110  virtual void setInformation(const covariance_t& information);
111 
112  // getters
115  virtual const measurement_t& measurement() const
116  {
117  return measurement_;
118  }
119 
122  virtual const covariance_t& information() const
123  {
124  return information_;
125  }
126 
129  virtual const covariance_t& covariance() const
130  {
131  return covariance_;
132  }
133 
134  // error term and Jacobian implementation
142  virtual bool Evaluate(double const* const * parameters, double* residuals,
143  double** jacobians) const;
144 
154  virtual bool EvaluateWithMinimalJacobians(double const* const * parameters,
155  double* residuals,
156  double** jacobians,
157  double** jacobiansMinimal) const;
158  // sizes
160  size_t residualDim() const
161  {
162  return kNumResiduals;
163  }
164 
166  size_t parameterBlocks() const
167  {
168  return parameter_block_sizes().size();
169  }
170 
174  size_t parameterBlockDim(size_t parameterBlockId) const
175  {
176  return base_t::parameter_block_sizes().at(parameterBlockId);
177  }
178 
180  virtual std::string typeInfo() const
181  {
182  return "ReprojectionError";
183  }
184 
185  protected:
186 
187  // the measurement
189 
191  std::shared_ptr<const camera_geometry_t> cameraGeometry_;
192 
193  // weighting related
197 
198 };
199 
200 } // namespace ceres
201 } // namespace okvis
202 
204 
205 #endif /* INCLUDE_OKVIS_CERES_REPROJECTIONERROR_HPP_ */
virtual const covariance_t & covariance() const
Get the covariance matrix.
Definition: ReprojectionError.hpp:129
std::shared_ptr< const camera_geometry_t > cameraGeometry_
The camera model:
Definition: ReprojectionError.hpp:191
Header file for the ErrorInterface class. A simple interface class that other error classes should in...
Header file for the PoseLocalParemerization class.
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
This evaluates the error term and additionally computes the Jacobians.
Definition: ReprojectionError.hpp:77
covariance_t squareRootInformation_
The 2x2 square root information matrix.
Definition: ReprojectionError.hpp:195
virtual std::string typeInfo() const
Residual block type as string.
Definition: ReprojectionError.hpp:180
virtual const measurement_t & measurement() const
Get the measurement.
Definition: ReprojectionError.hpp:115
measurement_t measurement_
The (2D) measurement.
Definition: ReprojectionError.hpp:188
virtual void setMeasurement(const measurement_t &measurement)
Set the measurement.
Definition: ReprojectionError.hpp:95
covariance_t covariance_
The 2x2 covariance matrix.
Definition: ReprojectionError.hpp:196
static const int kNumResiduals
Number of residuals (2)
Definition: ReprojectionError.hpp:70
The 2D keypoint reprojection error.
Definition: ReprojectionError.hpp:56
#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.
2D keypoint reprojection error base class.
Definition: ReprojectionErrorBase.hpp:71
covariance_t information_
The 2x2 information matrix.
Definition: ReprojectionError.hpp:194
virtual void setInformation(const covariance_t &information)
Set the information.
Definition: ReprojectionError.hpp:66
Header file for the ReprojectionErrorBase class.
::ceres::SizedCostFunction< 2, 7, 4, 7 > base_t
The base class type.
Definition: ReprojectionError.hpp:67
Eigen::Vector2d keypoint_t
The keypoint type (measurement type).
Definition: ReprojectionError.hpp:73
size_t parameterBlocks() const
Number of parameter blocks.
Definition: ReprojectionError.hpp:166
size_t parameterBlockDim(size_t parameterBlockId) const
Dimension of an individual parameter block.
Definition: ReprojectionError.hpp:174
Eigen::Vector2d measurement_t
Measurement type (2D).
Definition: ReprojectionErrorBase.hpp:75
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: ReprojectionError.hpp:87
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef GEOMETRY_TYPE camera_geometry_t
Make the camera geometry type accessible.
Definition: ReprojectionError.hpp:64
size_t residualDim() const
Residual dimension.
Definition: ReprojectionError.hpp:160
virtual const covariance_t & information() const
Get the information matrix.
Definition: ReprojectionError.hpp:122
void setCameraGeometry(std::shared_ptr< const camera_geometry_t > cameraGeometry)
Set the underlying camera model.
Definition: ReprojectionError.hpp:102
Header implementation file for the ReprojectionError class.
Eigen::Matrix2d covariance_t
Covariance / information matrix type (2x2).
Definition: ReprojectionErrorBase.hpp:78
uint64_t cameraId() const
Camera ID.
Definition: ReprojectionErrorBase.hpp:57