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: Sep 2, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
41 
43 namespace okvis {
45 namespace ceres {
46 
47 // Default constructor.
48 template<class GEOMETRY_T>
50  : cameraGeometry_(new camera_geometry_t) {
51 }
52 
53 // Construct with measurement and information matrix.
54 template<class GEOMETRY_T>
56  std::shared_ptr<const camera_geometry_t> cameraGeometry, uint64_t cameraId,
57  const measurement_t & measurement, const covariance_t & information) {
58  setCameraId(cameraId);
59  setMeasurement(measurement);
60  setInformation(information);
61  setCameraGeometry(cameraGeometry);
62 }
63 
64 // Set the information.
65 template<class GEOMETRY_T>
67  const covariance_t& information) {
68  information_ = information;
69  covariance_ = information.inverse();
70  // perform the Cholesky decomposition on order to obtain the correct error weighting
71  Eigen::LLT<Eigen::Matrix2d> lltOfInformation(information_);
72  squareRootInformation_ = lltOfInformation.matrixL().transpose();
73 }
74 
75 // This evaluates the error term and additionally computes the Jacobians.
76 template<class GEOMETRY_T>
77 bool ReprojectionError<GEOMETRY_T>::Evaluate(double const* const * parameters,
78  double* residuals,
79  double** jacobians) const {
80 
81  return EvaluateWithMinimalJacobians(parameters, residuals, jacobians, NULL); // debug test only
82 }
83 
84 // This evaluates the error term and additionally computes
85 // the Jacobians in the minimal internal representation.
86 template<class GEOMETRY_T>
88  double const* const * parameters, double* residuals, double** jacobians,
89  double** jacobiansMinimal) const {
90 
91  // We avoid the use of okvis::kinematics::Transformation here due to quaternion normalization and so forth.
92  // This only matters in order to be able to check Jacobians with numeric differentiation chained,
93  // first w.r.t. q and then d_alpha.
94 
95  // pose: world to sensor transformation
96  Eigen::Map<const Eigen::Vector3d> t_WS_W(&parameters[0][0]);
97  const Eigen::Quaterniond q_WS(parameters[0][6], parameters[0][3],
98  parameters[0][4], parameters[0][5]);
99 
100  // the point in world coordinates
101  Eigen::Map<const Eigen::Vector4d> hp_W(&parameters[1][0]);
102  //std::cout << hp_W.transpose() << std::endl;
103 
104  // the sensor to camera transformation
105  Eigen::Map<const Eigen::Vector3d> t_SC_S(&parameters[2][0]);
106  const Eigen::Quaterniond q_SC(parameters[2][6], parameters[2][3],
107  parameters[2][4], parameters[2][5]);
108 
109  // transform the point into the camera:
110  Eigen::Matrix3d C_SC = q_SC.toRotationMatrix();
111  Eigen::Matrix3d C_CS = C_SC.transpose();
112  Eigen::Matrix4d T_CS = Eigen::Matrix4d::Identity();
113  T_CS.topLeftCorner<3, 3>() = C_CS;
114  T_CS.topRightCorner<3, 1>() = -C_CS * t_SC_S;
115  Eigen::Matrix3d C_WS = q_WS.toRotationMatrix();
116  Eigen::Matrix3d C_SW = C_WS.transpose();
117  Eigen::Matrix4d T_SW = Eigen::Matrix4d::Identity();
118  T_SW.topLeftCorner<3, 3>() = C_SW;
119  T_SW.topRightCorner<3, 1>() = -C_SW * t_WS_W;
120  Eigen::Vector4d hp_S = T_SW * hp_W;
121  Eigen::Vector4d hp_C = T_CS * hp_S;
122 
123  // calculate the reprojection error
124  measurement_t kp;
125  Eigen::Matrix<double, 2, 4> Jh;
126  Eigen::Matrix<double, 2, 4> Jh_weighted;
127  if (jacobians != NULL) {
128  cameraGeometry_->projectHomogeneous(hp_C, &kp, &Jh);
129  Jh_weighted = squareRootInformation_ * Jh;
130  } else {
131  cameraGeometry_->projectHomogeneous(hp_C, &kp);
132  }
133 
134  measurement_t error = measurement_ - kp;
135 
136  // weight:
137  measurement_t weighted_error = squareRootInformation_ * error;
138 
139  // assign:
140  residuals[0] = weighted_error[0];
141  residuals[1] = weighted_error[1];
142 
143  // check validity:
144  bool valid = true;
145  if (fabs(hp_C[3]) > 1.0e-8) {
146  Eigen::Vector3d p_C = hp_C.template head<3>() / hp_C[3];
147  if (p_C[2] < 0.2) { // 20 cm - not very generic... but reasonable
148  //std::cout<<"INVALID POINT"<<std::endl;
149  valid = false;
150  }
151  }
152 
153  // calculate jacobians, if required
154  // This is pretty close to Paul Furgale's thesis. eq. 3.100 on page 40
155  if (jacobians != NULL) {
156  if (jacobians[0] != NULL) {
157  Eigen::Vector3d p = hp_W.head<3>() - t_WS_W * hp_W[3];
158  Eigen::Matrix<double, 4, 6> J;
159  J.setZero();
160  J.topLeftCorner<3, 3>() = C_SW * hp_W[3];
161  J.topRightCorner<3, 3>() = -C_SW * okvis::kinematics::crossMx(p);
162 
163  // compute the minimal version
164  Eigen::Matrix<double, 2, 6, Eigen::RowMajor> J0_minimal;
165  J0_minimal = Jh_weighted * T_CS * J;
166  if (!valid)
167  J0_minimal.setZero();
168 
169  // pseudo inverse of the local parametrization Jacobian:
170  Eigen::Matrix<double, 6, 7, Eigen::RowMajor> J_lift;
171  PoseLocalParameterization::liftJacobian(parameters[0], J_lift.data());
172 
173  // hallucinate Jacobian w.r.t. state
174  Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor> > J0(
175  jacobians[0]);
176  J0 = J0_minimal * J_lift;
177 
178  // if requested, provide minimal Jacobians
179  if (jacobiansMinimal != NULL) {
180  if (jacobiansMinimal[0] != NULL) {
181  Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor> > J0_minimal_mapped(
182  jacobiansMinimal[0]);
183  J0_minimal_mapped = J0_minimal;
184  }
185  }
186 
187  }
188  if (jacobians[1] != NULL) {
189  Eigen::Map<Eigen::Matrix<double, 2, 4, Eigen::RowMajor> > J1(
190  jacobians[1]); // map the raw pointer to an Eigen matrix for convenience
191  Eigen::Matrix4d T_CW = (T_CS * T_SW);
192  J1 = -Jh_weighted * T_CW;
193  if (!valid)
194  J1.setZero();
195 
196  // if requested, provide minimal Jacobians
197  if (jacobiansMinimal != NULL) {
198  if (jacobiansMinimal[1] != NULL) {
199  Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor> > J1_minimal_mapped(
200  jacobiansMinimal[1]);
201  Eigen::Matrix<double, 4, 3> S;
202  S.setZero();
203  S.topLeftCorner<3, 3>().setIdentity();
204  J1_minimal_mapped = J1 * S; // this is for Euclidean-style perturbation only.
205  }
206  }
207  }
208  if (jacobians[2] != NULL) {
209  Eigen::Vector3d p = hp_S.head<3>() - t_SC_S * hp_S[3];
210  Eigen::Matrix<double, 4, 6> J;
211  J.setZero();
212  J.topLeftCorner<3, 3>() = C_CS * hp_S[3];
213  J.topRightCorner<3, 3>() = -C_CS * okvis::kinematics::crossMx(p);
214 
215  // compute the minimal version
216  Eigen::Matrix<double, 2, 6, Eigen::RowMajor> J2_minimal;
217  J2_minimal = Jh_weighted * J;
218  if (!valid)
219  J2_minimal.setZero();
220 
221  // pseudo inverse of the local parametrization Jacobian:
222  Eigen::Matrix<double, 6, 7, Eigen::RowMajor> J_lift;
223  PoseLocalParameterization::liftJacobian(parameters[2], J_lift.data());
224 
225  // hallucinate Jacobian w.r.t. state
226  Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor> > J2(
227  jacobians[2]);
228  J2 = J2_minimal * J_lift;
229 
230  // if requested, provide minimal Jacobians
231  if (jacobiansMinimal != NULL) {
232  if (jacobiansMinimal[2] != NULL) {
233  Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor> > J2_minimal_mapped(
234  jacobiansMinimal[2]);
235  J2_minimal_mapped = J2_minimal;
236  }
237  }
238  }
239  }
240 
241  return true;
242 }
243 
244 }
245 }
Eigen::Matrix< Scalar_T, 3, 3 > crossMx(Scalar_T x, Scalar_T y, Scalar_T z)
Cross matrix of a vector [x,y,z]. Adopted from Schweizer-Messer by Paul Furgale.
Definition: operators.hpp:62
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
ReprojectionError()
Default constructor.
Definition: ReprojectionError.hpp:49
virtual void setInformation(const covariance_t &information)
Set the information.
Definition: ReprojectionError.hpp:66
static bool liftJacobian(const double *x, double *jacobian)
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: PoseLocalParameterization.cpp:131
Header file for the Transformation class.
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
File with some helper functions related to matrix/vector operations.
Eigen::Matrix2d covariance_t
Covariance / information matrix type (2x2).
Definition: ReprojectionErrorBase.hpp:78