I am trying to implement point to point ICP using a Turtlebot3 as part of a state estimation stack. Here are the sensor specs: http://www.robotis.us/360-laser-distance-sensor-lds-01-lidar/

Currently the transformations returned by my algorithm are short of what they should be - e.g. a 30cm movement in the x direction is detected as a translation of around 7cm. The same goes for rotations. I am struggling to find the issue(s) with my code; I am also wondering whether this LIDAR is suitable for such a purpose.

My initial guess for the transformation is provided by the state output by a kalman filter that is currently only running the prediction step. I have carefully verified that these estimates are very close to the true transformation in my short tests.

When I transform the LIDAR scan into [x,y] coordinates in the sensor reference frame, if the range of the point is less than the minimum range, I set it to the minimum range and likewise for the maximum range. I don't think this is the issue, but putting it here in case.

Here is my pseudo-code for ICP:

ICP(src, dst, guess):

    src = transform_points(src, guess)
    transform = guess

    for i in range MAX_ITERATIONS:
        // Returns the indicies of the coordinates in dst that are closest to each point in src  
        // and the distances between pairs
        dists, idxs = find_nearest_neighbour(src, dst)

        // Remove outliers
        src = src[dists <= 0.02]
        idxs = idxs[dists <= 0.02]
        dst = dst[idxs]

        tr = compute_transform(src, dst)
        src = transform_points(src, tr)

        // Update transform with dot product
        transform = tr * transform

    return transform

I have tried playing with the value of MAX_ITERATIONS, introducing a change in error termination condition and absolute error condition.

This way of removing outliers is quite crude, but without it, the robot's pose as computed by applying the transformation returned by ICP will diverge even if the robot is not moving. The transform is computed with around 250 out of the original 360 points.

Compute transform uses the closed form solution utilising centroids and SVD of cross-covariance matrix. I noticed that the first time this is called in the ICP loop (i.e. the iteration straight after the guess transform was applied), the transform returned "undoes" most of what the guess did to the src coordinates. This is the python code for the function in case this is the issue

def compute_transform(self, src, dst):
        src_centroid = np.mean(src, axis=0)
        dst_centroid = np.mean(dst, axis=0)
        s = src - src_centroid
        d = dst - dst_centroid
        ccovar = (d.T @ s) / dst.shape[0]
        u, _, v = np.linalg.svd(ccovar)
        # Ensure we get a proper rotation without a reflection:
        v[-1, :] *= np.linalg.det(u) * np.linalg.det(v)
        r = u @ v.T
        t = dst_centroid - r.T @ src_centroid
        return np.c_[r, t]

I am hoping someone might be able to spot any mistakes / suggest where to look / tell me that what I'm trying to do isn't possible as I've spent a good few weeks trying to debug my code to no avail. Thank you very much for your time.

  • $\begingroup$ Not an answer, but I don't think you're going to get anywhere with ICP alone with lidar because the laser beams a lidar unit generates are at particular angles. The linear distance changes with radius and you're going to have (are having) a lot of trouble when you try to match it up as the lidar moves. You could probably do okay with feature extraction and feature matching, but frame-to-frame ICP on scan points alone is going to be a struggle because they're rarely going to overlap... again because of the angular nature of the scans. $\endgroup$
    – Chuck
    Sep 26, 2020 at 23:30
  • $\begingroup$ Thanks for your input @Chuck. So the problem is that LIDAR points in consecutive scans are not going to be hitting the exact same point on the same object? - I didn't quite get what you meant by "the linear distance changes with radius". I'll have a look at feature extraction and matching, thanks for the suggestion. $\endgroup$ Sep 28, 2020 at 14:16
  • 1
    $\begingroup$ I mean sequential beams in a scan plane have a particular angular increment - usually it's something like 1 degree, or 1/6th of a degree, etc. between beams. The angular resolution between beams is constant, but as your distance to a wall or other measurable object changes the linear distance between points. The ICP algorithm is looking for linear distance between points but you're effectively changing the inter-point spacing or linear resolution by moving, so ICP is going to struggle then to match points that aren't sampled on an even interval between frames. Hope that makes sense :) $\endgroup$
    – Chuck
    Sep 28, 2020 at 14:49
  • $\begingroup$ @Chuck yep that makes perfect sense, thank you very much! $\endgroup$ Sep 28, 2020 at 15:01


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Browse other questions tagged or ask your own question.