rotate 3d point cloud to offset angle of floor plane given vNormal and floor point
-
26-12-2020 - |
Question
I'm working with kinect and ofxopeni. I have a point cloud in real world coordinates but I need rotate those points to offset the tilt of the camera. The floor plane should give me all the information I need but I can't work out how to calculate the axis and angle of rotation.
my initial idea was...
ofVec3f target_normal(0,1,0);
ofVec3f vNormal; //set this from Xn3DPlane floorPlane (not shown here)
ofVec3f ptPoint; //as above
float rot_angle = target_normal.angle(vNormal);
for(int i = 0; i < numPoints; i++){
cloudPoints[i].rotate(rot_angle, vNormal, ptPoint); //align my points to normal is (0 1 0)
}
This it seems was too simplistic by far. I've been fishing through various articles and can see that it most probably involves a quarterion or rotation matrix but I can't work out where to start. I'd be really grateful for any pointers to relevant articles or what is the best technique to get an axis and angle of rotation ? I'm imagining it can be done quite easily using ofQuarterion or an openni function but I can't work out how to implement.
best
Simon
Solution
I've never used ofxopeni, but this is the best mathematical explanation I can give.
You can rotate any set of data from one axis set to another using a TBN matrix,(tangent, bitangent, normal), where T B and N are your new set of axis. So, you already have the data for the normal, but you need to find a tangent. I'm not sure if your Xn3DPlane provides a tangent, but if it does, use that.
The bitangent is given by the cross-product of the normal and the tangent:
B = T x N
A TBN looks like this:
TBN = { Tx ,Ty ,Tz,
Bx, By, Bz,
Nx, Ny, Nz }
This will rotate your data on a new set of axis, but your plane also has an origin point, so we through in a translation:
A = {1 , 0 , 0, 0, { Tx , Ty , Tz , 0,
0, 1, 0, 0, Bx , By , Bz , 0,
0, 0, 1, 0, x Nx , Ny , Nz , 0,
-Px,-Py,-Pz,1} 0 , 0 , 0 , 1}
// The multiply the vertex, v, to change it's coordinate system.
v = v * A
If you can get things to this stage, you can transform all your points to the new coordinate system. One thing to note, the normal is now aligned with the z axis, if you want it to be aligned with the y, swap N and B in the TBN matrix.
EDIT: Final matrix calculation was slightly wrong. Fixed.