2
$\begingroup$

I am currently in the process of implementing Graph SLAM using ICP and g2o in python 2.7

The data I have are pose data (4 x 4 transformation matrix) and lidar data ( in the format of [x y z 1] with dimension of 4 x n)

As long as I am aware, for icp to work, you need to apply initial guessing (pose difference between the source and the destination node) before executing the ICP algorithm, which I am using later for adding more edges.

What I have done so far is to initially add the nodes and the respective edges of each node that is larger than a certain threshold, and I'm currently struggling through to apply ICP to fine tune my graph SLAM map.

Currently, I have been able to derive the transformation matrix of the source node and the destination node (the two nodes that I think are matching pairs for closing the loop), and trying to process the point clouds for each sourceLidar and DestinationLidar which both have dimensions of 4 x n (4 = [x y z 1).

I have set the sourcePoint cloud as source Lidar, but I need to transform the DestinationPointCloud with initial guessing.

What I have right now is: DestinationLidar values (4 x number of nodes) and transformation matrix of each source and destination node that I have derived from g2o and optimizer function.

I know I am meant to apply rotation and translation to these, but I am not sure how to execute these....

After this is executed, I will apply the icp and add any additional edges that lie below a set threshold, then regraph SLAM.

Please help me on how I can apply the transformation (both Rotation and Translation) to DestinationLidar to apply initial guessing.

Thanks alot

Improve this question
$\endgroup$
6
  • $\begingroup$ I'm confused on what exactly you are doing. Normally you use ICP to estimate the 4x4 transformation matrix between the 2 pointclouds. This transform is then used to create a Pose Graph which you solve via g2o. In the pose graph stage you should not have any points. Only pose nodes, and transformation edges between them. So the ICP and g2o pose graph are 2 separate steps. Therefore ICP can't refine the graph slam map. It is actually the other way around. So I need some more information on what exactly you are doing. The way to transform a point with an initial guess is $p_d=T_i*p_s$. $\endgroup$ – edwinem Apr 18 '20 at 16:05
  • $\begingroup$ @edwinem Sorry or the confusing question. So basically, the procedure I have constructed so far is: 1. Add vertexes (nodes) if it exceeds a certain threshold in yaw or euclidean distance and edges between all of the nodes. 2. Find the set of "matching pairs" of source and destination nodes for loop closing (it could be optimized in one go but I am trying to separate these steps) 3. This is the problematic part. So far, I have only applied ICP to raw source and destination node lidar data (point cloud), and construct more edges if the mean distance of the neighbors is less than 0.05m. $\endgroup$ – David Lee Apr 19 '20 at 1:10
  • $\begingroup$ @edwinem But it seems like the optimized plot shows a blob of cloud points... and I realized I haven't applied initial guessing to the lidar points. I have been given source and destination transformation matrix obtained from optimized poses (optimizer.getpose). But i'm not sure how to apply initial guess afterwards ( in this case, I've kept the source lidar data the same so I only need to transform the destination lidar data) $\endgroup$ – David Lee Apr 19 '20 at 1:14
  • $\begingroup$ Ok. Correct my understanding if I am wrong. But essentially you have a pose graph on which you want to do loop closure(source and destination nodes form the loop). So you have these 2 poses and their pointclouds and want to use ICP to estimate the transform between them. You also want to initialize your ICP algorithm with an initial guess as you know the locations of the source and destination poses. $\endgroup$ – edwinem Apr 19 '20 at 2:05
  • $\begingroup$ @edwinem yes that is Correct. In the end, I basically want to perform optimization on Graph slam (the back end side) by utilizing ICP with initial guesses. $\endgroup$ – David Lee Apr 19 '20 at 2:54

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.