PTAM/PTAMMの変数アクセス法

Trackerクラスからのアクセス

現在のキーフレームで観測される特徴点の三次元位置を列挙したい

PTAM/PTAMMの特徴はキーフレームという代表フレーム上の特徴点のみが三次元復元される。 現在のフレームに最も違いカメラ位置であるキーフレームで観測される特徴点のみを列挙したい場合には、 Trackerクラスのメンバ変数mCurrentKFから特徴点を参照すれば良い。

KeyFrame & nearestKF = mMapMaker.ClosestKeyFrame(mCurrentKF); //もともとprivateだがpublicに変更した
std::map::iterator p;
std::map::iterator s = nearestKF.mMeasurements.begin();
std::map::iterator e = nearestKF.mMeasurements.end();
for(p = s; p != e; ++p)
{
 MapPoint& mp = *(p->first);
 if(mp.bBad) continue; //三次元位置が推定されていない点も登録されている
 std::cout 
  << mp.veWorldPos[0] << "," 
  << mp.veWorldPos[1] << "," 
  << mp.veWorldPos[2] << std::endl;
}

現フレームでトラックされた特徴点の三次元位置を列挙したい

現フレームで新たに検出された特徴点のうち、 既に三次元位置が登録されている特徴点とのマッチングを終え、 マップ点の三次元位置と関連付けられている特徴点のみを列挙したい場合は、 特徴点を参照するタイミングに注意せねばならない。 単純には、以下のmy_process()ようにトラッキングを終えた直後に、参照すればよい。

void Tracker::TrackFrame(Image<CVD::byte> &imFrame, bool bDraw)
{
    :
    :
 
    // Decide what to do - if there is a map, try to track the map ...
    if(mpMap->IsGood())
    {
        if(mnLostFrames < NUM_LOST_FRAMES)  // .. but only if we're not lost!
        {
            if(mbUseSBIInit)
                CalcSBIRotation();
            ApplyMotionModel();       // 
            TrackMap();               //  These three lines do the main tracking work.
            UpdateMotionModel();      // 
 
            //このタイミングでトラッキングされたばかりの特徴点データを取得する。
            my_process();

この箇所では、TrackerDataからとランキングされている特徴点の三次元位置を以下のように取得する。

void Tracker::my_process()
{
    vector<TrackerData*>::iterator it = vIterationSet.begin();
    for(; it!= vIterationSet.end(); ++it)
    {
        if(! (*it)->bFound) continue;
 
        //2d
        Vector<2> & pos2d = (*it)->v2Found;
        cout << boost::fromat("2d point: %f, %f\n") % pos2d[0] % pos2d[1];
 
        //3d
        Vector<3> & pos3d = (*it)->Point.v3WorldPos;
        cout << boost::fromat("3d point: %f, %f, %f\n") % pos3d[0] % pos2d[1] % pos2d[2];
    }
}