I am trying to localize an object in a point cloud using ROS, PCL. For that I capture the scene and model using Asus xtion pro sensor. I use RGBDSLAMv2 for capturing the model.
Then I use ICP (nonlinear version) to find the transform from the model to each cluster of the cloud. The cluster with the lowest score is chosen as the best matching cluster.
Pseudocode:
Segment the point cloud into different clusters. ([Using euclidean clustering][3])
for each cluster i
Source: 3dmodel. Target: current cluster
Perform [ICP (nonlinear version)][2].
score[i] = icp.getFitnessScore()
T[i] = icp.getFinalTransformation()
end for
matchingCluster = cluster with minimum score
finalT = T[matchingCluster]
However, I am not able to find the correct transformation.
Here are the screenshots of the results I got:
The red colored object is the transformed model overlayed onto the scene. The yellow object represents the original model in the coordinate system of the scene.
Now, my concern is why there is no proper transformation? Am I missing something?
Second, I see that the object model and scene are in different coordinate system. So the model appears inverted when presented in the scene's coordinate system. Is there a way in which I can transform the model upright before running ICP?
Thanks :)