激光雷达--数据读取和显示

原文出处:http://blog.csdn.NET/renshengrumenglibing/article/details/8600669

我使用的激光雷达都是日本产北阳电机公司Hokuyo的产品,URG04LX和UTM-30LX。UTM-30LX的价格贵一些,10买的时候大概3W,现在taobao价貌似已经涨到5W了,当然URG04LX也涨价了,09年买的时候是1W,只是不知道现在涨到了多少。

URG-04LX基本参数:

激光类型:λ=785nm,Class1,0.8mw激光
扫描距离:20-5600 mm
扫描范围:240度
角度分辨率:0.36度
距离分辨率:1mm
精度:距离 0.06-1m: +30 距离 1-4m:30%
扫描时间:100ms (10Hz刷新率)
接口类型:USB 2.0
输入电压:5V
消耗电流:500mA (最高可达800mA)
重量:160g
尺寸:50x50x70mm
典型寿命:5年


UTM-30LX基本参数:
Voltage 12.0 V ±10%
Current 0.7 A (Rush current 1.0 A)
Detection range 0.1 m to approximately 60 m (<30 m guaranteed)
Laser wavelength 870 nm, Class 1
Scan angle 270°
Scan time 25 ms/scan (40.0 Hz)
Angular resolution 0.25°
Interface USB 2.0
Weight 8.2 oz (233 g)


北阳电机提供激光雷达的上位机软件,叫做UrgBenri,可以很方便的进行数据的采集、显示、存储。如果想用C++程序自己采集可以参照上一篇笔记。
UrgBenri采集存储的数据是ASCII格式,可以直接用记事本之类的软件打开,数据格式基本是这样的:

