opencv根据x和y坐标paint画轨迹trajactory

调用opencv的库可以将x,y坐标直接标记在数组中,然后将数组转换成opencv 的mat图片就可以将轨迹非常快的
画出来非常方便。



#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
unsigned char Sonars_logs[Map_Size][Map_Size]={128};
int main()
{
	
    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("stepdata.csv", ios::in);
	string lineStr;
	
	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_8UC1,cv::Scalar(127));
	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));
	
	Mat img3(Map_Size, Map_Size, CV_8UC1, cv::Scalar(128));
	Mat img2(Map_Size, Map_Size, CV_8UC1, cv::Scalar(0)); 
	memset(&Sonars_logs[0][0], 127, sizeof(Sonars_logs));
	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
		int linenumber = atoi(a0.c_str());
		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());
		string Rsonar_str =  Trim(lineArray[4]);
		int Rsonar = atoi(Rsonar_str.c_str());
		//string LIR =  Trim(lineArray[5])*10 ;//Same for IR sensor values
		//string RIR =  Trim(lineArray[6])*10 ; 
		cout << " first linenumber : " << linenumber << endl;// 
		 
		cout << "first  x : " << Fsonar << endl;//
		cout << " first y : " << Rsonar << endl;//
		printf("convert end \n");
		cv::Mat Aimg(Map_Size, Map_Size, CV_8UC1,cv::Scalar(0));
		
		 
		Sonars_logs[Yr][Xr] += 20;
		std::memcpy(Aimg.data, Sonars_logs, Map_Size*Map_Size*sizeof(unsigned char));
		Map_log_sonar = Aimg;
		img3 = Map_log_sonar;
		resize(img3,img3,Size(1000,1000));
		 imshow("Aimg",img3);
		
		waitKey(1);
		int Xrnext,Yrnext;
		if(Fsonar < 300)
		 Xrnext =   Fsonar; //Xr + float((cos(CalculateRadians(angle)) * (avencoder*mm_per_tick)*scale)) ;//
		//	Calculate new robot position from readings
		if(Rsonar < 300)
		 Yrnext =  Rsonar ;//Yr + float((sin(CalculateRadians(angle)) * (avencoder*mm_per_tick)*scale));

		
		Xr = Xrnext;
		Yr = Yrnext;
		
					
		
	}
}

sitemap