激光雷达数据采集或读取、显示、直线拟合、角点提取、圆弧拟合、位姿解算等

激光雷达数据采集或读取、显示、直线拟合、角点提取、圆弧拟合、位姿解算等

#include “Coordinate.h“


Coordinate::Coordinate（void）
{
}


Coordinate::~Coordinate（void）
{
}

//已知四条直线如何计算变换参数
void Coordinate::CalCoorTransPara（CoorTransPara &transPara
                                  LinePara W1
                                  LinePara W2 
                                  LinePara R1 
                                  LinePara R2）
{
    double theta = （ W1.Rho - R1.Rho + W2.Rho - R2.Rho ）/2;
    //double theta = （ W1.Rho - R1.Rho）;
    //求解出Xw Yw Xr Yr
    double Xw = （double）（W1.b - W2.b）/（W2.a - W1.a）;
    double Yw = W1.a*Xw + W1.b;

    double Xr = （double）（R1.b - R2.b）/（R2.a - R1.a）;
    double Yr = R1.a*Xr + R1.b;


    int Tx = （int）（Xw - cos（theta）*Xr + sin（theta）*Yr）;
    int Ty = （int）（Yw - sin（theta）*Xr - cos（theta）*Yr）;
    //交点判定，场地上的几条直线都是有角点的
    iPoint crossPoint;//交点
    iPoint vectorW1vectorR1;//向量
    //iPoint vectorR2vectorW2;
    if （W1.startPoint.x == W2.startPoint.x && W1.startPoint.y == W2.startPoint.y）
    {
        crossPoint = ipoint（W1.startPoint.xW1.startPoint.y）;
        vectorW1 = ipoint（W1.endPoint.x - W1.startPoint.x W1.endPoint.y - W1.startPoint.y）;
        //vectorW2 = ipoint（W2.endPoint.x - W2.startPoint.x W2.endPoint.y - W2.startPoint.y）;
    }else if （W1.endPoint.x == W2.startPoint.x && W1.endPoint.y == W2.startPoint.y）
    {
        crossPoint = ipoint（W1.endPoint.xW1.endPoint.y）;
        vectorW1 = ipoint（W1.startPoint.x - W1.endPoint.x W1.startPoint.y - W1.endPoint.y）;
        //vectorW2 = ipoint（W2.endPoint.x - W2.startPoint.x W2.endPoint.y - W2.startPoint.y）;
    }else if （W1.startPoint.x == W2.endPoint.x && W1.startPoint.y == W2.endPoint.y）
    {
         crossPoint = ipoint（W1.startPoint.xW1.startPoint.y）;
         vectorW1 = ipoint（W1.endPoint.x - W1.startPoint.x W1.endPoint.y - W1.startPoint.y）;
         //vectorW2 = ipoint（W2.startPoint.x - W2.endPoint.x W2.startPoint.y - W2.endPoint.y）;
    }else if （W1.endPoint.x == W2.endPoint.x && W1.endPoint.y == W2.endPoint.y）
    {
         crossPoint = ipoint（W1.endPoint.xW1.endPoint.y）;
         vectorW1 = ipoint（W1.startPoint.x - W1.endPoint.x W1.startPoint.y - W1.endPoint.y）;
         //vectorW2 = ipoint（W2.startPoint.x - W2.endPoint.x W2.startPoint.y - W2.endPoint.y）;
    }
    //将激光雷达下的两个点旋转到W系下
    transPara.theta = theta;
    transPara.Tx = Tx;
    transPara.Ty = Ty;
    iPoint R1ToW;
    //iPoint R2ToW;
    TransformCoord（transParaR1.startPointR1ToW）;
    //TransformCoord（transParaR2.startPointR2ToW）;
    vectorR1.x = R1ToW.x - crossPoint.x;
    vectorR1.y = R1ToW.y - crossPoint.y;
    //判断是否在同一侧？
    if （vectorW1.x * vectorR1.x + vectorW1.y*vectorR1.y < 0）
    {
        //旋转角度差了180度，需要调转180度
        transPara.theta = theta + PI;
        transPara.Tx = （int）（Xw - cos（transPara.theta）*Xr + sin（transPara.theta）*Yr）;
        transPara.Ty = （int）（Yw - sin（transPara.theta）*Xr - cos（transPara.theta）*Yr）;
    }else{

    }
    //数据测试

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        4520  2013-02-26 09:37  Coordinate.cpp
     文件        2556  2013-02-23 16:27  Coordinate.h
     文件       28810  2013-02-26 19:31  OpenRadar.cpp
     文件        3710  2013-02-26 18:40  OpenRadar.h
     文件       14454  2013-02-19 22:58  QSort.h
     文件       17788  2013-02-26 20:34  Radar.cpp
     文件        3895  2013-02-26 19:26  Serial.h
     文件       16690  2013-02-26 18:20  URG.h
     文件       10253  2013-02-26 19:56  WeightedFit.cpp
     文件        2329  2013-02-26 09:33  WeightedFit.h