[cpp]  view plain  copy
  1. [model]  
  2. UTM-30LX(Hokuyo Automatic Co.,Ltd.)  
  3. [frontStep]  
  4. 540  
  5. [minDistance]  
  6. 23  
  7. [maxDistance]  
  8. 60000  
  9. [motorSpeed]  
  10. 2400  
  11. [totalSteps]  
  12. 1440  
  13. [captureMode]  
  14. GD_Capture_mode  
  15. [scanMsec]  
  16. 25  
  17. [startStep]  
  18. 0  
  19. [endStep]  
  20. 1080  
  21. [serialNumber]  
  22. 00904066  
  23. [timestamp]  
  24. 793464  
  25. [logtime]  
  26. 2013:1:12:15:6:32:312  
  27. [scan]  
  28. 392;397;398;400;403;403;407;409;409;409;411;411;422;424;424;422;420;420;420;427;428;435;436;439;440;444;449;449;449;449;454;454;456;462;463;463;463;459;456;451;449;445;444;439;439;433;433;433;433;431;431;430;430;425;425;422;425;425;433;437;437;433;433;425;421;409;395;391;391;389;384;383;378;378;378;378;377;377;377;376;377;376;376;367;367;367;367;368;371;371;371;371;371;370;370;372;369;369;372;372;371;371;371;369;369;365;365;364;363;357;357;357;357;353;359;359;353;353;352;344;341;339;339;339;339;339;341;334;332;331;331;331;331;331;331;331;330;330;330;330;330;329;329;329;326;323;323;323;329;329;329;327;323;322;319;319;319;319;319;320;322;321;322;322;328;323;325;325;325;324;324;317;315;315;316;315;316;321;321;314;318;314;308;303;303;303;317;317;317;317;314;314;314;314;311;308;308;310;310;316;316;316;315;310;310;312;312;317;317;317;317;313;313;313;313;316;316;315;315;315;315;314;314;317;317;317;319;321;320;320;320;319;317;317;319;317;317;315;315;315;316;324;324;324;318;318;318;325;332;332;332;332;332;330;331;333;331;331;331;330;328;327;327;328;328;328;328;328;328;325;325;326;326;326;330;330;331;330;337;337;337;338;340;338;338;338;337;336;336;336;335;336;350;350;350;350;350;345;343;343;341;343;343;347;346;350;350;352;352;357;357;357;357;357;357;364;357;367;367;366;364;366;366;370;370;370;370;370;370;378;378;379;380;380;379;379;380;379;379;382;384;382;382;384;380;380;384;386;388;388;388;383;383;383;389;394;395;395;396;394;396;396;406;410;410;413;413;413;413;421;421;426;427;427;428;429;429;432;432;439;442;451;451;454;457;458;460;468;472;489;492;495;495;493;495;507;512;522;524;533;546;585;614;614;614;611;602;602;598;590;590;591;591;594;596;596;596;598;596;589;589;589;589;590;590;594;594;594;592;592;587;577;570;559;556;552;552;552;551;551;541;541;539;539;538;534;534;534;534;536;536;533;526;526;526;524;524;524;512;510;510;510;510;510;508;477;467;462;453;449;445;434;434;420;410;403;388;387;386;377;346;342;337;325;325;321;315;294;294;292;281;277;262;257;242;218;180;173;172;170;170;170;170;170;171;171;171;172;174;172;171;172;171;170;170;177;170;170;174;170;170;170;174;175;176;187;187;176;176;180;174;180;184;184;179;179;178;178;176;176;179;185;180;181;181;180;180;177;177;177;180;182;182;182;182;180;178;180;183;183;186;186;186;187;188;188;188;188;186;186;186;187;188;188;190;188;187;187;189;189;189;189;189;188;188;188;188;195;196;195;194;195;194;194;201;201;199;199;199;199;198;198;198;195;192;190;190;190;190;196;197;197;197;194;193;193;192;193;193;194;194;196;198;198;198;198;198;198;194;194;194;194;194;197;197;197;192;192;192;194;205;205;205;193;189;189;189;210;210;210;213;213;332;506;627;860;907;958;980;1031;1043;1111;1117;1117;1117;1000;986;985;985;985;981;981;978;971;970;970;970;970;976;976;977;977;981;981;981;945;945;945;946;956;970;975;979;979;975;948;933;920;898;859;836;823;814;799;799;794;794;793;793;789;773;748;745;736;732;727;727;727;727;729;743;744;744;744;742;729;725;711;706;706;706;708;699;698;693;690;689;688;684;675;668;665;660;660;660;660;659;659;649;644;644;646;649;647;649;647;647;634;631;631;634;628;628;625;623;622;615;601;601;601;601;601;601;589;589;589;589;590;590;590;583;583;583;580;576;576;575;575;577;577;572;569;568;567;567;568;566;568;568;566;566;561;561;561;559;557;557;557;561;562;562;561;557;557;557;559;559;560;561;561;561;561;563;563;561;561;561;559;556;556;556;562;562;562;556;556;552;552;550;547;543;547;548;548;555;555;555;553;553;547;547;547;547;547;542;541;541;541;548;551;558;558;552;552;558;559;561;564;564;564;567;567;567;568;568;570;568;568;570;573;573;573;573;568;567;568;565;565;567;573;567;567;566;566;566;574;574;574;568;568;568;570;572;572;572;573;569;569;569;573;575;579;579;579;582;581;581;585;587;587;587;585;581;581;581;586;586;586;586;586;586;597;605;605;607;610;612;612;613;620;623;625;629;645;648;650;650;650;645;638;638;638;645;647;647;647;647;647;647;674;695;696;696;696;695;681;681;696;701;737;814;880;880;880;860;828;799;791;764;737;735;723;723;723;722;722;722;722;723;728;733;733;733;711;705;683;677;672;671;669;668;662;641;641;639;634;630;630;612;606;606;609;606;606;590;585;578;578;575;575;560;560;556;551;549;548;547;538;536;535;520;520;517;515;513;509;503;489;489;490;490;490;490;485;479;465;465;465;468;468;468;458;457;448;448;446;440;440;439;431;405;405;405;413;416;419;419;418;416;418;418;418;416;408;408;408;410;410;410;403;396  
  29. [timestamp]  
  30. 793514  
  31. [logtime]  
  32. 2013:1:12:15:6:32:351  
  33. [scan]  

