Skip to content

Commit

Permalink
[FIX][ENH] fix bugs, make code cleaner, change LICENSE.
Browse files Browse the repository at this point in the history
  • Loading branch information
@ziv-lin
ziv-lin committed Jul 2, 2019
1 parent 675a1f4 commit 7aa0113
Show file tree
Hide file tree
Showing 6 changed files with 430 additions and 862 deletions.
915 changes: 290 additions & 625 deletions LICENSE
View file

Large diffs are not rendered by default.

151 changes: 77 additions & 74 deletions src/ceres_icp.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,37 +24,53 @@ struct ceres_icp_point2point
Eigen::Matrix<_T, 3, 1> m_current_pt;
Eigen::Matrix<_T, 3, 1> m_closest_pt;
_T m_motion_blur_s;
ceres_icp_point2point( const Eigen::Matrix<_T, 3, 1> &current_pt,
const Eigen::Matrix<_T, 3, 1> &closest_pt,
const _T &motion_blur_s = 1.0 ) : m_current_pt( current_pt ), m_closest_pt( closest_pt ), m_motion_blur_s( motion_blur_s ){};
Eigen::Matrix<_T, 4, 1> m_q_last;
Eigen::Matrix<_T, 3, 1> m_t_last;
_T m_weigh;
ceres_icp_point2point( const Eigen::Matrix<_T, 3, 1> current_pt,
const Eigen::Matrix<_T, 3, 1> closest_pt,
const _T &motion_blur_s = 1.0,
Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) ) : m_current_pt( current_pt ),
m_closest_pt( closest_pt ),
m_motion_blur_s( motion_blur_s ),
m_q_last( q_s ),
m_t_last( t_s )

{
m_weigh = 1.0;
};

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

Eigen::Quaternion<T> q_start{ T( 1.0 ), T( 0 ), T( 0 ), T( 0 ) };
Eigen::Matrix<T, 3, 1> t_start{ T( 0 ), T( 0 ), T( 0 ) };
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 ) };
Eigen::Matrix<T, 3, 1> t_last = m_t_last.template cast<T>();

Eigen::Quaternion<T> q_end{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
Eigen::Matrix<T, 3, 1> t_end{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };
Eigen::Quaternion<T> q_incre{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
Eigen::Matrix<T, 3, 1> t_incre{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };

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

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

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

residual[ 0 ] = pt_tranfromed( 0 ) - T( m_closest_pt( 0 ) );
residual[ 1 ] = pt_tranfromed( 1 ) - T( m_closest_pt( 1 ) );
residual[ 2 ] = pt_tranfromed( 2 ) - T( m_closest_pt( 2 ) );
residual[ 0 ] = ( pt_transfromed( 0 ) - T( m_closest_pt( 0 ) ) ) * T( m_weigh );
residual[ 1 ] = ( pt_transfromed( 1 ) - T( m_closest_pt( 1 ) ) ) * T( m_weigh );
residual[ 2 ] = ( pt_transfromed( 2 ) - T( m_closest_pt( 2 ) ) ) * T( m_weigh );
return true;
};

static ceres::CostFunction *Create( const Eigen::Matrix<_T, 3, 1> current_pt,
const Eigen::Matrix<_T, 3, 1> closest_pt,
const _T motion_blur_s = 1.0 )
const _T motion_blur_s = 1.0,
Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) )
{
return ( new ceres::AutoDiffCostFunction<
ceres_icp_point2point, 3, 4, 3>(
Expand All @@ -70,9 +86,9 @@ struct ceres_icp_point2line
Eigen::Matrix<_T, 3, 1> m_target_line_a, m_target_line_b;
Eigen::Matrix<_T, 3, 1> m_unit_vec_ab;
_T m_motion_blur_s;
Eigen::Matrix<_T, 4, 1> m_q_start;
Eigen::Matrix<_T, 3, 1> m_t_start;
_T m_weigth;
Eigen::Matrix<_T, 4, 1> m_q_last;
Eigen::Matrix<_T, 3, 1> m_t_last;
_T m_weigh;
ceres_icp_point2line( const Eigen::Matrix<_T, 3, 1> &current_pt,
const Eigen::Matrix<_T, 3, 1> &target_line_a,
const Eigen::Matrix<_T, 3, 1> &target_line_b,
Expand All @@ -81,63 +97,57 @@ struct ceres_icp_point2line
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 ),
m_target_line_b( target_line_b ),
m_motion_blur_s( motion_blur_s ),
m_q_start( q_s ),
m_t_start( t_s )
m_q_last( q_s ),
m_t_last( t_s )
{
m_unit_vec_ab = target_line_b - target_line_a;
m_unit_vec_ab = m_unit_vec_ab / m_unit_vec_ab.norm();
// m_weigth = 1/m_current_pt.norm();
m_weigth = 1.0;
// m_weigh = 1/m_current_pt.norm();
m_weigh = 1.0;
//cout << m_unit_vec_ab.transpose() <<endl;
};

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

Eigen::Quaternion<T> q_I( ( T ) 1.0, ( T ) 0.0, ( T ) 0.0, ( T ) 0.0 );

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 ) };
Eigen::Matrix<T, 3, 1> t_start{ ( T ) m_t_start( 0 ), ( T ) m_t_start( 1 ), ( T ) m_t_start( 2 ) };
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 ) };
Eigen::Matrix<T, 3, 1> t_last = m_t_last.template cast<T>();

