PointCloud-mesh registration tutorial

[lucarossini-iit]

Hello guys, I am a new user of this package and I was planning to use the registration feature for a camera calibration. Specifically, my idea is to make our humanoid robot watch its hand, acquire the PointCloud of the hand using the embedded camera on its head, and then register the PointCloud on the mesh using the computed transformation to calibrate the camera. Here's a picture: To do so, I follow these steps:

• load the mesh from file using mesh = trimesh.load('urdf_path')
• apply a transform to the mesh from the robot's forward kinematics using mesh.apply_transform(robot.fk('left_hand')
• register the PointCloud onto the mesh using the icp method: T, _, _ = trimesh.registration.icp(point_cloud, mesh)

Unfortunately, this is giving meaningless results with translations of the order of meters (while I am expecting few millimeters as you see from the picture above). I am aware that a normalization of the transformation matrix is required to match the mesh scale, but I was wondering if there is any PointCloud-mesh registration tutorial.

Thanks in advance

[Kiord]

Your approach sounds good. ICP gives better results when the two geometries are already roughly aligned. Did you try to align their scale and center of mass ?

[lucarossini-iit]

Hello Kiord,

Thank you for you reply. I just applied a rigid transformation to the mesh using the forward kinematics as argument of the mesh.apply_transform() method. Regarding the scale, I know for sure that the mesh is scaled by a factor of 1000 since the CAD of the ball-shape end-effector is in millimiters, but I understood that the ICP is able to return the scaled transformed, is it correct?

Thank you, Luca

[Kiord]

Can you visualize the mesh and point cloud before and after ICP ?

import pyvista as pv # I prefer pyvista for visualization but it shoud be doable with trimesh only
import trimesh as tm

point_cloud = ??? # shoud be an numpy array of shape (N, 3)
mesh = trimesh.load('urdf_path')
mesh.apply_transform(robot.fk('left_hand')

plotter = pv.Plotter()
plotter.add_mesh(pv.wrap(mesh), color='white', opacity=0.5)
plotter.add_mesh(pv.wrap(point_cloud), color='red', point_size=2)
_, registered_point_cloud, _ = tm.registration.icp(point_cloud, mesh)
plotter.add_mesh(pv.wrap(registered_point_cloud), color='green', point_size=2)
plotter.show()

[lucarossini-iit]

Hi Kiord, Thank you again for your help. I followed your procedure and this is what I see in pyvista: One of the problem is the size of the mesh which must be scaled of a factor 0.001. Is this possible to do when I load the mesh or should I generate a cad with smaller size?

Thank you, Luca

[Kiord]

The difference in scale is too much to handle for the ICP.

To scale a mesh, just scale the vertices by some factor mesh.vertices *= 0.001, but it assumes that the 'real' center of the mesh is the world origin.

why not scale the point cloud instead ?

# Scale point cloud
point_cloud = point_cloud * 1000
point_cloud_centroid = point_cloud.mean(axis=0)

mesh_centroid = mesh.vertices.mean(axis=0)

# Align the point cloud centroid to the mesh centroid
point_cloud = point_cloud - point_cloud_centroid [None, :] + mesh_centroid[None, :]

[do ICP]

[lucarossini-iit]

Indeed expanding the PointCloud was the best solution! Thank you very much for your help.

One last question: is there any way to mesh two meshes in a single mesh inside trimesh?

[Kiord]

I'm glad it was useful. To concatenate some meshes there is the function trimesh.util.concatenate (doc here).

[Hytac]

Hi, when im trying to load a urdf using mesh = trimesh.load('E:\\file.urdf')

It gives me the following error: ValueError: File type: urdf not supported. How did you make it happen?

[lucarossini-iit]

Hello Hytac, my previous comment was misleading. Actually, a .urdf is not a mesh but can associate a link to a mesh loading, usually, a .stl file, which is a mesh. You can create a .stl using simple CAD tools.

Create your mesh using the directory to the .stl file instead: trimesh.load('E:\\file.stl')

Let me know if this was helpful.