(七) carla真实世界坐标系与全局俯视地图像素坐标系变换

问题陈述

下图为 $carla$ 中 $Town07$ 的真实世界。

下图为 $carla$ 中 $Town07$ 的全局俯视地图。

现在就想将 $carla$ 中真实世界中的位置与全局俯视地图上的像素点对应起来，在全局俯视地图上实时地将车辆位置显示出来。

步骤

1.采集多组点对

首先采集多组 $carla$ 中真实世界中的位置与全局俯视地图上对应的像素点对。

1.1 在 $carla$ 安装目录下启动 $carla$

./CarlaUE4.sh

1.2 启动手动控制程序

将 $carla$ 中自带的PythonAPI/examples/manual_control.py 中下述程序进行更改:
更改 $world$


更改这两处，然后就可以加载 $Town07$ 地图进行手动控制车辆。
运行 $manual\_control.py$

1.3 采集点对

手动控制车辆，把车开到一些特殊的位置

这时在 $carla$ 真实世界中的位置和在全局俯视地图上对应的像素点位置如下图所示。


可以在下图所示界面的 $Location$ 处读到世界坐标系下的位置坐标。

在 $python$ 中显示图片，然后用鼠标点到大概位置处，读取像素坐标值。

import cv2
display_img = cv2.imread("./Town07.jpg")
cv2.imshow("display_img", display_img)
cv2.waitKey(0)

这样就完成了$carla$ 中真实世界中的位置与全局俯视地图上对应的像素点位置的采集。
重复上述步骤，多采集几组点，这里我采集了7组点。

2.求变换矩阵

有了这些点对后，就可以求变换矩阵了。这里利用 $opencv$ 中自带的 $cv::findHomography$ 函数进行求解。

vector<cv::Point2f> world_points; 
vector<cv::Point2f> image_points; 
cv::Mat H;
void read_points() 
{
    FILE* fp1, *fp2;
    world_points.clear();
    image_points.clear();
    fp1 = fopen("./input_image_points.txt","r");
    fp2 = fopen("./input_world_points.txt","r");
    float p_x,p_y;

    if(fp2 != nullptr) 
    {
        for(int i=0;i<7;i++) 
        {
        fscanf(fp2,"%f %f",&p_x,&p_y);
        cv::Point2f src_point(p_x,p_y);
        world_points.push_back(src_point);
        }
        fclose(fp2);
    } else 
    {
        printf("input_image_points.txt read error!\n");
        abort();
    }

    if(fp1 != nullptr) 
    {
        for(int i=0;i<7;i++) 
        {
	        fscanf(fp1,"%f %f",&p_x,&p_y);
	        cv::Point2f src_point(p_x,p_y);
	        image_points.push_back(src_point);
        }
        fclose(fp1);
    } 
    else 
    {
        printf("input_image_points.txt read error!\n");
        abort();
    }
    cout << "input world_points:\n";
    for(size_t i = 0;i<world_points.size();++i) 
    {
        cout << world_points[i] << endl;
    }
    cout << "input image_points:\n";
    for(size_t i = 0;i<world_points.size();++i) 
    {
        cout << image_points[i] << endl;
    }
}
void getH() 
{
    H = cv::findHomography(world_points,image_points,0);
    cout << "H:\n" << H << endl;
}
int main()
{
    read_points();
    getH();
    return 0;
}

将 $carla$ 中真实世界中的位置保存到一个 $txt$ 文件中；然后将与全局俯视地图上对应的像素点位置保存到一个 $txt$ 中，运行程序，得到一个变换矩阵。
这里得到的 $Town07$ 对应的变换矩阵为
$$\left[\begin{array}{ccc}1.624402368677193& 0.01825124828532574& 463.1594657435309\\ 0.02069841120825797& 1.61886922216472& 413.426694931497\\ 0& 0& 1\end{array}\right]$$

3.测试

有了这个变换矩阵，就可以将这个变换矩阵与车辆在 $carla$ 中的世界坐标相乘得到其在图片上的像素坐标位置。
也可以将车辆跑过的 $carla$ 中的世界坐标记录下来，然后将车辆走过的轨迹绘制出来。效果如下图所示。