写在前面

这篇文章分享的主要是交互界面的实现,因为我是基于C++实现的,所以可视化界面需要第三方库,我选择的是easyX,然后这篇文章主要介绍atan2()的坐标转换测试的可视化以及rrt探索枝干的可视化。

程序效果

黑色:障碍物(可由鼠标绘制);
绿色线段:探索路径;
绿色方块:终点
红色方块:起点
棕色线段:最终路径
点划线:路径探索下边界
绿色圆点:代表算法开始运行

程序介绍

atan2()测试

首先是对于atan2()函数输出的测试,函数使用是比较简单的,输入就是dx和dy,输出就是atan的弧度值,但是算法应用的是相机坐标系,不知道atan2()输出的弧度值是什么坐标系的,我们需要进行测试,于是就有了这个测试的可视化程序。

角度转换完毕之后就得到下面的测试图结果:

测试方法就是在空白部分坐标点击一下,测试程序会生成一条从特定点到鼠标所在点的线段,并且在图中显示出此时的弧度值。

可以看到转换之后弧度数值已经正常了,y轴正方向是正的,y轴负方向是负的,此时就可以带入算法中进行运算了。

程序实现如下所示:

void RRT::changeAngle(float& angle)
{
    if (angle < -PI / 2 && angle > -PI) { angle = -3 * PI / 2 - angle; }
    else { angle = PI / 2 - angle; }
}

void RRT::testAtan2Radian()
{
    ExMessage mse;

    while (1) {
        mse = getmessage(EM_MOUSE);
        if (mse.message == WM_LBUTTONDOWN) {
            line(480, 240, mse.x, mse.y);
            float theta = atan2((mse.x - 480), (mse.y - 240));
            changeAngle(theta);
            TCHAR s1[5];
            _stprintf_s(s1, sizeof(s1), _T("%.4f"), theta);
            outtextxy(mse.x, mse.y, s1);
        }
    }
}

探索节点线段

rrt在搜索的时候会不断延伸节点探索环境,因此就涉及到了绘制rrt枝干的可视化问题,于是我就写了这个测试例程。

测试的方法就是,基于纠正出来的atan()2函数返回的弧度值进行线段绘制,线段起点是鼠标坐标,然后往终点方向延伸特定长度。

生成了线段之后,测试程序将会在分割线下面显示线段2个端点的坐标。

示意图如下:

程序实现如下所示:

void RRT::showParaBelow(const Point* search_end)
{
    ExMessage mse;

    while (1) {
        mse = getmessage(EM_MOUSE);

        wchar_t s1[7];
        _stprintf_s(s1, sizeof(s1), _T("(%d,"), mse.x);
        outtextxy(100, 500, s1);
        wchar_t s2[7];
        _stprintf_s(s2, sizeof(s2), _T("%d)"), mse.y);
        outtextxy(150, 500, s2);

        Point p(mse.x, mse.y);
        search_end = getRRTNode(&p);

        _stprintf_s(s1, sizeof(s1), _T("(%d,"), search_end->x);
        outtextxy(300, 500, s1);
        _stprintf_s(s2, sizeof(s2), _T("%d)"), search_end->y);
        outtextxy(350, 500, s2);

        if (mse.message == WM_LBUTTONDOWN)
            line(p.x, p.y, search_end->x, search_end->y);
    }
}