1

第一次来这里，希望大家帮忙。

所以我做了一个四叉树函数，给定 i=1:4 坐标 (localmaplon[i],localmaplat[i])，它搜索并返回 4 个四叉树节点，这样我就可以看到 localmap 在哪些四叉树中。

我遇到的问题是在第一个解决方案之后，它将四叉树节点设置为它搜索的四叉树而不是根节点：

我的 cout 看起来像这样：

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

你们能帮我弄清楚我做错了什么吗！这是两个功能。这是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;
}
4

0 回答 0