izhengfan / se2lam

(ICRA 2019) Visual-Odometric On-SE(2) Localization and Mapping
https://github.com/izhengfan/se2lam
MIT License
404 stars 108 forks source link

usage of viewMP and mNormalVector should not be based on the camera frame #32

Open cy-goh opened 4 years ago

cy-goh commented 4 years ago

Hi

I am trying to understand the usage of the mViewMPs variable inside KeyFrame.h. As I understand it, 1 of the main purpose of mViewMps is to average the normal viewing vector inside the MapPoint variable. This is how you compute inside LocalMapper.cpp.

            Point3f x3d =
            cvu::triangulate(pMP->getMainMeasure(), mNewKF->keyPointsUn[i].pt,
                             Config::Kcam*pMP->mMainKF->Tcw.rowRange(0,3),
                             Config::Kcam*mNewKF->Tcw.rowRange(0,3));
            Point3f posNewKF = cvu::se3map(mNewKF->Tcw, x3d);
            if(pMP->acceptNewObserve(posNewKF, mNewKF->keyPoints[i])){
                continue;
            }
            if(posNewKF.z > Config::UPPER_DEPTH || posNewKF.z < Config::LOWER_DEPTH)
                continue;
            Eigen::Matrix3d infoNew, infoOld;
            Track::calcSE3toXYZInfo(posNewKF, mNewKF->Tcw, pMP->mMainKF->Tcw, infoNew, infoOld);
            mNewKF->setViewMP(posNewKF, i, infoNew);

When you set posNewKF based on cvu::se3map(mNewKF->Tcw, x3d), i think it means that the normal viewing vector is calculated from the angle between the keypoint and local camera center. This means that pMP->acceptNewObserve will pass as long as the map point is positioned around the same keypoint location inside the keyframe. If a map point is created from 2 keyframes of heading ~0degrees, acceptNewObserve will pass even from a keyframe of heading 180degrees, as long as it is around the same keypoint location in the image.

In ORB-SLAM2, the normal vector is calculated globally.

I like to check if this is bug, or if I misunderstood something. I will see if I can provide a pull request if it is a bug.