给定vNormal和地面点，将3d点云旋转到地面的偏移角度

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

Answer 1

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 TB 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.

TBN calculation.