Discrepancy between world and camera pose

[nplayle]

Hi Everyone,

I've got PTAM running on a gimballed camera. When I do a rotation with the camera the position reported in the 'view map' portion of the PTAM window stays constant (within 1cm or so).

However the position reported on the ros topic /vslam/pose is not constant and changes dramatically, around 20cm. The /vslam/pose_world topic doesn't change as expected. Graph below:

I've narrowed the offending code down to System.cc lines 296-304, but I can't quite pick out what's causing it.

    //world in the camera frame
    TooN::Matrix<3, 3, double> r_ptam = pose.get_rotation().get_matrix();
    TooN::Vector<3, double> t_ptam =  pose.get_translation();

    tf::StampedTransform transform_ptam(tf::Transform(tf::Matrix3x3(r_ptam(0, 0), r_ptam(0, 1), r_ptam(0, 2), r_ptam(1, 0), r_ptam(1, 1), r_ptam(1, 2), r_ptam(2, 0), r_ptam(2, 1), r_ptam(2, 2))
    , tf::Vector3(t_ptam[0] / scale, t_ptam[1] / scale, t_ptam[2] / scale)), header.stamp, frame_id, PtamParameters::fixparams().parent_frame);

    //camera in the world frame
    TooN::Matrix<3, 3, double> r_world = pose.get_rotation().get_matrix().T();
    TooN::Vector<3, double> t_world =  - r_world * pose.get_translation();

    tf::StampedTransform transform_world(tf::Transform(tf::Matrix3x3(r_world(0, 0), r_world(0, 1), r_world(0, 2), r_world(1, 0), r_world(1, 1), r_world(1, 2), r_world(2, 0), r_world(2, 1), r_world(2, 2))
        , tf::Vector3(t_world[0] / scale, t_world[1] / scale, t_world[2] / scale)), header.stamp, PtamParameters::fixparams().parent_frame, frame_id);

Does anyone have any ideas / a fix? I think it's just an incorrect transform and that the camera is calibrated correctly.

[simonlynen]

@nplayle thank you for the detailed description. I don't really understand what you are not happy with. Since these are quantities, which express the pose in different frames of reference, what you see is what is expected. topic "pose": The orientation and position of the world expressed in the camera frame of reference. If you rotate the camera you will not see a translation (original PTAM behaviour). topic "pose-world": The orientation and position of the camera expressed in the world frame of reference (inverse of pose). Unless you are at the origin of the system you will have an positional offset, and the lever arm will lead to a change in your reported position (active rotation).

[nplayle]

@simonlynen Sorry, you're 100% correct, it makes sense that the position from the camera would change as the co-ordinate system changes orientation. That explains why I couldn't find any errors in the transform then, can't believe that one slipped over my head.

Thanks!