[]内记录属性名,下一行会记录属性的内容,这个在读取的时候是非常的方便的。到[scan]不是文件结束了,而是后面又是一大串的数据,然后继续往下记录一帧一帧的数据。
首先用MATLAB编写了一份数据解析的代码,能够读取ubh文件并显示:
[cpp]  view plain  copy
  1. %根据ubh文件的特点,进行读取  
  2.   
  3. %1 读取各个属性  
  4. % Search for number of string matches per line.  
  5. % fid = fopen('ring.ubh''rt');  
  6. fid = fopen('utm30LX.ubh''rt');  
  7. lineNumber = 0;  
  8. while feof(fid) ==0  
  9.     tline = fgetl(fid);  
  10.     switch(tline)  
  11.         case '[model]'  
  12.            model = fgetl(fid); %sscanf(fgetl(fid),'%d')  
  13.         case '[frontStep]'  
  14.             frontStep = sscanf(fgetl(fid),'%d');  
  15.         case '[minDistance]'  
  16.             minDistance = sscanf(fgetl(fid),'%d');  
  17.         case '[maxDistance]'  
  18.             maxDistance = sscanf(fgetl(fid),'%d');  
  19.          case '[motorSpeed]'  
  20.             motorSpeed = sscanf(fgetl(fid),'%d');  
  21.          case '[totalSteps]'  
  22.             totalSteps = sscanf(fgetl(fid),'%d');  
  23.          case '[captureMode]'  
  24.             captureMode = fgetl(fid);  
  25.          case '[scanMsec]'  
  26.             scanMsec = fgetl(fid);  
  27.          case '[startStep]'  
  28.             startStep = sscanf(fgetl(fid),'%d');  
  29.          case '[endStep]'  
  30.             endStep = sscanf(fgetl(fid),'%d');  
  31.          case '[serialNumber]'  
  32.             serialNumber = fgetl(fid);  
  33.          case '[timestamp]'  
  34.             frontStep = sscanf(fgetl(fid),'%d');  
  35.          case '[logtime]'  
  36.             frontStep = sscanf(fgetl(fid),'%d');  
  37.          case '[scan]'  
  38.             [rho,count] = sscanf(fgetl(fid),'%d;');  
  39.             %数据显示  
  40.             rho = rho';  
  41.             length = size(rho,2);  
  42.             theta = 1:length;  
  43.             polar(theta/4*pi/180,rho);  
  44.             pause(0.25);  
  45.            %对数据进行处理  
  46.              
  47.         otherwise  
  48.             %disp('parameter init...');         
  49.     end    
  50.     lineNumber = lineNumber+1;  
  51.     disp(lineNumber);  
  52. %     if lineNumber >100  
  53. %        break;   
  54. %     end  
  55. end  
  56. fclose(fid);   

显示效果如下:


上一篇文章中提到,激光雷达的算法最终是需要移植到嵌入式设备上的,因此还是希望用C/C++来开发,这样便于移植。只是C++读取ubh格式的文件稍微有些麻烦,而在实时应用中也是直接从激光雷达获取一帧的数据进行处理,不会去读取ubh格式的文件,这里偷个懒,直接用Matlab将ubh文件拆分成一帧一帧的数据文件,然后再由C++一帧一帧的读取。
    Matlab已经能够将数据一帧一帧的读取出来,那么稍微改动几句据可以将每帧拆分成单个的文件存储。
