saimanoj18 / 3d-icp-covariance Goto Github PK

pcl::copyPointCloud(cld1,data_idx,data_pi);

Should the cld1 here be replaced by the original source pointcloud: cloud_in? because cld1 here is the pointcloud after icp alignment using final_transformation.

The data_pi parameter in calculate_ICP_COV function should be the corresponding points in the "source pointcloud" right?

Hi,

from what I have observed is that indeed the covariance grows if the computed transformation gets worse. But I think the scale is off. I took a Velodyne VLP32 point cloud and shifted it 1.5m in x direction. I used the transformed point cloud as source and raw point cloud as target. The estimate was set to the identity matrix. Therefore the error in x is 1.5m and the remaining errrors are 0. This results in a x variance of 4.24938e-7 which is a standard deviation of just 0.6mm.

Do you have an idea to bring this more in line with the real expected covariance/standard deviation?

The last item d2J_dzdx should be d2J_dcdx.