How can I obtain the optimized pose for each frame

[feel-fly]

How can I obtain the optimized pose for each frame I want to use ORBSLAM3 to get each frame's pose,so that I can use these pose to make desnse reconstruction offline. In ros_rgbd.cc,I use following code to get each frame's pose,but the result is mess cv::Mat Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());

[basedball]

TrackRGBD() returns a Sophus::SE3f object, not a cv::Mat.

[feel-fly]

you are sure，i made a mistake

[radhasaraf]

Couple of things you can try:

1. Confirm if it's Tcw that you are expecting or Twc. (Tcw takes you from world to camera whereas Twc takes you from camera to world)

2. The calibration pdf mentions, 'World (W) defines a fixed reference system, whose Z axis points in opposite direction of the gravity vector g'. Check if this is also the case for your local system. If not, you will have to use Tww', this way: Tcw * Tww'. You should be able to come up with Tww', if you know your local coordinate system. (For ex., if y axis is pointing up in your local case, then you need to make sure to rotate about the x axis so that W's y-axis is pointing up as well)

3. Confirm if your reference is the camera, if not, use the correct reference like so: Trc * Tcw, (where r is reference and Trc takes from camera frame to your reference frame)

Depending on your case, you may have to use either or all of these.