如何根据机器人左右轮的编码计数在图像上画轨迹

根据左右轮子的编码数转换成轨迹，然后在opencv的图片上将轨迹traj描出来。

data.txt部分如下

第3和第4为左右轮子的编码数。

0,0,128,54,105,60,60,469,496
0,0,130,170,103,60,60,469,496
0,0,132,555,555,60,60,470,496
0,0,134,555,555,60,60,469,496
0,0,135,169,555,60,60,469,496
0,0,137,169,97,60,60,498,500
0,0,139,169,104,60,60,469,496
0,0,140,170,103,60,60,469,496
0,0,142,169,105,60,60,469,496
0,0,144,170,103,60,60,469,497
0,0,145,169,104,60,60,469,496
0,0,147,169,104,60,60,469,496
0,0,149,169,105,60,60,469,497
0,0,150,169,104,60,60,498,498

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 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("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));

        Sonars_logs[Yr][Xr] = 1;
        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
        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);

    }
}

#ifndef OCC_GRID_h
#define OCC_GRID_h
//#include <opencv2/opencv.hpp>
//#ifdef __cplusplus
//extern "C" {
//#endif
extern int offline_test();

extern double Sonars_logs[300][300];
//#ifdef __cplusplus
//}
//#endif
#endif
最终执行完后，效果如下。

下一篇将根据x,y坐标画轨迹。