[cpp]  view plain  copy
  1. %将ubh文件拆分成一个一个的CSV文件  
  2. %根据ubh文件的特点,进行读取  
  3. %1 读取各个属性  
  4. % Search for number of string matches per line.  
  5. fid = fopen('utm30LX.ubh''rt');  
  6. lineNumber = 0;  
  7. fileName = 'data_0.csv';  
  8. frameCnt = 0;  
  9. while feof(fid) ==0  
  10.     tline = fgetl(fid);  
  11.     switch(tline)  
  12.         case '[model]'  
  13.            model = fgetl(fid); %sscanf(fgetl(fid),'%d')  
  14.         case '[frontStep]'  
  15.             frontStep = sscanf(fgetl(fid),'%d');  
  16.         case '[minDistance]'  
  17.             minDistance = sscanf(fgetl(fid),'%d');  
  18.         case '[maxDistance]'  
  19.             maxDistance = sscanf(fgetl(fid),'%d');  
  20.          case '[motorSpeed]'  
  21.             motorSpeed = sscanf(fgetl(fid),'%d');  
  22.          case '[totalSteps]'  
  23.             totalSteps = sscanf(fgetl(fid),'%d');  
  24.          case '[captureMode]'  
  25.             captureMode = fgetl(fid);  
  26.          case '[scanMsec]'  
  27.             scanMsec = fgetl(fid);  
  28.          case '[startStep]'  
  29.             startStep = sscanf(fgetl(fid),'%d');  
  30.          case '[endStep]'  
  31.             endStep = sscanf(fgetl(fid),'%d');  
  32.          case '[serialNumber]'  
  33.             serialNumber = fgetl(fid);  
  34.          case '[timestamp]'  
  35.             frontStep = sscanf(fgetl(fid),'%d');  
  36.          case '[logtime]'  
  37.             frontStep = sscanf(fgetl(fid),'%d');  
  38.          case '[scan]'  
  39.             [rho,count] = sscanf(fgetl(fid),'%d;');  
  40.             %将数据写入文件  
  41.             fileName =  sprintf('data_%d.csv',frameCnt);  
  42.             fidw = fopen(fileName,'w');  
  43.             fprintf(fidw,'%d,',rho);  
  44.             fclose(fidw);  
  45.             frameCnt = frameCnt + 1;  
  46.             %数据显示  
  47. %             rho = rho';  
  48. %             length = size(rho,2);  
  49. %             theta = ((1:length)/4 - 45)*pi/180;  
  50.         
  51. %             polar(theta,rho);  
  52. %             pause(0.1);  
  53.            %对数据进行处理      
  54.         otherwise  
  55.             %disp('parameter init...');         
  56.     end    
  57.     lineNumber = lineNumber+1;  
  58.     disp(lineNumber);  
  59. %     if lineNumber >100  
  60. %        break;   
  61. %     end  
  62. end  
  63. fclose(fid);   

此时已经有了一帧一帧的雷达数据,可以进行计算了。C++读取文件比较简单,不再讲,但是在进行算法实现直线一个关键的问题实现雷达数据的显示,OpenCV是进行图像处理常用开源库,里面有很多库函数激光雷达也是可以用的,因此决定使用C++ + OpenCV来开发激光雷达的算法。刚好OpenCV可以进行数据的显示。
    雷达数据显示的基本思路是将数据绘制到一张图片上进行显示。(高兴,这下到了OpenCV的老本行啦)
[cpp]  view plain  copy
  1. #pragma once  
  2. #include   
  3. #include   
  4. #include  
  5. #include  
  6. using namespace std;  
  7. #include   
  8.   
  9. static int RadarImageWdith  = 600;  
  10. static int RadarImageHeight = 600;  
  11.   
  12. class OpenRadar  
  13. {  
  14. public:  
  15.     OpenRadar(void);  
  16.     ~OpenRadar(void);  
  17.   
  18.     vector<int>RadarRho;  
  19.     //数据读取  
  20.     bool RadarRead(char *fileName);  
  21.     void CreateRadarImage(IplImage* RadarImage);  
  22.            
  23. };  

