forked from programath/pose_graph
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrelative_pose_factor.h
142 lines (116 loc) · 7.04 KB
/
relative_pose_factor.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
#ifndef RELATIVE_POSE_FACTOR_H
#define RELATIVE_POSE_FACTOR_H
#include <ceres/ceres.h>
#include <Eigen/Dense>
#include "util.h"
class RelativePoseFactor : public ceres::SizedCostFunction<6, 7, 7>
{
public:
RelativePoseFactor(const Eigen::Matrix<double, 3, 4> & rel_pose) {
rel_t_ = rel_pose.block<3, 1>(0, 3);
rel_rot_ = Eigen::Quaterniond(rel_pose.block<3, 3>(0, 0));
sqrt_info << 100.0, 100, 100, 1000, 1000, 1000;
}
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
Eigen::Vector3d Pi = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond Qi = Eigen::Quaterniond(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
Eigen::Vector3d Pj = Eigen::Vector3d(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Quaterniond Qj = Eigen::Quaterniond(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
// Ti^1 * Tj = Rel_pose --> Rel_pose^-1 * Ti^-1 * Tj = err. (Ti*Rel_pose)^-1 * Tj*[1, 1/2\theta] = 2[0 I]((Ti*Rel_pose)^-1 * Tj)_L * [0, 1/2I]
// Ti^1 * Tj = Rel_pose --> Rel_pose^-1 * Ti^-1 * Tj = err. (Ti*[1,1/2\theta]*Rel_pose)^-1 * Tj
// = -[0, I] * ((Tj*-1 * Ti*[1,1/2\theta]*Rel_pose)) = -2[0 I] * (Tj*-1 * Ti)_L * (Rel_pose)R * [0, 1/2I]
Eigen::Quaterniond Qj_est = Qi * rel_rot_;
Eigen::Vector3d Pj_est = Qi * rel_t_ + Pi; // FIXME: remove the rotation part
Eigen::Quaterniond err = Qj_est.inverse() * Qj;
Eigen::Map<Eigen::Matrix<double, 6, 1> > residual(residuals);
residual.block<3, 1>(0, 0) = Pj - Pj_est;
if (err.w() > 0)
residual.block<3, 1>(3, 0) = 2.0 * err.vec();
else
residual.block<3, 1>(3, 0) = -2.0 * err.vec();
residual = sqrt_info.asDiagonal() * residual;
if (jacobians) {
Eigen::Matrix3d Ri = Qi.toRotationMatrix();
Eigen::Matrix3d Rj = Qj.toRotationMatrix();
if (jacobians[0]) {
Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor> > jacobian_pose_i(jacobians[0]);
jacobian_pose_i.setZero();
jacobian_pose_i.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
jacobian_pose_i.block<3, 3>(0, 3) = Ri * skewSymmetric(rel_t_);
jacobian_pose_i.block<3, 3>(3, 3) = -(Qleft(Qj.inverse() * Qi) * Qright(rel_rot_)).block<3,3>(1,1);
jacobian_pose_i = sqrt_info.asDiagonal() * jacobian_pose_i;
}
if (jacobians[1]) {
Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor> > jacobian_pose_j(jacobians[1]);
jacobian_pose_j.setZero();
jacobian_pose_j.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
jacobian_pose_j.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity();
jacobian_pose_j.block<3, 3>(3, 3) = Qleft(err).block<3,3>(1,1);
jacobian_pose_j = sqrt_info.asDiagonal() * jacobian_pose_j;
}
}
return true;
}
void check(double const *const *parameters) {
double * res = new double [6];
double ** jaco = new double * [2];
jaco[0] = new double [6 * 7];
jaco[1] = new double [6 * 7];
Evaluate(parameters, res, jaco);
std::cout << "my" << std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor> >(jaco[0]) << std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor> >(jaco[1]) << std::endl;
Eigen::Vector3d Pi = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond Qi = Eigen::Quaterniond(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
Eigen::Vector3d Pj = Eigen::Vector3d(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Quaterniond Qj = Eigen::Quaterniond(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
// Ti^1 * Tj = Rel_pose --> Rel_pose^-1 * Ti^-1 * Tj = err. (Ti*Rel_pose)^-1 * Tj*[1, 1/2\theta] = 2[0 I]((Ti*Rel_pose)^-1 * Tj)_L * [0, 1/2I]
// Ti^1 * Tj = Rel_pose --> Rel_pose^-1 * Ti^-1 * Tj = err. (Ti*[1,1/2\theta]*Rel_pose)^-1 * Tj
// = -[0, I] * ((Tj*-1 * Ti*[1,1/2\theta]*Rel_pose)) = -2[0 I] * (Tj*-1 * Ti)_L * (Rel_pose)R * [0, 1/2I]
Eigen::Quaterniond Qj_est = Qi * rel_rot_;
Eigen::Vector3d Pj_est = Qi * rel_t_ + Pi;
Eigen::Quaterniond err = Qj_est.inverse() * Qj;
Eigen::Matrix<double, 6, 1> residual;
residual.block<3, 1>(0, 0) = Pj - Pj_est;
if (err.w() > 0)
residual.block<3, 1>(3, 0) = 2.0 * err.vec();
else
residual.block<3, 1>(3, 0) = -2.0 * err.vec();
residual = sqrt_info.asDiagonal() * residual;
const double eps = 1e-6;
Eigen::Matrix<double, 6, 12> num_jacobian;
for (int k = 0; k < 12; ++k) {
Eigen::Vector3d Pi = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond Qi = Eigen::Quaterniond(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
Eigen::Vector3d Pj = Eigen::Vector3d(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Quaterniond Qj = Eigen::Quaterniond(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
int a = k / 3, b = k % 3;
Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps;
if (a == 0)
Pi += delta;
else if (a == 1)
Qi = Qi * deltaQ(delta);
else if (a == 2)
Pj += delta;
else if (a == 3)
Qj = Qj * deltaQ(delta);
Eigen::Quaterniond Qj_est = Qi * rel_rot_;
Eigen::Vector3d Pj_est = Qi * rel_t_ + Pi;
Eigen::Quaterniond err = Qj_est.inverse() * Qj;
Eigen::Matrix<double, 6, 1> tmp_residual;
tmp_residual.block<3, 1>(0, 0) = Pj - Pj_est;
if (err.w() > 0)
tmp_residual.block<3, 1>(3, 0) = 2.0 * err.vec();
else
tmp_residual.block<3, 1>(3, 0) = -2.0 * err.vec();
tmp_residual = sqrt_info.asDiagonal() * tmp_residual;
num_jacobian.col(k) = (tmp_residual - residual) / eps;
}
std::cout << "num" << std::endl;
std::cout << num_jacobian << std::endl;
}
Eigen::Vector3d rel_t_;
Eigen::Quaterniond rel_rot_;
Eigen::Vector<double, 6, 1> sqrt_info;
};
#endif