CODEWITHSUNDEEP
c++opencvgeometrykinectcoordinate-transformation
solvador
solvador

Reputation: 95

3D Coordinate System Transformation (X,Y,Z) to (X',Y',Z')

I'm working with the new Kinect v2 for reference, and I have a working coordinate system in place for a given frame, with coordinates (x,y,z) in mm. What I am trying to do is line up, transform, or relate the coordinate systems of the Kinect camera and that of an object it is looking at.

This object has it's own coordinate frame, and moves only across its x, y, and z axes. The kinect tracks the object, returning the world x,y,z coordinates with the kinect at the origin. However, I can also specify a new origin within the same coordinate frame, just by taking into account the x,y, and z offsets.

I was thinking that if I have the object starting in a position with the same origin, I could figure out how to translate its x', y', and z' movements using the kinect-given coordinates.

You can see what I'm talking about here with this (bad) drawing.

Bad Drawing

Is there a way I can set up a coordinate frame, given a new set of x', y' and z' values? Let's say I have 3 sets of coordinates in BOTH the object's frame AND the kinect's frame.

So, how can I translate (x,y,z) to the (x',y',z') frame if I KNOW the initial values of 3 pairs of (x,y,z) and (x',y',z').

Upvotes: 2

Views: 5525

Answers (1)

solvador
solvador

Reputation: 95

I actually solved my own problem here using a simple change of basis method. Since the two coordinate frames are both orthonormal, and have the same origin, it was as simple as constructing a change-of-basis matrix, and using that to go from one coordinate system to the other.

Here is my code, in case anyone is looking how to do this with c++ / opencv. I use cv::Mat to do matrix manipulation.

// Object coordinate frame with orthonormal basis vectors u,v,w.
// Each basis vector has components x,y,z.

float U[3] = { ux, uy, uz };
float V[3] = { vx, vy, vz };
float W[3] = { wx, wy, wz };

// Create lenghts to normalize the vectors.
float ulength = sqrt(ux*ux + uy*uy + uz*uz);
float vlength = sqrt(vx*vx + vy*vy + vz*vz);
float wlength = sqrt(wx*wx + wy*wy + wz*wz);

// Setting up the change of basis matrix.
float data[3][3] = { { ux / ulength, uy / ulength, uz / ulength },
{ vx / vlength, vy / vlength, vz / vlength },
{ wx / wlength, wy / wlength, wz / wlength } };

// Store array into cv::Mat
cv::Mat M = cv::Mat(3, 3, CV_32FC1, &data);

// Create vector Mat of coordinates in kinect frame.
float kinectcoords[3] = { x, y, z};
cv::Mat D = cv::Mat(3, 1, CV_32FC1, &kinectcoords);

// Find coordinates in object frame.

// If D is the coordinate vector in the kinect frame, P is the coordinate vector
// in the object frame, and M is the change of basis matrix, then the method is
// P = Minv * D. cv::Mat objectcoords is my 'P' vector.

cv::Mat Minv = M.inv();
cv::Mat objectcoords = Minv * D;

float objx = objectcoords.at<float>(0);
float objy = objectcoords.at<float>(1);
float objz = objectcoords.at<float>(2);

Upvotes: 2

Related Questions