Livox-SDK/livox_cloud_undistortion

Points are only pushed in result cloud if dt to last point is zero

tiesus opened this issue · 0 comments

tiesus commented
if (dt_bi == 0) laserCloudtmp->push_back(pt);
double ratio_bi = dt_bi / dt_be;
/// Rotation from i-e
double ratio_ie = 1 - ratio_bi;


Eigen::Vector3d rso3_ie = ratio_ie * rso3_be;
SO3d Rie = SO3d::exp(rso3_ie);


/// Transform to the 'end' frame, using only the rotation
/// Note: Compensation direction is INVERSE of Frame's moving direction
/// So if we want to compensate a point at timestamp-i to the frame-e
/// P_compensate = R_ei * Pi + t_ei
Eigen::Vector3d tie = ratio_ie * tbe;
// Eigen::Vector3d tei = Eigen::Vector3d::Zero();
Eigen::Vector3d v_pt_i(pt.x, pt.y, pt.z);
Eigen::Vector3d v_pt_comp_e = Rie.inverse() * (v_pt_i - tie);


/// Undistorted point
pt.x = v_pt_comp_e.x();
pt.y = v_pt_comp_e.y();
pt.z = v_pt_comp_e.z();

}
}

laserCloudTemp is only filled if dt is 0. Distorted points are never pushed back.