snavely_reprojection_error.h
Go to the documentation of this file.
1 /*
2  * snavely_reprojection_error.h
3  *
4  * Created on: Aug 16, 2019
5  * Author: mathieu
6  */
7 
8 #ifndef CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_REPROJECTION_ERROR_H_
9 #define CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_REPROJECTION_ERROR_H_
10 
11 #include "ceres/ceres.h"
12 #include "ceres/rotation.h"
13 
15 
16 namespace ceres {
17 
18 // Templated pinhole camera model for used with Ceres. The camera is
19 // parameterized using 6 parameters: 3 for rotation, 3 for translation. The principal point is not modeled
20 // (i.e. it is assumed be located at the image center).
22  SnavelyReprojectionError(double observed_x, double observed_y, double fx, double fy)
24 
25  template <typename T>
26  bool operator()(const T* const camera,
27  const T* const point,
28  T* residuals) const {
29 
30  // camera[0,1,2] are the angle-axis rotation.
31  T p[3];
33  // camera[3,4,5] are the translation.
34  p[0] += camera[3];
35  p[1] += camera[4];
36  p[2] += camera[5];
37 
38  // Compute the center of distortion.
39  T xp = p[0] / p[2];
40  T yp = p[1] / p[2];
41 
42  // Compute final projected point position.
43  T predicted_x = fx * xp;
44  T predicted_y = fy * yp;
45 
46  // The error is the difference between the predicted and observed position.
47  residuals[0] = predicted_x - observed_x;
48  residuals[1] = predicted_y - observed_y;
49 
50  return true;
51  }
52  // Factory to hide the construction of the CostFunction object from
53  // the client code.
54  static ceres::CostFunction* Create(const double observed_x,
55  const double observed_y,
56  const double fx,
57  const double fy) {
58  return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 6, 3>(
60  }
61  double observed_x;
62  double observed_y;
63  double fx;
64  double fy;
65 };
66 
67 }
68 
69 #endif /* CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_REPROJECTION_ERROR_H_ */
ceres::SnavelyReprojectionError
Definition: snavely_reprojection_error.h:21
ceres::SnavelyReprojectionError::observed_x
double observed_x
Definition: snavely_reprojection_error.h:61
ceres::SnavelyReprojectionError::fy
double fy
Definition: snavely_reprojection_error.h:64
ceres::SnavelyReprojectionError::fx
double fx
Definition: snavely_reprojection_error.h:63
point
point
rotation.h
ceres::SnavelyReprojectionError::Create
static ceres::CostFunction * Create(const double observed_x, const double observed_y, const double fx, const double fy)
Definition: snavely_reprojection_error.h:54
ceres::AngleAxisRotatePoint
void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3])
ceres::SnavelyReprojectionError::observed_y
double observed_y
Definition: snavely_reprojection_error.h:62
p
Point3_ p(2)
Eigen::Triplet< double >
ceres
ceres::SnavelyReprojectionError::SnavelyReprojectionError
SnavelyReprojectionError(double observed_x, double observed_y, double fx, double fy)
Definition: snavely_reprojection_error.h:22
ULogger.h
ULogger class and convenient macros.
ceres::SnavelyReprojectionError::operator()
bool operator()(const T *const camera, const T *const point, T *residuals) const
Definition: snavely_reprojection_error.h:26
camera
Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0))


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:16