ROS topic: opencv Mat转nav_msgs/OccupancyGrid 并发布

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/Zed_Of_Zoe/article/details/116703598

目的

在处理SLAM地图模型时, 通常需要把点云映射到二维图像上进行处理, 需要用到opencv Mat.
处理完成后, 需要将opencv Mat转为ros消息nav_msgs/OccupancyGrid并发布.
其中需要注意物理坐标与图像坐标的转换

涉及的主要全局变量

cv::Mat MapMat; // 二维图像
double minx, miny, maxx, maxy; // 物理坐标的最大/最小值, 单位 m
int xsize, ysize; // 二维图像的尺寸, xsize为rows, ysize为cols
float resolution; // 二维图像的分辨率, 单位 m


vector<PointTypePose> keyPoses6D_normal; // 关键帧的6自由度位姿
vector<pcl::PointCloud<PointType>::Ptr> ground_cloud_key_vec; // 关键帧中的地面点云

物理坐标与图像坐标的转换

由于某些原因, 我使用的物理坐标系与图像坐标系的XY轴是相反的.

点云映射到二维图像

// 初始化MapMat矩阵
PointType pt1, pt2;
pcl::getMinMax3D(*laser_cloud_surround, pt1, pt2);
minx = pt1.z, miny = pt1.x, maxx = pt2.z, maxy = pt2.x;
// 多加20像素以免不越界
xsize = ceil((maxx - minx) / resolution + 20), ysize = ceil((maxy - miny) / resolution + 20);

unsigned char tmp_MapMat[xsize][ysize];
for(int i = 0; i < xsize; i++)
{
    unsigned char *data = *(tmp_MapMat + i);
    for(int j = 0; j < ysize; j++)
        *(data + j) = 127;
}

/*
用点云更新tmp_MapMat
*/
// 更新MapMat示例
int ground_cloud_key_vec_size = ground_cloud_key_vec.size();
int pose_size = keyPoses6D_normal.size();
//ground_cloud_all->clear();
pcl::PointCloud<PointType>::Ptr tmp_ground_cloud_all;
tmp_ground_cloud_all.reset(new pcl::PointCloud<PointType>());

for(int index = 0; index < pose_size; index++)
{
    PointTypePose pt = keyPoses6D_normal[index];
    if(ground_cloud_key_vec_size > index)
    {
        pcl::PointCloud<PointType>::Ptr ground_cloud_key;
        ground_cloud_key.reset(new pcl::PointCloud<PointType>());
    
        ground_cloud_key = transformPointCloud(ground_cloud_key_vec[index], &pt);
        *tmp_ground_cloud_all += *ground_cloud_key;
        for(auto p:ground_cloud_key->points)
        {
            Eigen::Vector2i coor = PointType2MatCoor(p);
            for(int i = -1; i <= 1; i++)
            {
                int x = coor[0] + i;
                unsigned char* data = *(tmp_MapMat + x);
                for(int j = -1; j <= 1; j++)
                {
                    int y = coor[1] + j;
                    if(x<xsize && x>=0 && y<ysize && y>=0)
                    {
                        unsigned char pixel = *(data + y);
                        if(pixel == 255)
                            continue;
                        if(pixel < 255 - free_acc_val)
                            *(data + y) = pixel + free_acc_val;
                        else
                            *(data + y) = 255;
                    }
                }
            }
        }
    }
}

IsMapMatUpdating = true;
MapMat.release();
MapMat = cv::Mat(xsize, ysize, CV_8UC1, tmp_MapMat);
IsMapMatUpdating = false;

物理坐标转图像坐标

其中, 物理坐标转图像坐标的函数,
考虑到MapMat的更新和发布在不同的thread, 重载了一个加入rows和cols参数的函数

Eigen::Vector2i PointType2MatCoor(PointType pt)
{
    int coor0 = xsize - ((pt.x - minx) / resolution) - 10;
    int coor1 = ysize - ((pt.y - miny) / resolution) - 10;
    return Eigen::Vector2i(coor0, coor1);
}

Eigen::Vector2i PointType2MatCoor(PointType pt, int rows, int cols)
{
    int coor0 = rows - ((pt.x - minx) / resolution) - 10;
    int coor1 = cols - ((pt.y - miny) / resolution) - 10;
    return Eigen::Vector2i(coor0, coor1);
}

图像坐标转物理坐标

相反地, 图像坐标转物理坐标的函数,
考虑到MapMat的更新和发布在不同的thread, 重载了一个加入rows和cols参数的函数

PointType MatCoor2PointType(Eigen::Vector2i coor)
{
    double x = (xsize - coor[0] - 10) * resolution + minx;
    double y = (ysize - coor[1] - 10) * resolution + miny;
    PointType res;
    res.x = x, res.y = y;
    return res;
}

PointType MatCoor2PointType(Eigen::Vector2i coor, int rows, int cols)
{
    double x = (rows - coor[0] - 10) * resolution + minx;
    double y = (cols - coor[1] - 10) * resolution + miny;
    PointType res;
    res.x = x, res.y = y;
    return res;
}

opencv Mat转nav_msgs/OccupancyGrid 并发布

注意图像坐标系和物理坐标系相反, 体现在对origin和gridmap.data的赋值

if(!IsMapMatUpdating && IsMapUpdated)
{
    Mat MapMat_copy = MapMat.clone();
    int rows = MapMat_copy.rows, cols = MapMat_copy.cols;
    nav_msgs::OccupancyGrid gridmap;
    gridmap.info.resolution = resolution;
    PointType origin = MatCoor2PointType(Eigen::Vector2i(rows-1, cols-1), rows, cols);
    gridmap.info.origin.position.x = origin.x;
    gridmap.info.origin.position.y = origin.y;
    gridmap.info.origin.position.z = 1.0;
    gridmap.info.origin.orientation.x = 0.0;
    gridmap.info.origin.orientation.y = 0.0;
    gridmap.info.origin.orientation.z = 0.0;
    gridmap.info.origin.orientation.w = 1.0;
    gridmap.info.width = rows;
    gridmap.info.height = cols;
    gridmap.data.resize(rows * cols);
    for(int x = 0; x < rows; x++)
    {
        unsigned char * data = MapMat_copy.ptr<uchar>(rows-1 - x);
        for(int y = 0; y < cols; y++)
            gridmap.data[y * rows + x] = 100 - *(data + cols-1 - y) * 100 / 255;
    }
    gridmap.header.stamp = ros::Time().fromSec(time_now);
    gridmap.header.frame_id = "/map";
    pubMapMatRVIZ.publish(gridmap);
}