corelib
src
optimizer
ceres
bundle
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
14
#include <
rtabmap/utilite/ULogger.h
>
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).
21
struct
SnavelyReprojectionError
{
22
SnavelyReprojectionError
(
double
observed_x
,
double
observed_y
,
double
fx
,
double
fy
)
23
:
observed_x
(
observed_x
),
observed_y
(
observed_y
),
fx
(
fx
),
fy
(
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];
32
ceres::AngleAxisRotatePoint
(
camera
,
point
,
p
);
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>(
59
new
SnavelyReprojectionError
(
observed_x
,
observed_y
,
fx
,
fy
)));
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