Rotation and Translation are identity and 0 always about libicp HOT 1 CLOSED

Hi ashnarayan13, it's a bug at the code below in icpPointToPlane.cpp, where i changed the raw search method from k-nearest to r-nearest and used a fixed r threshold(0.05), thus when you input points with distance over 0.05, the point normal will not be calculated correctly, since it has no neighbor in radius 0.05.

double* IcpPointToPlane::computeNormals (const int32_t num_neighbors,const double flatness) {
    double *M_normal = (double*)malloc(m_kd_tree->N*m_dim*sizeof(double));
    kdtree::KDTreeResultVector neighbors;
    for (int32_t i=0; i<m_kd_tree->N; i++) {
      //M_tree->n_nearest_around_point(i,0,num_neighbors,neighbors);
      m_kd_tree->r_nearest_around_point(i,0,0.05,neighbors);
      if (m_dim==2) computeNormal(neighbors,M_normal+i*2,flatness);
      else        computeNormal(neighbors,M_normal+i*3,flatness);
    }
    return M_normal;
 }

Just comment 'm_kd_tree->r_nearest_around_point(i,0,0.05,neighbors);' and use n_nearest_around_point(). Or you can checkout the new commit which fixed this bug.

By the way, i found another mistake at fitIterate() in icp.cpp, check this commit for details.

from libicp.