C# Question

Transforming a point cloud from one space to another

I have a robot arm and a Microsoft Kinect 2.0. The Kinect can output 3D point clouds, which is offset by the Kinects origo and rotation. The robot arm can move its outermost wrist to XYZ positions (and rotations) given in mm.

Imagine that the Kinect is mounted on the arm.
The robot moves to position X:0, Y: 570, Z: -950, and rotates the wrist so that the Kinect points directly downwards (rotated -90 degrees around the Z-axis).

If I get a ColorMesh from the Kinect, how would I rotate and offset the mesh so that it is in robot space instead of 3D camera space? I don't know much about this so any help is appreciated.

Here's an illustration of the real-life setup in mind:
The sphere denotes the workspace reach of the arm.
enter image description here

I was figuring the class to look something like this:

public class CameraToRobotCalibrator
{
//offset in mm compared to robot origo
public const float x_offset = 0f;
public const float y_offset = 570f;
public const float z_offset = -950f;

//camera rotated around its own origo
public const float x_rotation = 0f;
public const float y_rotation = 0f;
public const float z_rotation = -90f;

public ColorMesh ConvertToRobospace(ColorMesh mesh)
{
//?
}
}


I'm guessing if it was for 1 point it wouldn't be too hard, but I don't know about an entire mesh with vertices and faces?

Answer

Given the specifications of the robot and its degree of freedom, you can usually calculate a 4x4 matrix that describes the endeffector's position and orientation. This is specific to the robot's design. Let's call this matrix E.

Then, you need to mount the Kinect on the endeffector. This will introduce another translation and possibly a rotation. So you need another matrix M that represents how the Kinect is mounted on the endeffector. It is the transform from the endeffector's coordinate system to the Kinect's coordinate system. Ideally, this matrix would be calibrated somehow. A good initial guess might be derived by measuring.

Then, the overall transform from the robot's root coordinate system to the Kinect coordinate system is T = R * M (assuming column-major matrices).

And this is the matrix that you need to transform your mesh. Transforming a mesh is just like transforming a single point. But now, you transform all vertex positions (add 1 as w-component) and all vertex normals (add 0 as w-component). Keep the face indices as they are.

Comments