About the trace of rotation matrix during calculating camera pose error

[DreamWaterFound]

Thanks for your excellent work!

I have confused with the operation of computing the trace of rotation matrix during calculating camera pose error in Tracking.cc:

https://github.com/halajun/VDO_SLAM/blob/2c3d23313954b6ef509959417a32d1d72538f0db/src/Tracking.cc#L724

...
cv::Mat T_lc_inv = mCurrentFrame.mTcw*Converter::toInvMatrix(mLastFrame.mTcw);
cv::Mat T_lc_gt = mLastFrame.mTcw_gt*Converter::toInvMatrix(mCurrentFrame.mTcw_gt);
cv::Mat RePoEr_cam = T_lc_inv*T_lc_gt;

float t_rpe_cam = std::sqrt( RePoEr_cam.at<float>(0,3)*RePoEr_cam.at<float>(0,3) + RePoEr_cam.at<float>(1,3)*RePoEr_cam.at<float>(1,3) + RePoEr_cam.at<float>(2,3)*RePoEr_cam.at<float>(2,3) );
float trace_rpe_cam = 0;
for (int i = 0; i < 3; ++i)
{
    // NOTICE HERE
    if (RePoEr_cam.at<float>(i,i)>1.0)
         trace_rpe_cam = trace_rpe_cam + 1.0-(RePoEr_cam.at<float>(i,i)-1.0);
    else
        trace_rpe_cam = trace_rpe_cam + RePoEr_cam.at<float>(i,i);
}
cout << std::fixed << std::setprecision(6);
float r_rpe_cam = acos( (trace_rpe_cam -1.0)/2.0 )*180.0/3.1415926;
...

Why we need to determine whether the pivot elements are greater than 1 rather than compute trace directly? And what is the meaning of the + 1.0 - (RePoEr_cam.at<float>(i, i) - 1.0) operation?

That seems related to computational robustness, but I can't figure out why. I sincerely look forward to your reply. Thank you very much! :heart:

[halajun]

Hi @DreamWaterFound. In very rare case, the "RePoEr_cam" matrix is not an orthogonal matrix but near, with the diagonal values slightly greater than 1, for example =1.001; That makes ''acos( (trace_rpe_cam -1.0)/2.0 )'' a non-real number. So I did it to avoid the axis angle being an imaginary number, and show the result with no bug (though it is not a good way to do so). A standard way to do should be to find the nearest orthogonal matrix of 'RePoEr_cam', please refer to here. Anyway, it is just for debug, please ignore it, and i will fix it in later version. If you want to evaluate results of VDO-SLAM, I suggest you run and save the results, and use your own evaluation tool to evaluate them. Hope this helps.

[DreamWaterFound]

Thanks !!