3D Transformation Question

[Insomniacp]

So, I have a unique ish problem I am trying to solve efficiently (IE I need it to run on 300,000 points in real time (3 times per second minimum)). To do this I planned on using matrix transformations.
So this is the data I have, from GPS coordinates I calculate the position on earth surface in meters, being spherical I will have an arbitrary up axis (the normal of the position vector for now). The second set of data is from a kinect sensor which will give me a cloud of points (x,y,z) positions for each point from the 640x480 depth map. This data is also in meters but is relative to the camera.
Variables.
cameraGPSPosition: camX, camY, camZ (meters, double)
cameraGPSHeading: heading (degrees true, double)
cameraGPSNormal: normalX, normalY, normalZ (from the position vector, this does assume earth is a perfect sphere and thats okay right now, later on I will add a sensor to detect which direction is actually up).
pointCloud: points (x,y,z, each being a double, meters)

So what I want to do is take each point which is relative to the kinect camera and transform their position and orientation to the cameraGPSPosition on Earths surface with the up vector being the cameraGPSNormal and the points to be facing in the proper cameraGPSHeading direction. So I can easily just move the points to the cameraGPSPosition but I am stuck trying to reorient them once there to the proper heading and with the new up vector. Thanks for the time to read and answer.

[Insomniacp]

Due to complexity of making this calculation I have decided to instead use a different method. I now convert the latitude and longitude from gps to UTM coordinates. UTM is a flat mapping in meters which allows me to get the position on the map and then raise the height based on the gps data. This eliminates the need for complex transformations on a large number of points. I found existing code that accurately converts lat and long to UTM coords and I will be doing the rest from there.