激光雷达数据读取以及显示C++（需配置OpenCV2.4）

#include "OpenRadar.h" #define pi 3.141592653 OpenRadar::OpenRadar(void) { } OpenRadar::~OpenRadar(void) { } bool OpenRadar::RadarRead(char *fileName){ FILE* fp = NULL; int dis = 0; int totalCnt = 0; fp = fopen(fileName,"r"); if (fp == NULL) { //cout<<"failed to read"<<endl; return false; }else { //cout<<"successed to read"<<endl; RadarRho.clear(); while(!feof(fp)) { fscanf(fp, "%d, ", &dis); RadarRho.push_back(dis); //printf("%d ", dis); } //cout<<"Total Count: "<<RadarRho.size()<<endl; } fclose(fp); return true; } void OpenRadar::CreateRadarImage(IplImage* RadarImage){ //RadarImage = cvCreateImage(cvSize(RadarImageWdith,RadarImageHeight),IPL_DEPTH_8U,1); cvZero(RadarImage); //在中心加上一个圆心 cvCircle(RadarImage, cvPoint(RadarImageWdith/2,RadarImageHeight/2),3, CV_RGB(0,255,255), -1, 8,0); int x,y; double theta,rho; unsigned char * pPixel = 0; int halfWidth = RadarImageWdith/2; int halfHeight = RadarImageHeight/2; for (int i = 0; i < RadarRho.size();i++) { theta = (i/4.0 - 45)*pi/180; rho = RadarRho.at(i); x = (int)(rho*cos(theta)/5) + halfWidth; y = (int)(-rho*sin(theta)/5)+ halfHeight; if (x >= 0 && x < RadarImageWdith && y >= 0 && y < RadarImageHeight) { pPixel = (unsigned char*)RadarImage->imageData + y*RadarImage->widthStep + 3*x+2; *pPixel = 255; }else{ //cout<<"x: "<<x<<" y: "<<y<<endl; } } }