Eigen::Quaternion<T> q_end{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
Eigen::Matrix<T, 3, 1> t_end{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };
Eigen::Quaternion<T> q_incre{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
Eigen::Matrix<T, 3, 1> t_incre{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };

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

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

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 ) ) };
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 ) ) };
Eigen::Matrix<T, 3, 1> tar_line_pt_a = m_target_line_a.template cast<T>();
Eigen::Matrix<T, 3, 1> vec_line_ab_unit = m_unit_vec_ab.template cast<T>();

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

residual[ 0 ] = residual_vec( 0 ) * T( m_weigth );
residual[ 1 ] = residual_vec( 1 ) * T( m_weigth );
residual[ 2 ] = residual_vec( 2 ) * T( m_weigth );

//cout << " *** " << residual[0] << " " << residual[1] << " " << residual[2] << " *** " << endl;
residual[ 0 ] = residual_vec( 0 ) * T( m_weigh );
residual[ 1 ] = residual_vec( 1 ) * T( m_weigh );
residual[ 2 ] = residual_vec( 2 ) * T( m_weigh );

//cout << residual[0]<< " " << residual[1] << " " <<endl;
return true;
};

static ceres::CostFunction *Create( const Eigen::Matrix<_T, 3, 1> &current_pt,
const Eigen::Matrix<_T, 3, 1> &target_line_a,
const Eigen::Matrix<_T, 3, 1> &target_line_b,
const _T motion_blur_s = 1.0,
Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) )
Eigen::Matrix<_T, 4, 1> q_last = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
Eigen::Matrix<_T, 3, 1> t_last = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) )
{
// TODO: can be vector or distance
return ( new ceres::AutoDiffCostFunction<
ceres_icp_point2line, 3, 4, 3>(
new ceres_icp_point2line( current_pt, target_line_a, target_line_b, motion_blur_s ) ) );
new ceres_icp_point2line( current_pt, target_line_a, target_line_b, motion_blur_s, q_last, t_last ) ) );
}
};

Expand All @@ -149,9 +159,9 @@ struct ceres_icp_point2plane
Eigen::Matrix<_T, 3, 1> m_target_line_a, m_target_line_b, m_target_line_c;
Eigen::Matrix<_T, 3, 1> m_unit_vec_ab, m_unit_vec_ac, m_unit_vec_n;
_T m_motion_blur_s;
_T m_weigth;
Eigen::Matrix<_T, 4, 1> m_q_start;
Eigen::Matrix<_T, 3, 1> m_t_start;
_T m_weigh;
Eigen::Matrix<_T, 4, 1> m_q_last;
Eigen::Matrix<_T, 3, 1> m_t_last;
ceres_icp_point2plane( const Eigen::Matrix<_T, 3, 1> &current_pt,
const Eigen::Matrix<_T, 3, 1> &target_line_a,
const Eigen::Matrix<_T, 3, 1> &target_line_b,
Expand All @@ -162,8 +172,8 @@ struct ceres_icp_point2plane
m_target_line_b( target_line_b ),
m_target_line_c( target_line_c ),
m_motion_blur_s( motion_blur_s ),
m_q_start( q_s ),
m_t_start( t_s )
m_q_last( q_s ),
m_t_last( t_s )

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

m_unit_vec_n = m_unit_vec_ab.cross( m_unit_vec_ac );
//m_unit_vec_n = m_unit_vec_n / m_unit_vec_n.norm();
// m_weigth = 1/m_current_pt.norm();
m_weigth = 1.0;
//m_weigth = motion_blur_s;
//cout << " --- " << m_unit_vec_n.transpose() << endl;
m_weigh = 1.0;
};

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

