Graphite
Loading...
Searching...
No Matches
reprojection_error.cuh
Go to the documentation of this file.
1
2#pragma once
3#include "projection_jacobians.cuh"
4namespace graphite {
5
6template <typename D, typename M, typename T>
7__device__ static void bal_reprojection_error(const D *camera, const D *point,
8 const M *obs, D *error) {
9 // Compute the reprojection error according to the camera model at
10 // https://grail.cs.washington.edu/projects/bal/
11
12 Eigen::Map<const Eigen::Matrix<D, 3, 1>> X(point);
13 Eigen::Map<const Eigen::Matrix<D, 9, 1>> cam(camera);
14 Eigen::Map<const Eigen::Matrix<T, 2, 1>> observation(obs->data());
15 Eigen::Matrix<D, 3, 1> P;
16
17 // Rodrigues formula for rotation vector (angle-axis) with Taylor expansion
18 // for small theta Adapted from g2o bal example
19 Eigen::Matrix<D, 3, 1> rvec = cam.template head<3>();
20 D theta = rvec.norm();
21
22 if (theta > D(0)) {
23 Eigen::Matrix<D, 3, 1> axis = rvec / theta;
24 D cth = cos(theta);
25 D sth = sin(theta);
26
27 Eigen::Matrix<D, 3, 1> axx = axis.cross(X);
28 D adx = axis.dot(X);
29
30 P = X * cth + axx * sth + axis * adx * (D(1) - cth);
31
32 } else {
33 auto rxx = rvec.cross(X);
34 P = X + rxx; // + D(0.5) * rvec.cross(rxx);
35 }
36
37 // Translate
38 Eigen::Matrix<D, 3, 1> t = cam.template segment<3>(3);
39 P += t;
40
41 // Perspective division
42 Eigen::Matrix<D, 2, 1> p = -P.template head<2>() / P(2);
43
44 // Radial distortion
45 D f = cam(6);
46 D k1 = cam(7);
47 D k2 = cam(8);
48
49 D r2 = p.squaredNorm();
50 D radial_distortion = D(1.0) + k1 * r2 + k2 * r2 * r2;
51
52 // Project to pixel coordinates and compute reprojection error
53 Eigen::Matrix<D, 2, 1> reprojection_error =
54 f * radial_distortion * p - observation.template cast<D>();
55
56 // Assign the error
57 error[0] = reprojection_error(0);
58 error[1] = reprojection_error(1);
59}
60
61template <typename D, typename M, typename T>
62__device__ static void bal_reprojection_error_simple(const D *camera,
63 const D *point,
64 const M *obs, D *error) {
65
66 Eigen::Map<const Eigen::Matrix<D, 3, 1>> X(point);
67 Eigen::Map<const Eigen::Matrix<D, 9, 1>> cam(camera);
68 Eigen::Map<const Eigen::Matrix<T, 2, 1>> observation(obs->data());
69
70 // Extract rotation vector and build rotation matrix using Eigen's AngleAxis
71 Eigen::Matrix<D, 3, 1> rvec = cam.template head<3>();
72 D theta = rvec.norm();
73 Eigen::Matrix<D, 3, 3> R = Eigen::Matrix<D, 3, 3>::Identity();
74
75 if (theta > D(0)) {
76 Eigen::AngleAxis<D> angle_axis(theta, rvec / theta);
77 R = angle_axis.toRotationMatrix();
78 }
79
80 // Apply rotation and translation
81 Eigen::Matrix<D, 3, 1> t = cam.template segment<3>(3);
82 Eigen::Matrix<D, 3, 1> P = R * X + t;
83
84 // Perspective division
85 Eigen::Matrix<D, 2, 1> p = -P.template head<2>() / P(2);
86
87 // Radial distortion
88 D f = cam(6);
89 D k1 = cam(7);
90 D k2 = cam(8);
91 D r2 = p.squaredNorm();
92 D radial_distortion = D(1.0) + k1 * r2 + k2 * r2 * r2;
93 // Project to pixel coordinates and compute reprojection error
94 Eigen::Matrix<D, 2, 1> reprojection_error =
95 f * radial_distortion * p - observation.template cast<D>();
96 // Assign the error
97 error[0] = reprojection_error(0);
98 error[1] = reprojection_error(1);
99}
100
101template <typename T, typename D, size_t I>
102__device__ static void bal_jacobian_simple(const T *camera, const T *point,
103 const Eigen::Matrix<T, 2, 1> *obs,
104 D *jacobian) {
105 Eigen::Map<const Eigen::Matrix<D, 3, 1>> X(point);
106 Eigen::Map<const Eigen::Matrix<D, 9, 1>> cam(camera);
107
108 Eigen::Matrix<T, 2, 9> Jcam;
109 Eigen::Matrix<T, 2, 3> Jpoint;
110
111 const Eigen::Matrix<T, 3, 1> rvec = cam.template head<3>();
112 const Eigen::Matrix<T, 3, 1> t = cam.template segment<3>(3);
113 const T f = cam(6);
114 const T k1 = cam(7);
115 const T k2 = cam(8);
116
117 projection_simple<T>(rvec, t, f, k1, k2, X, Jcam, Jpoint);
118
119 if constexpr (I == 0) {
120 Eigen::Map<Eigen::Matrix<D, 2, 9>> Jcam_map(jacobian);
121 Jcam_map = Jcam;
122 } else if constexpr (I == 1) {
123 Eigen::Map<Eigen::Matrix<D, 2, 3>> Jpoint_map(jacobian);
124 Jpoint_map = Jpoint;
125 }
126}
127
128} // namespace graphite