激光雷达数据读取、显示、分割、直线拟合C++需配置OpenCV2.4

激光雷达数据读取、显示、分割、直线拟合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 rho = 0;
    double theta;
    double deltaTeta;
#ifdef UTM30LX
       theta = （135.0 + 90）*pi/180;
       deltaTeta = -0.25*pi/180;
#elif defined URG04LX
    theta = （120.0 + 90）*pi/180;
    deltaTeta = -0.36*pi/180;
#endif

    int totalCnt = 0;
    fp = fopen（fileName“r“）;
    if （fp == NULL）
    {
        //cout<<“failed to read“<        return false;
    }else {
        //cout<<“successed to read“<        RadarRho.clear（）;
        RadarTheta.clear（）;

        while（!feof（fp））
        {

            fscanf（fp “%d “ &rho）;
            RadarRho.push_back（rho）;
            RadarTheta.push_back（theta）;
            theta += deltaTeta;
            //printf（“%d  “ rho）;
        }
        //cout<<“Total Count: “<        //
    }
    fclose（fp）;
    return true;
}

void OpenRadar::CreateRadarImage（IplImage* RadarImagevector& RadarRhovector& RadarTheta）{
    //RadarImage = cvCreateImage（cvSize（RadarImageWdithRadarImageHeight）IPL_DEPTH_8U1）;
    cvZero（RadarImage）;
    //在中心加上一个圆心
    //int halfWidth  = RadarImageWdith/2;
    //int halfHeight = RadarImageHeight/2;
    int dx =  RadarImageWdith/2;
    int dy =  RadarImageHeight*3/4;

    cvCircle（RadarImage cvPoint（dxdy）3 CV_RGB（0255255） -1 80）;

    int xy;
    double thetarho;
    unsigned char * pPixel = 0;
    


    //颜色
    int colorIndex = 0colorRGB;
    int R = 255G = 0B = 0;
    int pointCnt = 0;
  

    for （int i = 0; i < RadarRho.size（）;i++）
    {
        //theta = （pointCnt/4.0 - 45）*pi/180;
        theta = RadarTheta.at（i）;
        rho = RadarRho.at（i）;
        if （rho < 0）
        {
           //雷达数据断点标志
            colorRGB = usualColor[colorIndex];
            R = colorRGB/65536;
            G = （colorRGB%65536）/256;
            B = colorRGB%256; 
            colorIndex = （colorIndex + 1）%10;
        }else {
            pointCnt++;
        }

        x = （int）（rho*cos（theta）/10） + dx;
        y = （int）（-rho*sin（theta）/10）+ dy;

        if （x >= 0 && x < RadarImageWdith && y >= 0 && y < RadarImageHeight）
        {
            pPixel = （unsigned char*）RadarImage->imageData + y*RadarImage->widthStep + 3*x;
            pPixel[0] = B;
            pPixel[1] = G;
            pPixel[2] = R;
        }else{
            //cout<<“x: “<        }
    }

}

int OpenRadar::BreakRadarRho（）{
    int breakCnt = 0;
    int rho = 0;
    
    int lastRho = RadarRho.at（0）;
    double theta = RadarTheta.at（0）;
    int dis = 0;
    int Dmax = 50;

    BreakedRadarRho.clear（）;
    BreakedRadarTheta.clear（）;

    BreakedRadarRho.push_back（lastRho）;
    BreakedRadarTheta.push_back（theta）;

    for （int i = 1; i< RadarRho.si

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2013-02-20 21:52  OpenRadar2.0完成直线拟合\
     文件       10432  2013-02-20 21:16  OpenRadar2.0完成直线拟合\OpenRadar.cpp
     文件        1949  2013-02-20 21:45  OpenRadar2.0完成直线拟合\OpenRadar.h
     文件       14454  2013-02-19 22:58  OpenRadar2.0完成直线拟合\QSort.h
     文件        1798  2013-02-20 21:50  OpenRadar2.0完成直线拟合\Radar.cpp
     文件        5311  2013-02-20 21:41  OpenRadar2.0完成直线拟合\WeightedFit.cpp
     文件         802  2013-02-20 20:50  OpenRadar2.0完成直线拟合\WeightedFit.h
     文件       52969  2013-02-20 21:49  OpenRadar2.0完成直线拟合\激光雷达直线拟合2.jpg