OpenRadar.cpp
[cpp]  view plain  copy
  1. #include "OpenRadar.h"  
  2. #define pi 3.141592653  
  3.   
  4. OpenRadar::OpenRadar(void)  
  5. {  
  6. }  
  7.   
  8.   
  9. OpenRadar::~OpenRadar(void)  
  10. {  
  11. }  
  12. bool OpenRadar::RadarRead(char *fileName){  
  13.     FILE* fp = NULL;  
  14.     int dis = 0;  
  15.     int totalCnt = 0;  
  16.     fp = fopen(fileName,"r");  
  17.     if (fp == NULL)  
  18.     {  
  19.         //cout<<"failed to read"<  
  20.         return false;  
  21.     }else {  
  22.         //cout<<"successed to read"<  
  23.         RadarRho.clear();  
  24.         while(!feof(fp))  
  25.         {  
  26.   
  27.             fscanf(fp, "%d, ", &dis);  
  28.             RadarRho.push_back(dis);  
  29.             //printf("%d  ", dis);  
  30.         }  
  31.         //cout<<"Total Count: "<  
  32.     }  
  33.     fclose(fp);  
  34.     return true;  
  35. }  
  36.   
  37. void OpenRadar::CreateRadarImage(IplImage* RadarImage){  
  38.     //RadarImage = cvCreateImage(cvSize(RadarImageWdith,RadarImageHeight),IPL_DEPTH_8U,1);  
  39.     cvZero(RadarImage);  
  40.     //在中心加上一个圆心  
  41.     cvCircle(RadarImage, cvPoint(RadarImageWdith/2,RadarImageHeight/2),3, CV_RGB(0,255,255), -1, 8,0);  
  42.   
  43.     int x,y;  
  44.     double theta,rho;  
  45.     unsigned char * pPixel = 0;  
  46.     int halfWidth  = RadarImageWdith/2;  
  47.     int halfHeight = RadarImageHeight/2;  
  48.     for (int i = 0; i < RadarRho.size();i++)  
  49.     {  
  50.         theta = (i/4.0 - 45)*pi/180;  
  51.         rho = RadarRho.at(i);  
  52.   
  53.         x = (int)(rho*cos(theta)/5) + halfWidth;  
  54.         y = (int)(-rho*sin(theta)/5)+ halfHeight;  
  55.   
  56.         if (x >= 0 && x < RadarImageWdith && y >= 0 && y < RadarImageHeight)  
  57.         {  
  58.             pPixel = (unsigned char*)RadarImage->imageData + y*RadarImage->widthStep + 3*x+2;  
  59.             *pPixel = 255;  
  60.         }else{  
  61.             //cout<<"x: "<  
  62.         }  
  63.     }  
  64.   
  65. }  

main.cpp
[cpp]  view plain  copy
  1. #include"OpenRadar.h"  
  2. #include   
  3. #include   
  4. #include "io.h"  
  5. using namespace std;  
  6. const int MAX_POINT_COUNT = 1200;  
  7. int Rho[MAX_POINT_COUNT] = {0};   
  8.   
  9. int main(){  
  10.     OpenRadar openRadar;  
  11.     char fileName[32] = "csv\\data_0.csv";  
  12.     int frameCnt = 0;  
  13.     char key;  
  14.     IplImage* RadarImage = cvCreateImage(cvSize(RadarImageWdith,RadarImageHeight),IPL_DEPTH_8U,3);  
  15.     while (access(fileName,0) == 0)  
  16.     {  
  17.         sprintf(fileName,"csv\\data_%d.csv",frameCnt);  
  18.         openRadar.RadarRead(fileName);  
  19.           
  20.         openRadar.CreateRadarImage(RadarImage);  
  21.         cvNamedWindow("Radar",1);  
  22.         cvShowImage("Radar",RadarImage);  
  23.         key = cvWaitKey(30);  
  24.         if (key == 27)//esc退出  
  25.         {  
  26.             break;  
  27.         }  
  28.         frameCnt++;  
  29.     }  
  30.     cvReleaseImage(&RadarImage);  
  31.     cvDestroyWindow("Radar");  
  32.     return 0;  
  33. }  
显示效果:

此时逐帧读取数据就可以连续的进行数据显示,下一步就是数据处理了~~那就看下一篇吧~~

激光雷达数据读取以及显示代码:


http://download.csdn.net/detail/renshengrumenglibing/5076486

你可能感兴趣的:(SLAM)