#ifndefROTATION_H#defineROTATION_H#include#include#include//// math functions needed for rotation conversion. // dot and cross production template<typenameT>inline T DotProduct(const T x[3],const T y[3]){return(x[0]* y[0]+ x[1]* y[1]+ x[2]* y[2]);}template<typenameT>inlinevoidCrossProduct(const T x[3],const T y[3], T result[3]){
result[0]= x[1]* y[2]- x[2]* y[1];
result[1]= x[2]* y[0]- x[0]* y[2];
result[2]= x[0]* y[1]- x[1]* y[0];}//// Converts from a angle anxis to quaternion : template<typenameT>inlinevoidAngleAxisToQuaternion(const T *angle_axis, T *quaternion){const T &a0 = angle_axis[0];const T &a1 = angle_axis[1];const T &a2 = angle_axis[2];const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;if(theta_squared >T(std::numeric_limits<double>::epsilon())){const T theta =sqrt(theta_squared);const T half_theta = theta *T(0.5);const T k =sin(half_theta)/ theta;
quaternion[0]=cos(half_theta);
quaternion[1]= a0 * k;
quaternion[2]= a1 * k;
quaternion[3]= a2 * k;}else{// in case if theta_squared is zeroconst T k(0.5);
quaternion[0]=T(1.0);
quaternion[1]= a0 * k;
quaternion[2]= a1 * k;
quaternion[3]= a2 * k;}}template<typenameT>inlinevoidQuaternionToAngleAxis(const T *quaternion, T *angle_axis){const T &q1 = quaternion[1];const T &q2 = quaternion[2];const T &q3 = quaternion[3];const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;// For quaternions representing non-zero rotation, the conversion// is numercially stableif(sin_squared_theta >T(std::numeric_limits<double>::epsilon())){const T sin_theta =sqrt(sin_squared_theta);const T &cos_theta = quaternion[0];// If cos_theta is negative, theta is greater than pi/2, which// means that angle for the angle_axis vector which is 2 * theta// would be greater than pi...const T two_theta =T(2.0)*((cos_theta <0.0)?atan2(-sin_theta,-cos_theta):atan2(sin_theta, cos_theta));const T k = two_theta / sin_theta;
angle_axis[0]= q1 * k;
angle_axis[1]= q2 * k;
angle_axis[2]= q3 * k;}else{// For zero rotation, sqrt() will produce NaN in derivative since// the argument is zero. By approximating with a Taylor series,// and truncating at one term, the value and first derivatives will be// computed correctly when Jets are used..const T k(2.0);
angle_axis[0]= q1 * k;
angle_axis[1]= q2 * k;
angle_axis[2]= q3 * k;}}template<typenameT>inlinevoidAngleAxisRotatePoint(const T angle_axis[3],const T pt[3], T result[3]){//内联函数作用是是pt经过轴角angle_axis旋转后得到的点resultconst T theta2 =DotProduct(angle_axis, angle_axis);if(theta2 >T(std::numeric_limits<double>::epsilon())){// Away from zero, use the rodriguez formula 远离零，使用罗德里格斯公式//// result = pt costheta +// (w x pt) * sintheta +// w (w . pt) (1 - costheta)//// We want to be careful to only evaluate the square root if the// norm of the angle_axis vector is greater than zero. Otherwise// we get a division by zero.// 我们要注意的是，仅当轴角向量的范数大于零时才计算平方根。否则我们得到的除法是零。const T theta =sqrt(theta2);const T costheta =cos(theta);const T sintheta =sin(theta);const T theta_inverse =1.0/ theta;const T w[3]={angle_axis[0]* theta_inverse,
angle_axis[1]* theta_inverse,
angle_axis[2]* theta_inverse};//这个表示的单位长度的向量// Explicitly inlined evaluation of the cross product for// performance reasons.// 出于性能原因，对交叉积进行了明确的内联评估。/*const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
w[2] * pt[0] - w[0] * pt[2],
w[0] * pt[1] - w[1] * pt[0] };*/
T w_cross_pt[3];CrossProduct(w, pt, w_cross_pt);const T tmp =DotProduct(w, pt)*(T(1.0)- costheta);// (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);
result[0]= pt[0]* costheta + w_cross_pt[0]* sintheta + w[0]* tmp;
result[1]= pt[1]* costheta + w_cross_pt[1]* sintheta + w[1]* tmp;
result[2]= pt[2]* costheta + w_cross_pt[2]* sintheta + w[2]* tmp;}else{// Near zero, the first order Taylor approximation of the rotation// matrix R corresponding to a vector w and angle w is//// R = I + hat(w) * sin(theta)//// But sintheta ~ theta and theta * w = angle_axis, which gives us//// R = I + hat(w)//// and actually performing multiplication with the point pt, gives us// R * pt = pt + w x pt.//// Switching to the Taylor expansion near zero provides meaningful// derivatives when evaluated using Jets.//// Explicitly inlined evaluation of the cross product for// performance reasons./*const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };*/
T w_cross_pt[3];CrossProduct(angle_axis, pt, w_cross_pt);
result[0]= pt[0]+ w_cross_pt[0];
result[1]= pt[1]+ w_cross_pt[1];
result[2]= pt[2]+ w_cross_pt[2];}}#endif// rotation.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
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
1.2.4 输出
/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/cmake-build-debug/pose_graph_ceres_SE3 ./src/sphere.g2o
Input g2o file:./src/sphere.g2o
25009799
iter cost cost_change |gradient||step| tr_ratio tr_radius ls_iter iter_time total_time
04.913445e+050.00e+003.71e+020.00e+000.00e+001.00e+0401.41e-021.82e-0211.707888e+08-1.70e+083.71e+026.61e+03-3.07e+035.00e+0312.01e-012.20e-0121.672780e+08-1.67e+083.71e+026.61e+03-3.57e+031.25e+0311.91e-014.11e-0131.474954e+08-1.47e+083.71e+026.43e+03-5.09e+031.56e+0211.89e-016.00e-0141.225161e+08-1.22e+083.71e+026.63e+03-1.12e+049.77e+0011.87e-017.87e-0159.940300e+06-9.45e+063.71e+021.79e+03-5.70e+033.05e-0111.86e-019.73e-0164.906778e+056.67e+023.52e+024.37e+026.15e+009.16e-0111.94e-011.17e+0076.610989e+05-1.70e+053.52e+021.05e+02-7.05e+024.58e-0111.84e-011.35e+0085.614216e+05-7.07e+043.52e+025.93e+01-4.94e+021.14e-0111.84e-011.54e+0095.033791e+05-1.27e+043.52e+021.90e+01-2.61e+021.43e-0211.84e-011.72e+00104.917091e+05-1.03e+033.52e+022.79e+00-1.40e+028.94e-0411.89e-011.91e+00114.907347e+05-5.70e+013.52e+021.79e-01-1.20e+022.79e-0511.90e-012.10e+00124.906795e+05-1.76e+003.52e+025.62e-03-1.19e+024.37e-0711.84e-012.28e+00
Solver Summary(v 2.0.0-eigen-(3.3.9)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)
Original Reduced
Parameter blocks 25002500
Parameters 1500015000
Residual blocks 97999799
Residuals 5879458794
Minimizer TRUST_REGION
Sparse linear algebra library SUITE_SPARSE
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver SPARSE_NORMAL_CHOLESKY SPARSE_NORMAL_CHOLESKY
Threads 11
Linear solver ordering AUTOMATIC 2500
Cost:
Initial 4.913445e+05
Final 4.906778e+05
Change 6.667215e+02
Minimizer iterations 13
Successful steps 2
Unsuccessful steps 11Time(in seconds):
Preprocessor 0.004068
Residual only evaluation 0.053879(13)
Jacobian & residual evaluation 0.020577(2)
Linear solver 2.403726(13)
Minimizer 2.507487
Postprocessor 0.000336
Total 2.511891
Termination:CONVERGENCE(Function tolerance reached.|cost_change|/cost:5.615779e-08<=1.000000e-06)
进程已结束,退出代码0