I have replaced g2o for map optimization with GTSAM. But gtsam composes increments on the right of the previous estimate, which I do not want to change. I need to set the covariance matrix for the Sim3 betweenfactors in GTSAM which need to be obtained from lsd-slam's Sim3Tracker. But Sim3Tracker (also SE3 tracker) all do left-composition of increments, hence their covariance matrix cannot be directly used. What should the approach be to obtained the required covariance matrix for right-compositional increments that GTSAM would do to the keyFrame poses ?
Would making the simaple change
// apply increment. pretty sure this way round is correct, but hard to test.
Sim3 new_referenceToFrame =Sim3::exp(inc.cast<sophusType>()) * referenceToFrame;
//Sim3 new_referenceToFrame = referenceToFrame * Sim3::exp((inc));
// apply increment. pretty sure this way round is correct, but hard to test.
Sim3 new_referenceToFrame = referenceToFrame*Sim3::exp(inc.cast<sophusType>());
//Sim3 new_referenceToFrame = referenceToFrame * Sim3::exp((inc));
Hi all!
I have replaced g2o for map optimization with GTSAM. But gtsam composes increments on the right of the previous estimate, which I do not want to change. I need to set the covariance matrix for the Sim3 betweenfactors in GTSAM which need to be obtained from lsd-slam's Sim3Tracker. But Sim3Tracker (also SE3 tracker) all do left-composition of increments, hence their covariance matrix cannot be directly used. What should the approach be to obtained the required covariance matrix for right-compositional increments that GTSAM would do to the keyFrame poses ?
Would making the simaple change
(line 236: https://github.com/tum-vision/lsd_slam/blob/master/lsd_slam_core/src/Tracking/Sim3Tracker.cpp)
to the hacked version
do the job ?