solving for unknown 2d rotation + translation

Question

This is a continuation of the question I asked here. I thought I'll ask a more concrete question. So far, this is what I have done to transform from a feature point detected by camera sensor in to a map point, where feature is a 2d homogenous vector.

map_point = T_world_origin.inverse() * T_world_gps * T_gps_camera * feature_point

There is significant ghosting on the map, as shown below, and I suspected that T_gps_camera is inaccurate. I wish to solve for T_gps_camera (3x3 homogoenous matrix) analytically, and I arrive at the equation below,

M * T_world_origin.inverse() * T_world_gps_t * T_gps_camera * feature_point = T_world_origin.inverse() * T_world_gps_t+1 * T_gps_camera * feature_point

where M is a 3x3 matrix I have calculated using ICP of the map points at time t and t+1. Following this, how should I use the ICP information I have to solve for T_GPS_camera?

Answer 1

Assuming your camera is rigidly mounted to your vehicle, and that the GPS is also rigidly mounted to the vehicle, then there should be a fixed transform between the GPS and camera.

Your M matrix is the correction that shifts your data between time steps. This should just have to correct some minor white noise, from GPS noise for example. It should be zero mean. If there is some fixed offset that you haven't modeled, then you should notice your M matrix have some non-zero mean.

I would look carefully at your M matrix over time - consider getting the rotation angle from the 2x2 sub-matrix and the x/y offset from the position vector and look at those values, especially as you make turns. As I mentioned in your other question, I think you're missing a rotation sensor and that is affecting your data especially on turns.

If I'm correct, you should see zero-mean noise during straight sections and then some non-zero mean during turns. Once you get the rotation-induced issues worked out (adding a compass, IMU, etc.), then you should go back to seeing zero-mean rotations and offsets during all maneuvers.

If you're seeing some non-zero means during straight sections, then I would take those average values and update your GPS-camera offset accordingly. You could do this on the fly, by using a low-pass filter to update the GPS-camera transform, but I worry that the turns aren't correct at the moment and it shouldn't be necessary because again everything is fixed.

So, tl;dr - use the mean of your M matrix when you're getting good data to adjust your uncertain transform. Monitor the mean of the M matrix to ensure all your data is being transformed well with your known transforms.