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?