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;

    }
}