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.