近期正在做全局规划+局部动态规划的项目,目前遇到的问题是,我们如何利用C++处理pgm地图文件。即将地图信息要与像素点结合起来。所以我们需要知道地图读取和处理的底层原理,这样更好地在非ROS平台下移植。
如下几条信息需要了解:
(1)data[]是按照那张地图图片的自底向上,自左至右逐个像素点存储的.
(2) 在使用二维地图定位导航时,建好的地图文件中包括
m
a
p
.
p
g
m
map.pgm
map.pgm和
m
a
p
.
y
a
m
l
map.yaml
map.yaml.其中.yaml文件如下:
image: map.pgm #文件名
resolution: 0.050000 #地图分辨率 单位:米/像素
origin: [-49.0286, -107.401, 0.0] #图像左下角在地图坐标下的坐标
negate: 0 #是否应该颠倒 白:自由/黑:的语义(阈值的解释不受影响)
occupied_thresh: 0.65 #占用概率大于此阈值的像素被认为已完全占用
free_thresh: 0.196 #用率小于此阈值的像素被认为是完全空闲的
需要注意的是origin: [-49.0286, -107.401, 0.0] #图像左下角在地图坐标下的坐标,我们后续利用这条信息,建立像素与世界坐标系之间的关系。
(3)实际上,我们在路径规划实施过程中,是接收到地图像素信息data[],(一维数组),然后将其复原为原来的像素坐标,再进行路径规划处理。
data[]复原成地图图片像素坐标关系为:
for(int i = 0; i<map_info_width*map_info_height; i++)
{
x = i%map_info_width; //还原为像素坐标
y = i/map_info_width; //还原为像素坐标
if(data[i] != 0)
{ cout<<"obstacle:"<<endl;
//PG.map_generator_.addCollision({x, y}, 3);
PG.map_generator_.addCollision({x, y}, 3);
}
cout<<endl;
}
像素坐标wx,wy复原成data索引如下:
/**
* @brief Given two map coordinates... compute the associated index
* @param mx The x coordinate
* @param my The y coordinate
* @return The associated index
*/
inline unsigned int getIndex(unsigned int mx, unsigned int my) const
{
return my * size_x_ + mx;
}
重点是my * size_x_ + mx;
把二维数映射成成一维数组索引。
s
i
z
e
size
size_
x
代表地图宽度(
y
)
x代表地图宽度(y)
x代表地图宽度(y)。
(4) 由地图坐标->图像像素坐标
基于地图的坐标转换到图像坐标系上
w
x
w
y
w_x w_y
wxwy表示地图坐标系下的坐标,resolution为分辨率,则:
image_x = (wx - origin_x) / resolution
image_y = (wy - origin_y) / resolution
(5)由图像像素坐标->地图坐标
image_x,image_y表示在图像像素坐标系中的坐标
w
x
w
y
w_x w_y
wxwy表示地图坐标系下的坐标,resolution为分辨率,则:
wx=image_x*resolution+origin_x
wy=image_y*resolution+origin_y
我们举了一个从地图pgm读取到处理成目标地图数据格式data[] 的例子。
int main(int argc, char **argv)
{
PathGenerator PG;
//Read pgm
cv::Mat m4 = cv::imread("/home/juchunyu/20231013/globalPlanner/AStar-ROS/map/map.pgm",cv::IMREAD_GRAYSCALE);
cout << "图像宽为:" << m4.cols << "\t高度为:" << m4.rows << "\t通道数为:" << m4.channels() << endl;
/*
for (int r = 0; r < m4.rows; ++r) {
for (int c = 0; c < m4.cols; ++c) {
int data = m4.at(r, c);
}
}
cout<<"0"<
// Round goal coordinate
float goal_x = 10;//round(goal_msg->pose.position.x*10)/10;
float goal_y = 10;//round(goal_msg->pose.position.y*10)/10;
double origin[3] = {-9.500000, -10.000000, 0.0};
double occupied_thresh = 0.65;
double free_thresh = 0.196;
int Occupy = 1;
int NoOccupy = 0;
double map_resolution = 0.05;
/*
vector> maze = {
{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
{ 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1 },
{ 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1 },
{ 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1 },
{ 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1 },
{ 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1 },
{ 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1 },
{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }
};
*/
vector<vector<int>> maze = {
{ 0, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0 }
};
vector<int> data;
for(int i = m4.rows-1;i >= 0;i--){
for(int j = 0;j < m4.cols;j++){
if(m4.at<unsigned char>(i,j)/255 > free_thresh){
data.push_back(Occupy);
} else {
data.push_back(NoOccupy);
}
}
}
/*
int num = 0;
for(int i =maze.size()-1;i >= 0;i--){
for(int j = 0;j < maze[0].size();j++){
num++;
if(maze[i][j] > free_thresh){
data.push_back(Occupy);
} else {
data.push_back(NoOccupy);
}
}
}
cout<<"cishu"<
for(int i = 0;i<data.size();i++){
cout<<data[i]<<" ";
}
cout<<endl;
//cout<<"maze.size()="<
//cout<<"maze[0].size()"<
//cv::imshow("res_mat", m4);
//cv::waitKey(0);
// map_exsit_ = false;
//map_info_ = map_msg->info;
//int map_info_width = maze[0].size();//m4.cols;
//int map_info_height = maze.size();//m4.rows;
int map_info_width = m4.cols;
int map_info_height = m4.rows;
// Generate Map, Options
PG.map_generator_.setWorldSize({map_info_width, map_info_height}); //{x, y}
PG.map_generator_.setHeuristic(AStar::Heuristic::manhattan);
PG.map_generator_.setDiagonalMovement(true);
cout<<"-3"<<endl;
// Add Wall
int x, y;
for(int i = 0; i<map_info_width*map_info_height; i++)
{
x = i%map_info_width;
y = i/map_info_width;
cout<<"i"<<i<<endl;
cout<<"sum:"<<map_info_width*map_info_height<<endl;
double v = double(i/(map_info_width*map_info_height));
cout<<v<<"%"<<endl;
if(data[i] != 0)
{ cout<<"obstacle:"<<endl;
//PG.map_generator_.addCollision({x, y}, 3);
PG.map_generator_.addCollision({x, y}, 3);
cout<<"("<<x<<","<<y<<")"<<" ";
}
cout<<endl;
}
cout<<"-2"<<endl;
// Remmaping coordinate
AStar::Vec2i target;
target.x = 162;//6;//2;//161;//(goal_x - origin[0]) / map_resolution;
target.y = 105;//3;//9;//112; //(goal_y - origin[1]) / map_resolution;
AStar::Vec2i source;
source.x = 94;//0;//94;//(0 - origin[0]) / map_resolution;
source.y = 99;//99;//(0 - origin[1]) / map_resolution;
cout<<"1"<<endl;
// Find Path
auto path = PG.map_generator_.findPath(source, target);
cout<<"2"<<endl;
//cout<x<<' '<y<
//nav_msgs::Path path_msg;
if(path.empty())
{
cout<<"\033[1;31mFail generate path!\033[0m"<<endl;
//ROS_INFO("\033[1;31mFail generate path!\033[0m");
}
for(auto coordinate=path.end()-1; coordinate>=path.begin(); --coordinate)
{
// geometry_msgs::PoseStamped point_pose;
// Remmaping coordinate
//point_pose.pose.position.x = (coordinate->x + map_info_.origin.position.x / map_info_.resolution) * map_info_.resolution;
//point_pose.pose.position.y = (coordinate->y + map_info_.origin.position.y / map_info_.resolution) * map_info_.resolution;
//path_msg.poses.push_back(point_pose);
cout<<coordinate->x<<" "<<coordinate->y<<endl;
}
//path_msg.header.frame_id = "map";
// pub_robot_path_.publish(path_msg);
//ROS_INFO("\033[1;36mSuccess generate path!\033[0m");
// ros::spin();
return 0;
}
完整工程参见https://github.com/JackJu-HIT/A-star/tree/master.
------add 20231212
如下是一个模拟地图读取pgm过程,并解析成地图坐标data,再将其还原成图片显示的过程。
//Read pgm
cv::Mat m4 = cv::imread("/home/juchunyu/20231013/globalPlanner/AStar-ROS/map/map.pgm",cv::IMREAD_GRAYSCALE);
cout << "图像宽为:" << m4.cols << "\t高度为:" << m4.rows << "\t通道数为:" << m4.channels() << endl;
cv::imshow("oriinal", m4); //原始地图图片,原点在左上角,像素坐标系
//cv::waitKey(0);
vector<char> data;
for(int i = m4.rows-1;i >= 0;i--){
for(int j = 0;j < m4.cols;j++){
{
data.push_back(m4.at<unsigned char>(i,j)); //处理成data[]一维数组
}
}
}
cv::Mat map_info( m4.cols, m4.rows, CV_8UC1);//initlize
cv::Mat map_info_Rotate( m4.cols, m4.rows, CV_8UC1);//initlize
for(int i = (m4.cols)*(m4.rows) - 1; i > 0; i--)
{
int x = i%m4.cols; //还原为像素坐标
int y = i/m4.cols; //还原为像素坐标 // 还原成二维数组,但原点在左下角
//unsigned char value = 255;
map_info.at<unsigned char>(x, y) = data[i];
//cout<
}
//
int l = 0;int k = m4.rows;
for(int j = 0,k = m4.rows; j < m4.rows; j++,k--){
for(int i = 0,l = 0; i < m4.cols;i++,l++){
map_info_Rotate.at<unsigned char>(l,k) = map_info.at<unsigned char>(i, j);
}
}
//如果显示,需要将坐标系原点从左下角转换成左上角。
cv::imshow("map_info", map_info);
cv::rotate(map_info, map_info_Rotate,cv::ROTATE_90_COUNTERCLOCKWISE);// cv::ROTATE_90_CLOCKWISE);
cv::imshow("map_info_Rotate", map_info_Rotate);
cv::waitKey(0);
return 0;
//Read pgm
cv::Mat m4 = cv::imread("/home/juchunyu/20231013/globalPlanner/AStar-ROS/map/map.pgm",cv::IMREAD_GRAYSCALE);
cout << "图像宽为:" << m4.cols << "\t高度为:" << m4.rows << "\t通道数为:" << m4.channels() << endl;
cv::imshow("oriinal", m4);
//cv::waitKey(0);
vector<char> data;
for(int i = m4.rows-1;i >= 0;i--){
for(int j = 0;j < m4.cols;j++){
{
data.push_back(255 - m4.at<unsigned char>(i,j));
}
}
}
cv::Mat map_info( m4.cols, m4.rows, CV_8UC1);//initlize
cv::Mat map_info_Rotate( m4.cols, m4.rows, CV_8UC1);//initlize
for(int i = (m4.cols)*(m4.rows) - 1; i > 0; i--)
{
int x = i%m4.cols; //还原为像素坐标
int y = i/m4.cols; //还原为像素坐标
//unsigned char value = 255;
map_info.at<unsigned char>(x, y) = data[i];
//cout<
}
//
int l = 0;int k = m4.rows;
for(int j = 0,k = m4.rows; j < m4.rows; j++,k--){
for(int i = 0,l = 0; i < m4.cols;i++,l++){
map_info_Rotate.at<unsigned char>(l,k) = map_info.at<unsigned char>(i, j);
}
}
cv::imshow("map_info", map_info);
cv::rotate(map_info, map_info_Rotate,cv::ROTATE_90_COUNTERCLOCKWISE);// cv::ROTATE_90_CLOCKWISE);
cv::imshow("map_info_Rotate", map_info_Rotate);
cv::waitKey(0);