使用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进行测试。
微信图片_20210412194359_副本.jpg 
看上去更平滑一些。
微信截图_20210416173529.png 


sitemap