Eigen::Quaternion<T> q_I( ( T ) 1.0, ( T ) 0.0, ( T ) 0.0, ( T ) 0.0 );
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 ) };
Eigen::Matrix<T, 3, 1> t_last = m_t_last.template cast<T>();

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 ) };
Eigen::Matrix<T, 3, 1> t_start{ ( T ) m_t_start( 0 ), ( T ) m_t_start( 1 ), ( T ) m_t_start( 2 ) };
Eigen::Quaternion<T> q_incre{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
Eigen::Matrix<T, 3, 1> t_incre{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };

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

Eigen::Quaternion<T> q_interpolate = q_I.slerp( ( T ) m_motion_blur_s, q_end );
// Eigen::Matrix< T, 3, 1 > t_interpolate = t_start * T ( 1.0 - m_motion_blur_s ) + t_end * T ( m_motion_blur_s );
Eigen::Matrix<T, 3, 1> t_interpolate = t_end * T( m_motion_blur_s );

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

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 ) ) };
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 ) ) };
Eigen::Matrix<T, 3, 1> tar_line_pt_a = m_target_line_a.template cast<T>();
Eigen::Matrix<T, 3, 1> vec_line_plane_norm = m_unit_vec_n.template cast<T>();

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

residual[ 0 ] = residual_vec( 0 ) * T( m_weigth );
residual[ 1 ] = residual_vec( 1 ) * T( m_weigth );
residual[ 2 ] = residual_vec( 2 ) * T( m_weigth );
residual[ 0 ] = residual_vec( 0 ) * T( m_weigh );
residual[ 1 ] = residual_vec( 1 ) * T( m_weigh );
residual[ 2 ] = residual_vec( 2 ) * T( m_weigh );
//cout << residual_vec.rows() << " " <<residual_vec.cols() <<endl;

//cout << " *** " << residual_vec[0] << " " << residual_vec[1] << " " << residual_vec[2] ;
Expand All @@ -223,13 +226,13 @@ struct ceres_icp_point2plane
const Eigen::Matrix<_T, 3, 1> &target_line_b,
const Eigen::Matrix<_T, 3, 1> &target_line_c,
const _T motion_blur_s = 1.0,
Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) )
Eigen::Matrix<_T, 4, 1> q_last = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
Eigen::Matrix<_T, 3, 1> t_last = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) )
{
// TODO: can be vector or distance
return ( new ceres::AutoDiffCostFunction<
ceres_icp_point2plane, 3, 4, 3>(
new ceres_icp_point2plane( current_pt, target_line_a, target_line_b, target_line_c, motion_blur_s, q_s, t_s ) ) );
new ceres_icp_point2plane( current_pt, target_line_a, target_line_b, target_line_c, motion_blur_s, q_last, t_last ) ) );
}
};

Expand Down
20 changes: 10 additions & 10 deletions src/eigen_math.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,40 +10,40 @@
namespace Eigen_math
{
template <typename T>
static T vector_project_on_vector(const T &vec_a, const T &vec_b)
static T vector_project_on_vector( const T &vec_a, const T &vec_b )
{
return vec_a.dot(vec_b) * vec_b / (vec_b.norm() * vec_b.norm());
return vec_a.dot( vec_b ) * vec_b / ( vec_b.norm() * vec_b.norm() );
}

template <typename T>
static T vector_project_on_unit_vector(const T &vec_a, const T &vec_b)
static T vector_project_on_unit_vector( const T &vec_a, const T &vec_b )
{
return vec_a.dot(vec_b) * vec_b;
return vec_a.dot( vec_b ) * vec_b;
}

template <typename T>
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)
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 )
{
T vec_a_norm = vec_a.norm();
T vec_b_norm = vec_b.norm();
if (vec_a_norm == 0 || vec_b_norm == 0) // zero vector is pararrel to any vector.
if ( vec_a_norm == 0 || vec_b_norm == 0 ) // zero vector is pararrel to any vector.
{
return 0.0;
}
else
{
if (if_force_sharp_angle)
if ( if_force_sharp_angle )
{
// return acos( abs( vec_a.dot( vec_b ) )*0.9999 / ( vec_a_norm * vec_b_norm ) );
return acos(abs(vec_a.dot(vec_b)) / (vec_a_norm * vec_b_norm));
return acos( abs( vec_a.dot( vec_b ) ) / ( vec_a_norm * vec_b_norm ) );
}
else
{
// return acos( (vec_a.dot(vec_b))*0.9999 / (vec_a_norm*vec_b_norm));
return acos((vec_a.dot(vec_b)) / (vec_a_norm * vec_b_norm));
return acos( ( vec_a.dot( vec_b ) ) / ( vec_a_norm * vec_b_norm ) );
}
}
}
} // namespace Eigen_math

#endif
#endif
Loading

0 comments on commit 7aa0113

Please sign in to comment.