使用opencv c++占用栅格地图的实现（对数几率回归）（二）
上一篇简单简述了python opencv 借助numpy的数据结构运算实现了 占用栅格地图的构建，其中有很多巧妙的设计，今天使用opencv c++进行复现和
移植，在没有numpy的数据结构支撑下使用opencv来实现，看效果。
其中
在 numpy中会使用到l2的范数，dist = np.linalg.norm(robotpt - x)
这个在c/c++中是不存在，只能使用 sqrt 和 pow实现求距离。
另外opencv的 exp是对 浮点矩阵就 指数，这个需要注意。
numpy的所有二维array都要使用 opencv 的mat 替代和二维数组替换，涉及到 数组转mat 的方式。
sensor_model.cpp 如下
#include <iostream>
#include <stdlib.h>
#include <algorithm>
#include <time.h>
#include <unistd.h>
#include <sys/time.h>
#include <stack>
#include <opencv2/opencv.hpp>
#include "sensor_model.h"
#include "occ_grid.h"
using namespace std;
using namespace cv;
static Point get_centre(vector<Point> v) {
double x = 0, y = 0;
int s = v.size();
for (int i = 0; i < v.size(); i++) {
x += v[i].x * 100/s;
y += v[i].y * 100/s;
}
return Point((int) x/100, (int) y/100);
}
//计算两个点的角度
static double CalculateAngle(Point Mar1Point, Point Mar2Point)
{
// double k = (double)(Mar2Point.y - Mar1Point.y) / (Mar2Point.x - Mar1Point.x); //计算斜率
double arcLength1 = atan2(Mar2Point.y - Mar1Point.y,Mar2Point.x - Mar1Point.x); //弧度
double current_angle = arcLength1 * 180 / M_PI; //角度
return current_angle;
}
//int Map_Size = 100;
//int Xr = float(Map_Size/2);// #Set intial robot position
//int Yr = float(Map_Size/2);
//int Rangle =0;
int SonarModelArray(cv::Mat occmap,int Xr,int Yr,int Rangle,int SonarDist,int cellsize,float scale)
{
int Map_Size = occmap.cols;
//这里必须重新构建新地图，如果直接使用occmap 绘制，会导致重复使用出现重影
Mat mapgrid(Map_Size, Map_Size, CV_8UC1, cv::Scalar(0));
int thick = int(cellsize * scale);
SonarDist = int(SonarDist * scale);
//创建一张图片 100*100的unsigned char 矩阵 都为0
//Mat img1(Map_Size, Map_Size, CV_8UC1, cv::Scalar(0));
//Returns: Sonar_log - Array of log odds probabilities.
//This can be added to existing log odds occ map to update map with latest sonar data.
//double **Sonar_log[Map_Size][Map_Size];
int row = occmap.rows;//300
int col = occmap.cols;//300
int thicknesss=-1;
//printf("SonarDist:%d \n",SonarDist);
//在新地图中使用画椭圆
ellipse(mapgrid,Point(int(Xr),int(Yr)),Size(SonarDist, SonarDist), Rangle, -15, 15, Scalar(255), thicknesss);
//Mat dst;
//resize(src, dst, cv::Size(1000, 1000));
//创建一个mat用于存储新的坐标
//std::vector<cv::Point2i> locations; // output, locations of non-zero pixels
Mat wLocMat = Mat::zeros(mapgrid.size(),CV_8UC1);
//findNonZero(src, locations);
//在新地图中找到非0,不能使用原地图
findNonZero(mapgrid, wLocMat);
//for(int m=0;m<row;m++)
// memset(Sonars_logs[m],0,col);
for (int i = 0; i < wLocMat.total(); i++ ) {
// cout << "Zero#" << i << ": " << wLocMat.at<Point>(i).x << ", " << wLocMat.at<Point>(i).y << endl;
//使用范数求距离
//使用范数求距离
float dealtx = pow(wLocMat.at<Point>(i).x - Xr,2);
float dealty = pow(wLocMat.at<Point>(i).y - Yr,2);
float dist = sqrt(dealtx + dealty);
//printf("dist:%d \n",dist);//dist zhengque
int Ximg = wLocMat.at<Point>(i).x ; //# 是一个值 对应x
int Yimg = wLocMat.at<Point>(i).y ;//# 是一个值 对应Y
Point startp,robotp;
startp.x = Ximg;
startp.y = Yimg;
robotp.x = Xr;
robotp.y = Yr;
float theta= CalculateAngle(robotp,startp) - Rangle;
/////数量一致 小数点有偏差
if(theta < -180) //#Note:numpy and OpenCV X and Y reversed
theta = theta + 360 ;
else if( theta > 180)
theta = theta - 360 ;
double sigma_t = 5;//这里可以直接去搜标准的正态分布 一看就明白
double A = 1 / (sqrt(2*M_PI*sigma_t));
double C = pow((theta/sigma_t),2);
double B = exp(-0.5*C);
double Ptheta = A*B ;
//小数点有偏差
double Pdist = (SonarDist - dist/2)/SonarDist;
double P = (Pdist*2)*Ptheta;
double Px=0,logPx=0 ;
//printf("dist:%f %d %d\n",dist,SonarDist,thick);
if (dist > (SonarDist - thick) && dist < (SonarDist + thick))// #occupied region
{
Px = 0.5 + Ptheta;//这路的Ptheta 可以改成0.5*cell
//printf("Px:%f \n",Px);
logPx = log(Px/(1-Px));
Sonars_logs[Yimg][Ximg] = logPx;
//printf("logPx:%f \n",logPx);
// #occ = np.append(occ,[x],0)
}
else{// #free region
Px = 0.5 - P;//这的P 可以改成0.5*cell
//printf("Px free:%f \n",Px);
logPx = log(Px/(1-Px));
Sonars_logs[Yimg][Ximg] = logPx;
//printf("logPx free:%f \n",logPx);
//printf("p:%d,%d \n",Yimg,Ximg);
}
}
//unsigned char *ptmp = NULL;//这是关键的指针！！ ptmp指针指向的是img2这个图像
return 0;
}
最终使用
SonarModelArray(cv::Mat occmap,int Xr,int Yr,int Rangle,int SonarDist,int cellsize,float scale) 调用。
occ_grid.cpp
如下
#include <iostream> #include <stdlib.h> #include <algorithm> #include <time.h> #include <unistd.h> #include <sys/time.h> #include <stack> #include <opencv2/opencv.hpp> #include "sensor_model.h" #include "occ_grid.h" #include <fstream> #include <sstream> using namespace std; using namespace cv; string Trim(string& str) { str.erase(0,str.find_first_not_of(" \t\r\n")); str.erase(str.find_last_not_of(" \t\r\n") + 1); return str; } static double CalculateRadians(int degree) { //弧度 double current_angle = degree *M_PI/ 180 ; //角度 return current_angle; } #define Map_Size 300 double Sonars_logs[Map_Size][Map_Size]={0}; int offline_test_Array() { int mm_per_tick = 5 ;//Encoder resolution int Xr = float(Map_Size/2);// #Set intial robot position int Yr = float(Map_Size/2); // 读文件 ifstream inFile("Data.txt", ios::in); string lineStr; vector<vector<string>> strArray; int cnt =0; int cellsize = 50 ;// #Set cell width and height in mm float scale = float(1)/cellsize;// #Set map scale Mat src(Map_Size, Map_Size, CV_8UC1, cv::Scalar(0)); int row = Map_Size;//src.rows;//300 int col = Map_Size;//src.cols;//300 Mat Map_log_sonar = Mat(row, col, CV_64F,cv::Scalar(0.0)); Mat Map_log_sonar2= Mat(row, col, CV_64F,cv::Scalar(0.0)); Mat Map_log_sonar_res= Mat(row, col, CV_64F,cv::Scalar(0.0)); while (getline(inFile, lineStr)) { // 打印整行字符串 // cout << lineStr << endl; // 存成二维表结构 stringstream ss(lineStr); string str; vector<string> lineArray; // 按照逗号分隔 while (getline(ss, str, ',')){ lineArray.push_back(str); } string angle_str = Trim(lineArray[2]) ;//Robot heading angle int angle = atoi(angle_str.c_str()); int inv_angle = angle+30 ;//angle for rear sonar sensor int left_angle = angle-30 ; string a0 = Trim(lineArray[0]); string a1 = Trim(lineArray[1]); int avencoder = (atoi(a0.c_str()) + atoi(a1.c_str()))/2 ;//Distance travelled by robot is approximated to average of both encoder readings string Fsonar_str = Trim(lineArray[3]) ;//Sonar values are in cm, convert to mm by muliplying by 10 int Fsonar = atoi(Fsonar_str.c_str())*10; string Rsonar_str = Trim(lineArray[4]); int Rsonar = atoi(Rsonar_str.c_str())*10; //string LIR = Trim(lineArray[5])*10 ;//Same for IR sensor values //string RIR = Trim(lineArray[6])*10 ; cout << " angle : " << angle << endl;// cout << " inv_angle : " << inv_angle << endl;// cout << " Fsonar : " << Fsonar << endl;// cout << " Rsonar : " << Rsonar << endl;// printf("convert begin \n"); cv::Mat Aimg(Map_Size, Map_Size, CV_64F,cv::Scalar(0.0)); if (Fsonar < 5000){ SonarModelArray(src,Xr,Yr,angle,Fsonar,cellsize,scale); std::memcpy(Aimg.data, Sonars_logs, Map_Size*Map_Size*sizeof(double)); //cv::add(Aimg,Map_log_sonar,Map_log_sonar); Map_log_sonar = Map_log_sonar + Aimg; } if (Rsonar < 5000){ SonarModelArray(src,Xr,Yr,inv_angle,Rsonar,cellsize,scale); std::memcpy(Aimg.data, Sonars_logs, Map_Size*Map_Size*sizeof(double)); cv::add(Aimg,Map_log_sonar,Map_log_sonar); Map_log_sonar = Map_log_sonar + Aimg; } if (Rsonar < 5000){ SonarModelArray(src,Xr,Yr,left_angle,Rsonar,cellsize,scale); std::memcpy(Aimg.data, Sonars_logs, Map_Size*Map_Size*sizeof(double)); cv::add(Aimg,Map_log_sonar,Map_log_sonar); Map_log_sonar = Map_log_sonar + Aimg; } int Xrnext = Xr + float((cos(CalculateRadians(angle)) * (avencoder*mm_per_tick)*scale)) ;// // Calculate new robot position from encoder readings int Yrnext = Yr + float((sin(CalculateRadians(angle)) * (avencoder*mm_per_tick)*scale)); //#cv2.line(map_image, (int(Xr),int(Yr)), (int(Xrnext),int(Yrnext)), 150, 1) Xr = Xrnext; Yr = Yrnext; exp(Map_log_sonar, Map_log_sonar2);//e的0次方等于1 如果MAP_LOG_SONAR 的数据有0的存在，//经过求e后在map_log_sona2中变成1 然后下面的1/2是显示灰色的原因 Mat img2(Map_Size, Map_Size, CV_8UC1, cv::Scalar(0)); Mat img3(Map_Size, Map_Size, CV_8UC1, cv::Scalar(0)); img2 =1- (1 / ( 1 + Map_log_sonar2)); img3 = 1 -img2; //resize(src, dst, cv::Size(1000, 1000)); resize(img3,img3,Size(1000,1000)); imshow("Map_log",img3); waitKey(0); } } int offline_test() { //int Map_Size = 300; int mm_per_tick = 5 ;//Encoder resolution int Xr = float(Map_Size/2);// #Set intial robot position int Yr = float(Map_Size/2); // 读文件 ifstream inFile("Data.txt", ios::in); string lineStr; vector<vector<string>> strArray; int cnt =0; int cellsize = 50 ;// #Set cell width and height in mm float scale = float(1)/cellsize;// #Set map scale Mat src(Map_Size, Map_Size, CV_8UC1, cv::Scalar(0)); int row = Map_Size;//src.rows;//300 int col = Map_Size;//src.cols;//300 Mat Map_log_sonar = Mat(row, col, CV_64F,cv::Scalar(0.0)); Mat Map_log_sonar2= Mat(row, col, CV_64F,cv::Scalar(0.0)); Mat Map_log_sonar_res= Mat(row, col, CV_64F,cv::Scalar(0.0)); cv::Mat Aimg(Map_Size, Map_Size, CV_64F,cv::Scalar(0.0)); while (getline(inFile, lineStr)) { // 打印整行字符串 // cout << lineStr << endl; // 存成二维表结构 stringstream ss(lineStr); string str; vector<string> lineArray; // 按照逗号分隔 while (getline(ss, str, ',')){ lineArray.push_back(str); } string angle_str = Trim(lineArray[2]) ;//Robot heading angle int angle = atoi(angle_str.c_str()); int inv_angle = angle+180 ;//angle for rear sonar sensor string a0 = Trim(lineArray[0]); string a1 = Trim(lineArray[1]); int avencoder = atoi(a0.c_str())+ atoi(a1.c_str())/2 ;//Distance travelled by robot is approximated to average of both encoder readings string Fsonar_str = Trim(lineArray[3]) ;//Sonar values are in cm, convert to mm by muliplying by 10 int Fsonar = atoi(Fsonar_str.c_str())*10; string Rsonar_str = Trim(lineArray[4]); int Rsonar = atoi(Rsonar_str.c_str())*10; //string LIR = Trim(lineArray[5])*10 ;//Same for IR sensor values //string RIR = Trim(lineArray[6])*10 ; cout << " angle : " << angle << endl;// cout << " inv_angle : " << inv_angle << endl;// cout << " Fsonar : " << Fsonar << endl;// cout << " Rsonar : " << Rsonar << endl;// printf("convert begin \n"); if (Fsonar < 5000){ double **Sonar_logs =SonarModel(src,Xr,Yr,angle,Fsonar,cellsize,scale); double *data = NULL; for(int i=0;i< Map_Size; i++) //行数--高度 { data = Aimg.ptr<double>(i); //指向第i行的数据 for(int j=0;j<Map_Size;j++) //列数 -- 宽度 { data[j] = Sonar_logs[i][j]; } } //std::memcpy(Aimg.data, Sonar_log, Map_Size*Map_Size*sizeof(double)); //cv::add(Aimg,Map_log_sonar,Map_log_sonar); Map_log_sonar = Map_log_sonar + Aimg; for(int i=0;i< Map_Size;i++) free(Sonar_logs[i]); free(Sonar_logs); } /*if (Rsonar < 5000){ double **Sonar_logs =SonarModel(src,Xr,Yr,inv_angle,Rsonar,cellsize,scale); double* data =NULL; for(int i=0;i<Map_Size;i++) //行数--高度 { data = Aimg.ptr<double>(i); //指向第i行的数据 for(int j=0;j<Map_Size;j++) //列数 -- 宽度 { data[j] = Sonar_logs[i][j]; } } //std::memcpy(Aimg.data, Sonar_log, Map_Size*Map_Size*sizeof(double)); cv::add(Aimg,Map_log_sonar,Map_log_sonar); for(int i=0;i< Map_Size;i++) free(Sonar_logs[i]); free(Sonar_logs); }*/ int Xrnext = Xr + float((cos(CalculateRadians(angle)) * (avencoder*mm_per_tick)*scale)) ;// // Calculate new robot position from encoder readings int Yrnext = Yr + float((sin(CalculateRadians(angle)) * (avencoder*mm_per_tick)*scale)); //#cv2.line(map_image, (int(Xr),int(Yr)), (int(Xrnext),int(Yrnext)), 150, 1) Xr = Xrnext; Yr = Yrnext; exp(Map_log_sonar, Map_log_sonar2); Mat img2(Map_Size, Map_Size, CV_8UC1, cv::Scalar(0)); ; img2 = (1 / ( 1 + Map_log_sonar2)); //resize(src, dst, cv::Size(1000, 1000)); resize(img2,img2,Size(1000,1000)); imshow("Map_log",img2); waitKey(0); } }main.cpp
#include <iostream> #include <stdlib.h> #include <algorithm> #include <time.h> #include <unistd.h> #include <sys/time.h> #include <stack> #include <opencv2/opencv.hpp> #include <math.h> #include "sensor_model.h" #include "occ_grid.h"
int main(int argc ,char *argv[])
{ offline_test_Array();
}
依旧使用 rovibot进行测试。
看上去更平滑一些。
