How is ICP done? about voxblox HOT 8 CLOSED

The ICP is very new and unfortunately not documented anywhere. It is different from any other ICP implementation I am aware of, I intend to write a quick doc on it sometime in the next two months.

Some quick points on how it works:

• There is no scan-to-scan matching, every scan is immediately matched to the full TSDF map.
• There is no point-matching, distances and gradients are simply taken from the TSDF values
• A rough approximation of the Hessian is used to give matches a weighing.
• Rather than iterating all points and computing a single overall transform, hundreds of transforms are computed on small batches of (by default 20) points.
• These are then fused together taking into account their weighting.
• There is no actual iteration of point matches, each small batch just uses the current transform given by the previous batches and performs one step.

For more details see the first PR (#198) that implemented a more standard ICP approach and the next PR with the current approach (#205).

The basement_dataset.launch provides a sample of how the approach works to correct for drifting input odometry in the basement dataset https://projects.asl.ethz.ch/datasets/doku.php?id=basement2018

from voxblox.

Thanks for the information.

from voxblox.

One more small question that I forgot, if you could answer, regarding on how to use the ICP that was implemented.

There is the "icp_corrected_frame" option and the "pose_corrected_frame" option, which are a bit unclear what each thing does.

In my current set up, I just have the "world" frame and the "camera" frame. My best guess is to set icp_corrected_frame to "camera" and pose_corrected_frame to "world". Am I understanding it correctly? And if that is the case, why not get them directly from "world_frame" and "sensor_frame"?

from voxblox.

These two frames both provide information about your odometry and how ICP is changing it. In the vast majority of applications you don't need to rename them.

If we have the following frames:

• ground_truth_world: a static frame everything is relative to
• sensor_frame: the true drift free pose of the sensor relative to the ground_truth_world frame.
• world_frame: the drifting estimate of the pose the system believes ground_truth_world to have.

Then assuming the ICP is perfectly accurate and corrects all drift:

ground_truth_world * icp_corrected_frame = world_frame
ground_truth_world * pose_corrected_frame = sensor_frame

from voxblox.

I now do get better the idea of icp_corrected_frame and pose_corrected_frame however I feel more confused overall. Let me break down my confusion to different questions.

1. The thing I think I have misunderstood the most. This ICP is not meant to do localization on its own but to help correct the existing localization algorithm/estimation?

2. From my understanding, ground_truth is more related to validation afterwords and not for during the actual execution. When running a SLAM algorithm, one will only have the word frame and sensor frame (in the simple case). So is icp_corrected_frame just meant to be used for validation?

3. With the current ROS implementation, is the tf map automatically updated? Do the frames given at icp_corrected_frame and pose_corrected_frame get updated automatically?

4. There is also the icp_transform publisher. This is just a different way to get the ICP correction or does it serve a different role?

from voxblox.

1. It was designed for correcting an initial odometry estimate. However, in environments that provide enough information from the structure alone (i.e. not flat fields or long corridors) if your movement between frames is less than the truncation distance it can be made to work without it. If you publish identity for the world to sensor transform the pure ICP output will then be given by the icp_corrected_frame.

2. The ground truth frame is unobserved. The intention of the ICP step is to estimate its pose and how far the estimated world frame of the input odometry has drifted from this position.

3. Whenever an ICP alignment is performed a TF connecting world to icp_corrected_frame and icp_corrected_frame to pose_corrected_frame are published.

4. The icp_transform publishes the transform connecting the world and icp_corrected_frame. This information is already given by the TF and is just provided so that nodes that only accept TransformStamped messages can make use of it.

If you are curious about how this is implemented or want to modify it the logic for generating and publishing these frames is found here https://github.com/ethz-asl/voxblox/blob/master/voxblox_ros/src/tsdf_server.cc#L271

from voxblox.

OK now I understand. Thank you a lot for the information!

from voxblox.

Sorry to bring back an old thread.

I was just going over the ICP code and noticed something I don't understand in icp.h

    SquareMatrix<3> rotation_matrix = SquareMatrix<3>::Identity();

    SquareMatrix<dim> H =
        src_demean.topRows<dim>() * tgt_demean.topRows<dim>().transpose();

    // Compute the Singular Value Decomposition
    Eigen::JacobiSVD<SquareMatrix<dim>> svd(
        H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    SquareMatrix<dim> u = svd.matrixU();
    SquareMatrix<dim> v = svd.matrixV();

To me this implies that only the first three correlations of the mini-batch are used to compute the rotation.
Is this correct or am I misunderstanding?

Thanks!

from voxblox.