tree - Quad Tree 検索がルートに戻らない

Question

初めてここに来ました、皆さんが助けてくれることを願っています。

そこで、i=1:4 の座標 (localmaplon[i],localmaplat[i]) を指定して、4 つの quadtree ノードを検索して返す quadtree 関数を作成したので、localmap がどの quadtree にあるかを確認できます。

私が抱えている問題は、最初の解決策の後で、四分木ノードをルート ノードではなく検索する四分木として設定することです。

私のカウトは次のようになります。

i: 1 //good.
i: 10 //good.
i: 11 //good.
i: 12 //good.
i: 13 //good.
index returned: 13 //good.
index returned: 13 //good.
index returned: 13 //good.
index returned: 13 //good.
index returned: 13 //good.
0 index: 13 //good.
index: 13 <-- returns index 13! //good.
i: 13 <--Starts the next getQuadTree on index 13. Desired effect is 1 like above code.
index returned: 13 //bad
0 index: 13 //bad
index: 13 //bad
i: 13 //bad
index returned: 13 //bad
0 index: 13 //bad
index: 13 //bad
i: 13 //bad
index returned: 13 //bad
0 index: 13 //bad
index: 13 //bad

私が間違っていることを理解するのを手伝ってくれませんか！ここに2つの機能があります。これは solveLocalBuckets 関数です。

//Solve for local buckets in a local map using the quadtree    
QuadTree QuadTree::solveLocalBuckets(Robot* Bot) {
float *ptrLat;
float *ptrLon;
ptrLat=&Bot->localmaplat[0];
ptrLon=&Bot->localmaplon[0];

MatrixXf bucketMatrix(1, Bot->localmapsize);
vector <QuadTree> vecQt;
vecQt.push_back(*this);

for (int i=0; i<Tad->localmapsize; i++) {
    vecQt.push_back(vecQt[0].getQuadTree((ptrLat+i), (ptrLon+i)));
    cout << "0 index: " << vecQt[0].index << endl;
    cout << "index: " << vecQt[i+1].index << endl;
}

return vecQt[0];
}

これは getQuadTree 関数です。

//Gets a quadtree given a latitude and longitude from a quadtree.
QuadTree QuadTree::getQuadTree(float* tlat, float* tlon) {

cout << "i: " << this->index << endl;

if (hasChild) {
    if (*tlon >= centerLon) {
        if (*tlat >= centerLat) {
            //Qaudrant 1
            if (NE->hasChild) {
                *this = NE->getQuadTree(tlat, tlon);
            } else {
                cout << "index returned: " << this->index << endl;
                return *NE;
            }
        } else if (*tlat < centerLat) {
            //Qaudrant 4
            if (SE->hasChild) {
                *this = SE->getQuadTree(tlat, tlon);
            } else {
                cout << "index returned: " << this->index << endl;
                return *SE;
            }
        } else {
            ROS_INFO("Robot latitidue data corrupt.");
        }
    } else if (*tlon < centerLon) {
        if (*tlat >= centerLat) {
            //Quadrant 2
            if (NW->hasChild) {
                *this = NW->getQuadTree(tlat, tlon);
            } else {
                cout << "index returned: " << this->index << endl;
                return *NW;
            }
        } else if (*tlat < centerLat) {
            //Quadrant 3
            if (SW->hasChild) {
                *this = SW->getQuadTree(tlat, tlon);
            } else {
                cout << "index returned: " << this->index << endl;
                return *SW;
            }
        } else {
            ROS_INFO("Robot latitidue data corrupt.");
        }

    } else {
        ROS_INFO("Robot longitutde data corrupt.");
    }
}
cout << "index returned: " << this->index << endl;
return *this;
}