Skip to content

Commit 7aa0113

Browse files
ziv-linziv-lin
committed
[FIX][ENH] fix bugs, make code cleaner, change LICENSE.
1 parent 675a1f4 commit 7aa0113

File tree

6 files changed

+430
-862
lines changed

6 files changed

+430
-862
lines changed

‎LICENSE‎

Lines changed: 290 additions & 625 deletions
Large diffs are not rendered by default.

‎src/ceres_icp.hpp‎

Lines changed: 77 additions & 74 deletions
Original file line numberDiff line numberDiff line change
@@ -24,37 +24,53 @@ struct ceres_icp_point2point
2424
Eigen::Matrix<_T, 3, 1> m_current_pt;
2525
Eigen::Matrix<_T, 3, 1> m_closest_pt;
2626
_T m_motion_blur_s;
27-
ceres_icp_point2point( const Eigen::Matrix<_T, 3, 1> &current_pt,
28-
const Eigen::Matrix<_T, 3, 1> &closest_pt,
29-
const _T &motion_blur_s = 1.0 ) : m_current_pt( current_pt ), m_closest_pt( closest_pt ), m_motion_blur_s( motion_blur_s ){};
27+
Eigen::Matrix<_T, 4, 1> m_q_last;
28+
Eigen::Matrix<_T, 3, 1> m_t_last;
29+
_T m_weigh;
30+
ceres_icp_point2point( const Eigen::Matrix<_T, 3, 1> current_pt,
31+
const Eigen::Matrix<_T, 3, 1> closest_pt,
32+
const _T &motion_blur_s = 1.0,
33+
Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
34+
Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) ) : m_current_pt( current_pt ),
35+
m_closest_pt( closest_pt ),
36+
m_motion_blur_s( motion_blur_s ),
37+
m_q_last( q_s ),
38+
m_t_last( t_s )
39+
40+
{
41+
m_weigh = 1.0;
42+
};
3043

