Get pose every frame, without MRPT?

[antithing]

Hi, thank you for making this code available. I have the plslam_dataset code running great on the kitti dataset, but as I am not using MRPT, I can't use the scene functions.

The variable StVO->curr_frame->DT is the frame-to-frame delta, is that correct? Can I just add these to get the current camera position? Or is there a function that i can call to grab the global pose every frame?

Thank you again!

[rubengooj]

Hi, thanks for your interest!

You're correct, for a StereoFrame DT is the pose increment from previous frame to itself, but if you access to the KeyFrame its global transformation is given by T_kf_w (the pose of the KF wrt to the world reference frame) and x_kf_w ( x_kf_w = logmap_se3(T_kf_w) ), and the uncertainty of the 6D pose is given by the matrix xcov_kf_w.

[antithing]

Thank you! So to get the pose every frame I add the DT to the previous keyframe incrementally? Then reset on a new keyframe? Is that right?

[rubengooj]

Yes, exactly :) In the VO repository you can also see the convention we followed for pose composition

[antithing]

Thanks! I have this:

Eigen::Matrix4d pose;
Eigen::Quaterniond quat;
Eigen::Vector3d poseTran;

if (StVO->needNewKF())
            {
// grab StF and update KF in StVO (the StVO thread can continue after this point)
                PLSLAM::KeyFrame* curr_kf = new PLSLAM::KeyFrame(StVO->curr_frame);
                // update KF in StVO
                StVO->currFrameIsKF();
                map->addKeyFrame(curr_kf);

                pose = curr_kf->T_kf_w;
                Eigen::Vector3d t(pose(0, 3), pose(1, 3), pose(2, 3));
                poseTran = t;

                Eigen::Matrix3d rotMat = pose.block(0, 0, 3, 3);
                Eigen::Quaterniond poseQuat(rotMat);

                poseFile << n << " " << poseTran.x() << " " << poseTran.y() << " " << poseTran.z()
                    << " " << quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w() << std::endl;

            }
            else
            {
                //additive camera pos
                pose = pose + StVO->curr_frame->Tfw;

                Eigen::Vector3d t(StVO->curr_frame->Tfw(0, 3), StVO->curr_frame->Tfw(1, 3), StVO->curr_frame->Tfw(2, 3));
                poseTran = poseTran + t;

                Eigen::Matrix3d rotMat = StVO->curr_frame->Tfw.block(0, 0, 3, 3);
                Eigen::Quaterniond poseQuat(rotMat);
                quat = quat * poseQuat;

            }

But I see strange results. if I store the keyframes in a csv as above, when i load them and the map points into a 3d programme, I am having trouble lining them up, and there seems to be a 180 degree rotation on the Y axis of the rotation that should not be there.

If I Store the frame poses, as above, they are very very wrong. If you have a moment, could you take a look and see what I am doing wrong here?

Thanks again!

[antithing]

Hi @rubengooj , sorry to bug you again, i am still stuck on this, if you have a moment could you give me a hand please? How can i get the pose every frame, and have it line up with the global map points? Thank you again!