3144
template <typename T>
3245
bool operator()( const T *_q, const T *_t, T *residual ) const
3346
{
3447

35-
Eigen::Quaternion<T> q_start{ T( 1.0 ), T( 0 ), T( 0 ), T( 0 ) };
36-
Eigen::Matrix<T, 3, 1> t_start{ T( 0 ), T( 0 ), T( 0 ) };
48+
Eigen::Quaternion<T> q_last{ ( T ) m_q_last( 0 ), ( T ) m_q_last( 1 ), ( T ) m_q_last( 2 ), ( T ) m_q_last( 3 ) };
49+
Eigen::Matrix<T, 3, 1> t_last = m_t_last.template cast<T>();
3750

38-
Eigen::Quaternion<T> q_end{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
39-
Eigen::Matrix<T, 3, 1> t_end{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };
51+
Eigen::Quaternion<T> q_incre{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
52+
Eigen::Matrix<T, 3, 1> t_incre{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };
4053

41-
Eigen::Quaternion<T> q_interpolate = q_start.slerp( ( T ) m_motion_blur_s, q_end );
42-
Eigen::Matrix<T, 3, 1> t_interpolate = t_start * T( 1.0 - m_motion_blur_s ) + t_end * T( m_motion_blur_s );
54+
Eigen::Quaternion<T> q_interpolate = Eigen::Quaternion<T>::Identity().slerp( ( T ) m_motion_blur_s, q_incre );
55+
Eigen::Matrix<T, 3, 1> t_interpolate = t_incre * T( m_motion_blur_s );
4356

4457
Eigen::Matrix<T, 3, 1> pt{ T( m_current_pt( 0 ) ), T( m_current_pt( 1 ) ), T( m_current_pt( 2 ) ) };
45-
Eigen::Matrix<T, 3, 1> pt_tranfromed;
58+
Eigen::Matrix<T, 3, 1> pt_transfromed;
59+
pt_transfromed = q_last * ( q_interpolate * pt + t_interpolate ) + t_last;
4660

47-
pt_tranfromed = q_interpolate * pt + t_interpolate;
61+
//Eigen::Matrix< T, 3, 1 > vec_err = ( pt_transfromed - m_closest_pt );
4862

49-
residual[ 0 ] = pt_tranfromed( 0 ) - T( m_closest_pt( 0 ) );
50-
residual[ 1 ] = pt_tranfromed( 1 ) - T( m_closest_pt( 1 ) );
51-
residual[ 2 ] = pt_tranfromed( 2 ) - T( m_closest_pt( 2 ) );
63+
residual[ 0 ] = ( pt_transfromed( 0 ) - T( m_closest_pt( 0 ) ) ) * T( m_weigh );
64+
residual[ 1 ] = ( pt_transfromed( 1 ) - T( m_closest_pt( 1 ) ) ) * T( m_weigh );
65+
residual[ 2 ] = ( pt_transfromed( 2 ) - T( m_closest_pt( 2 ) ) ) * T( m_weigh );
5266
return true;
5367
};
5468

5569
static ceres::CostFunction *Create( const Eigen::Matrix<_T, 3, 1> current_pt,
5670
const Eigen::Matrix<_T, 3, 1> closest_pt,
57-
const _T motion_blur_s = 1.0 )
71+
const _T motion_blur_s = 1.0,
72+
Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
73+
Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) )
5874
{
5975
return ( new ceres::AutoDiffCostFunction<
6076
ceres_icp_point2point, 3, 4, 3>(
@@ -70,9 +86,9 @@ struct ceres_icp_point2line
7086
Eigen::Matrix<_T, 3, 1> m_target_line_a, m_target_line_b;
7187
Eigen::Matrix<_T, 3, 1> m_unit_vec_ab;
7288
_T m_motion_blur_s;
73-
Eigen::Matrix<_T, 4, 1> m_q_start;
74-
Eigen::Matrix<_T, 3, 1> m_t_start;
75-
_T m_weigth;
89+
Eigen::Matrix<_T, 4, 1> m_q_last;
90+
Eigen::Matrix<_T, 3, 1> m_t_last;
91+
_T m_weigh;
7692
ceres_icp_point2line( const Eigen::Matrix<_T, 3, 1> &current_pt,
7793
const Eigen::Matrix<_T, 3, 1> &target_line_a,
7894
const Eigen::Matrix<_T, 3, 1> &target_line_b,
@@ -81,63 +97,57 @@ struct ceres_icp_point2line
8197
Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) ) : m_current_pt( current_pt ), m_target_line_a( target_line_a ),
8298
m_target_line_b( target_line_b ),
8399
m_motion_blur_s( motion_blur_s ),
84-
m_q_start( q_s ),
85-
m_t_start( t_s )
100+
m_q_last( q_s ),
101+
m_t_last( t_s )
86102
{
87103
m_unit_vec_ab = target_line_b - target_line_a;
88104
m_unit_vec_ab = m_unit_vec_ab / m_unit_vec_ab.norm();
89-
// m_weigth = 1/m_current_pt.norm();
90-
m_weigth = 1.0;
105+
// m_weigh = 1/m_current_pt.norm();
106+
m_weigh = 1.0;
91107
//cout << m_unit_vec_ab.transpose() <<endl;
92108
};
93109

94110
template <typename T>
95111
bool operator()( const T *_q, const T *_t, T *residual ) const
96112
{
97113

98-
Eigen::Quaternion<T> q_I( ( T ) 1.0, ( T ) 0.0, ( T ) 0.0, ( T ) 0.0 );
99-
100-
Eigen::Quaternion<T> q_start{ ( T ) m_q_start( 0 ), ( T ) m_q_start( 1 ), ( T ) m_q_start( 2 ), ( T ) m_q_start( 3 ) };
101-
Eigen::Matrix<T, 3, 1> t_start{ ( T ) m_t_start( 0 ), ( T ) m_t_start( 1 ), ( T ) m_t_start( 2 ) };
114+
Eigen::Quaternion<T> q_last{ ( T ) m_q_last( 0 ), ( T ) m_q_last( 1 ), ( T ) m_q_last( 2 ), ( T ) m_q_last( 3 ) };
115+
Eigen::Matrix<T, 3, 1> t_last = m_t_last.template cast<T>();
102116

103-
Eigen::Quaternion<T> q_end{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
104-
Eigen::Matrix<T, 3, 1> t_end{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };
117+
Eigen::Quaternion<T> q_incre{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
118+
Eigen::Matrix<T, 3, 1> t_incre{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };
105119

106-
Eigen::Quaternion<T> q_interpolate = q_I.slerp( ( T ) m_motion_blur_s, q_end );
107-
// Eigen::Matrix< T, 3, 1 > t_interpolate = t_start * T ( 1.0 - m_motion_blur_s ) + t_end * T ( m_motion_blur_s );
108-
Eigen::Matrix<T, 3, 1> t_interpolate = t_end * T( m_motion_blur_s );
120+
Eigen::Quaternion<T> q_interpolate = Eigen::Quaternion<T>::Identity().slerp( ( T ) m_motion_blur_s, q_incre );
121+
Eigen::Matrix<T, 3, 1> t_interpolate = t_incre * T( m_motion_blur_s );
109122

110-
Eigen::Matrix<T, 3, 1> pt{ T( m_current_pt( 0 ) ), T( m_current_pt( 1 ) ), T( m_current_pt( 2 ) ) };
123+
Eigen::Matrix<T, 3, 1> pt = m_current_pt.template cast<T>();
111124
Eigen::Matrix<T, 3, 1> pt_transfromed;
112-
pt_transfromed = q_start * ( q_interpolate * pt + t_interpolate ) + t_start;
125+
pt_transfromed = q_last * ( q_interpolate * pt + t_interpolate ) + t_last;
113126

114-
Eigen::Matrix<T, 3, 1> tar_line_pt_a{ T( m_target_line_a( 0 ) ), T( m_target_line_a( 1 ) ), T( m_target_line_a( 2 ) ) };
115-
Eigen::Matrix<T, 3, 1> vec_line_ab_unit{ T( m_unit_vec_ab( 0 ) ), T( m_unit_vec_ab( 1 ) ), T( m_unit_vec_ab( 2 ) ) };
127+
Eigen::Matrix<T, 3, 1> tar_line_pt_a = m_target_line_a.template cast<T>();
128+
Eigen::Matrix<T, 3, 1> vec_line_ab_unit = m_unit_vec_ab.template cast<T>();
116129

117130
Eigen::Matrix<T, 3, 1> vec_ac = pt_transfromed - tar_line_pt_a;
118131
Eigen::Matrix<T, 3, 1> residual_vec = vec_ac - Eigen_math::vector_project_on_unit_vector( vec_ac, vec_line_ab_unit );
119132

120-
residual[ 0 ] = residual_vec( 0 ) * T( m_weigth );
121-
residual[ 1 ] = residual_vec( 1 ) * T( m_weigth );
122-
residual[ 2 ] = residual_vec( 2 ) * T( m_weigth );
123-
124-
//cout << " *** " << residual[0] << " " << residual[1] << " " << residual[2] << " *** " << endl;
133+
residual[ 0 ] = residual_vec( 0 ) * T( m_weigh );
134+
residual[ 1 ] = residual_vec( 1 ) * T( m_weigh );
135+
residual[ 2 ] = residual_vec( 2 ) * T( m_weigh );
125136

126-
//cout << residual[0]<< " " << residual[1] << " " <<endl;
127137
return true;
128138
};
129139

130140
static ceres::CostFunction *Create( const Eigen::Matrix<_T, 3, 1> &current_pt,
131141
const Eigen::Matrix<_T, 3, 1> &target_line_a,
132142
const Eigen::Matrix<_T, 3, 1> &target_line_b,
133143
const _T motion_blur_s = 1.0,
134-
Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
135-
Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) )
144+
Eigen::Matrix<_T, 4, 1> q_last = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
145+
Eigen::Matrix<_T, 3, 1> t_last = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) )
136146
{
137147
// TODO: can be vector or distance
138148
return ( new ceres::AutoDiffCostFunction<
139149
ceres_icp_point2line, 3, 4, 3>(
140-
new ceres_icp_point2line( current_pt, target_line_a, target_line_b, motion_blur_s ) ) );
150+
new ceres_icp_point2line( current_pt, target_line_a, target_line_b, motion_blur_s, q_last, t_last ) ) );
141151
}
142152
};
143153

@@ -149,9 +159,9 @@ struct ceres_icp_point2plane
149159
Eigen::Matrix<_T, 3, 1> m_target_line_a, m_target_line_b, m_target_line_c;
150160
Eigen::Matrix<_T, 3, 1> m_unit_vec_ab, m_unit_vec_ac, m_unit_vec_n;
151161
_T m_motion_blur_s;
152-
_T m_weigth;
153-
Eigen::Matrix<_T, 4, 1> m_q_start;
154-
Eigen::Matrix<_T, 3, 1> m_t_start;
162+
_T m_weigh;
163+
Eigen::Matrix<_T, 4, 1> m_q_last;
164+
Eigen::Matrix<_T, 3, 1> m_t_last;
155165
ceres_icp_point2plane( const Eigen::Matrix<_T, 3, 1> &current_pt,
156166
const Eigen::Matrix<_T, 3, 1> &target_line_a,
157167
const Eigen::Matrix<_T, 3, 1> &target_line_b,
@@ -162,8 +172,8 @@ struct ceres_icp_point2plane
162172
m_target_line_b( target_line_b ),
163173
m_target_line_c( target_line_c ),
164174
m_motion_blur_s( motion_blur_s ),
165-
m_q_start( q_s ),
166-
m_t_start( t_s )
175+
m_q_last( q_s ),
176+
m_t_last( t_s )
167177

168178
{
169179
//assert( motion_blur_s <= 1.5 && motion_blur_s >= 0.0 );
@@ -175,42 +185,35 @@ struct ceres_icp_point2plane
175185
m_unit_vec_ac = m_unit_vec_ac / m_unit_vec_ac.norm();
176186

177187
m_unit_vec_n = m_unit_vec_ab.cross( m_unit_vec_ac );
178-
//m_unit_vec_n = m_unit_vec_n / m_unit_vec_n.norm();
179-
// m_weigth = 1/m_current_pt.norm();
180-
m_weigth = 1.0;
181-
//m_weigth = motion_blur_s;
182-
//cout << " --- " << m_unit_vec_n.transpose() << endl;
188+
m_weigh = 1.0;
183189
};
184190

185191
template <typename T>
186192
bool operator()( const T *_q, const T *_t, T *residual ) const
187193
{
188194

189-
Eigen::Quaternion<T> q_I( ( T ) 1.0, ( T ) 0.0, ( T ) 0.0, ( T ) 0.0 );
195+
Eigen::Quaternion<T> q_last{ ( T ) m_q_last( 0 ), ( T ) m_q_last( 1 ), ( T ) m_q_last( 2 ), ( T ) m_q_last( 3 ) };
196+
Eigen::Matrix<T, 3, 1> t_last = m_t_last.template cast<T>();
190197

191-
Eigen::Quaternion<T> q_start{ ( T ) m_q_start( 0 ), ( T ) m_q_start( 1 ), ( T ) m_q_start( 2 ), ( T ) m_q_start( 3 ) };
192-
Eigen::Matrix<T, 3, 1> t_start{ ( T ) m_t_start( 0 ), ( T ) m_t_start( 1 ), ( T ) m_t_start( 2 ) };
198+
Eigen::Quaternion<T> q_incre{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
199+
Eigen::Matrix<T, 3, 1> t_incre{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };
193200

194-
Eigen::Quaternion<T> q_end{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
195-
Eigen::Matrix<T, 3, 1> t_end{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };
201+
Eigen::Quaternion<T> q_interpolate = Eigen::Quaternion<T>::Identity().slerp( ( T ) m_motion_blur_s, q_incre );
202+
Eigen::Matrix<T, 3, 1> t_interpolate = t_incre * T( m_motion_blur_s );
196203

197-
Eigen::Quaternion<T> q_interpolate = q_I.slerp( ( T ) m_motion_blur_s, q_end );
198-
// Eigen::Matrix< T, 3, 1 > t_interpolate = t_start * T ( 1.0 - m_motion_blur_s ) + t_end * T ( m_motion_blur_s );
199-
Eigen::Matrix<T, 3, 1> t_interpolate = t_end * T( m_motion_blur_s );
200-
201-
Eigen::Matrix<T, 3, 1> pt{ T( m_current_pt( 0 ) ), T( m_current_pt( 1 ) ), T( m_current_pt( 2 ) ) };
204+
Eigen::Matrix<T, 3, 1> pt = m_current_pt.template cast<T>();
202205
Eigen::Matrix<T, 3, 1> pt_transfromed;
203-
pt_transfromed = q_start * ( q_interpolate * pt + t_interpolate ) + t_start;
206+
pt_transfromed = q_last * ( q_interpolate * pt + t_interpolate ) + t_last;
204207

205-
Eigen::Matrix<T, 3, 1> tar_line_pt_a{ T( m_target_line_a( 0 ) ), T( m_target_line_a( 1 ) ), T( m_target_line_a( 2 ) ) };
206-
Eigen::Matrix<T, 3, 1> vec_line_plane_norm{ T( m_unit_vec_n( 0 ) ), T( m_unit_vec_n( 1 ) ), T( m_unit_vec_n( 2 ) ) };
208+
Eigen::Matrix<T, 3, 1> tar_line_pt_a = m_target_line_a.template cast<T>();
209+
Eigen::Matrix<T, 3, 1> vec_line_plane_norm = m_unit_vec_n.template cast<T>();
207210

208211
Eigen::Matrix<T, 3, 1> vec_ad = pt_transfromed - tar_line_pt_a;
209-
Eigen::Matrix<T, 3, 1> residual_vec = Eigen_math::vector_project_on_unit_vector( vec_ad, vec_line_plane_norm ) * T( m_weigth );
212+
Eigen::Matrix<T, 3, 1> residual_vec = Eigen_math::vector_project_on_unit_vector( vec_ad, vec_line_plane_norm ) * T( m_weigh );
210213

211-
residual[ 0 ] = residual_vec( 0 ) * T( m_weigth );
212-
residual[ 1 ] = residual_vec( 1 ) * T( m_weigth );
213-
residual[ 2 ] = residual_vec( 2 ) * T( m_weigth );
214+
residual[ 0 ] = residual_vec( 0 ) * T( m_weigh );
215+
residual[ 1 ] = residual_vec( 1 ) * T( m_weigh );
216+
residual[ 2 ] = residual_vec( 2 ) * T( m_weigh );
214217
//cout << residual_vec.rows() << " " <<residual_vec.cols() <<endl;
215218

216219
//cout << " *** " << residual_vec[0] << " " << residual_vec[1] << " " << residual_vec[2] ;
@@ -223,13 +226,13 @@ struct ceres_icp_point2plane
223226
const Eigen::Matrix<_T, 3, 1> &target_line_b,
224227
const Eigen::Matrix<_T, 3, 1> &target_line_c,
225228
const _T motion_blur_s = 1.0,
226-
Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
227-
Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) )
229+
Eigen::Matrix<_T, 4, 1> q_last = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
230+
Eigen::Matrix<_T, 3, 1> t_last = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) )
228231
{
229232
// TODO: can be vector or distance
230233
return ( new ceres::AutoDiffCostFunction<
231234
ceres_icp_point2plane, 3, 4, 3>(
232-
new ceres_icp_point2plane( current_pt, target_line_a, target_line_b, target_line_c, motion_blur_s, q_s, t_s ) ) );
235+
new ceres_icp_point2plane( current_pt, target_line_a, target_line_b, target_line_c, motion_blur_s, q_last, t_last ) ) );
233236
}
234237
};
235238

‎src/eigen_math.hpp‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -10,40 +10,40 @@
1010
namespace Eigen_math
1111
{
1212
template <typename T>
13-
static T vector_project_on_vector(const T &vec_a, const T &vec_b)
13+
static T vector_project_on_vector( const T &vec_a, const T &vec_b )
1414
{
15-
return vec_a.dot(vec_b) * vec_b / (vec_b.norm() * vec_b.norm());
15+
return vec_a.dot( vec_b ) * vec_b / ( vec_b.norm() * vec_b.norm() );
1616
}
1717

1818
template <typename T>
19-
static T vector_project_on_unit_vector(const T &vec_a, const T &vec_b)
19+
static T vector_project_on_unit_vector( const T &vec_a, const T &vec_b )
2020
{
21-
return vec_a.dot(vec_b) * vec_b;
21+
return vec_a.dot( vec_b ) * vec_b;
2222
}
2323

2424
template <typename T>
25-
T vector_angle(const Eigen::Matrix<T, 3, 1> &vec_a, const Eigen::Matrix<T, 3, 1> &vec_b, int if_force_sharp_angle = 0)
25+
T vector_angle( const Eigen::Matrix<T, 3, 1> &vec_a, const Eigen::Matrix<T, 3, 1> &vec_b, int if_force_sharp_angle = 0 )
2626
{
2727
T vec_a_norm = vec_a.norm();
2828
T vec_b_norm = vec_b.norm();
29-
if (vec_a_norm == 0 || vec_b_norm == 0) // zero vector is pararrel to any vector.
29+
if ( vec_a_norm == 0 || vec_b_norm == 0 ) // zero vector is pararrel to any vector.
3030
{
3131
return 0.0;
3232
}
3333
else
3434
{
35-
if (if_force_sharp_angle)
35+
if ( if_force_sharp_angle )
3636
{
3737
// return acos( abs( vec_a.dot( vec_b ) )*0.9999 / ( vec_a_norm * vec_b_norm ) );
38-
return acos(abs(vec_a.dot(vec_b)) / (vec_a_norm * vec_b_norm));
38+
return acos( abs( vec_a.dot( vec_b ) ) / ( vec_a_norm * vec_b_norm ) );
3939
}
4040
else
4141
{
4242
// return acos( (vec_a.dot(vec_b))*0.9999 / (vec_a_norm*vec_b_norm));
43-
return acos((vec_a.dot(vec_b)) / (vec_a_norm * vec_b_norm));
43+
return acos( ( vec_a.dot( vec_b ) ) / ( vec_a_norm * vec_b_norm ) );
4444
}
4545
}
4646
}
4747
} // namespace Eigen_math
4848

49-
#endif
49+
#endif

0 commit comments

Comments
 (0)