獲取激光雷達的點雲數據並利用pangolin顯示

設備	型號
激光雷達	Hypersen的固態激光雷達
系統	Ubuntu16.03
安裝庫	eigen-3.3、Pangolin

顯示效果：

代碼鏈接：github

主要代碼：

bool Lidar3d::lidar_thread_func() {
    uint32_t i=0;
    uint32_t a = 0;
    bool display=true;
    double zoomout=0.01;

    //
    //新建一個窗口
    pangolin::CreateWindowAndBind("zhou",640,480);
    //啓動深度測試
    glEnable(GL_DEPTH_TEST);
    //定義投影和初始模型視圖矩陣
    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(640,480,420,420,320,240,0.2,100),
            //對應的是gluLookAt,攝像機位置,參考點位置,up vector(上向量)
            pangolin::ModelViewLookAt(0,0,10,0,0,0,pangolin::AxisNegY));
    //在窗口中創建交互式視圖
    pangolin::Handler3D handler(s_cam);
    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0,1.0,0.0,1.0,-640.0f/480.0f)
            .SetHandler(&handler);

    glClear(GL_COLOR_BUFFER_BIT );
    glClearColor(0.0f, 0.0f, 0.0f, 1.0f);

    pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.2,0.0);
    pangolin::Var<double> plan_x("menu.plan_x",0);
    pangolin::Var<double> plan_y("menu.plan_y",0);
    pangolin::Var<double> now_yaw("menu.now_yaw",0);
    pangolin::Var<double> plan_yaw("menu.plan_yaw",0);
    pangolin::Var<double> now_plan_yaw("menu.now_plan_yaw",0);
    //

    /*Init Device*/
    do
    {
        // 設置測量包類型
        HPS3D_SetMeasurePacketType(ROI_DATA_PACKET);

        connect_number = HPS3D_AutoConnectAndInitConfigDevice(handle_lidar3d);
        printf("connect_number = %d\n",connect_number);
        if(connect_number == 0){
            printf("error,connect_number = %d\n",connect_number);
            break;
        }

        // 獲取點雲數據
        HPS3D_SetPointCloudEn(true);
        HPS3D_GetPointCloudEn();

        for(i = 0;i<connect_number;i++)
        {
            //設定偏移，距離差0cm
            HPS3D_SetDistanceOffset(&handle_lidar3d[i], -0);

            HPS3D_SetOpticalEnable(&handle_lidar3d[i],true);
            // 設置數據類型
            handle_lidar3d[i].RetPacketType=FULL_ROI_PACKET;
            // 單次測量
            handle_lidar3d[i].RunMode = RUN_SINGLE_SHOT;
        }
        a = 1;
    }while(a==0);

    while(a==1)
    {
        HPS3D_SingleMeasurement(&handle_lidar3d[0]);
        for(int i=0;i<9600;i++){
            mydata.distance[i][0]=handle_lidar3d[0].MeasureData.point_cloud_data->point_data[i].x;
            mydata.distance[i][1]=handle_lidar3d[0].MeasureData.point_cloud_data->point_data[i].y;
            mydata.distance[i][2]=handle_lidar3d[0].MeasureData.point_cloud_data->point_data[i].z;
        }
        lidar3d_plan(mydata,40,10,600);
        if(display){
            plan_x=1;
            //清除屏幕並激活要渲染到的視圖
            glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
            d_cam.Activate(s_cam);
            pangolin::glDrawColouredCube();//畫三維方塊
            //線條長度
            pangolin::glDrawAxis(2);//三維座標軸，紅——x軸，綠——y軸，藍——z軸

            // 畫map
            for (i = 0; i < 9600 ; i++) {
                glColor3f(0.0f, 1.0f, 0.0f);
                glPointSize(1);
                glBegin(GL_POINTS);
                glVertex3d(mydata.distance[i][0]*zoomout,mydata.distance[i][1]*zoomout,mydata.distance[i][2]*zoomout);
                glEnd();
            }
            //結束
            pangolin::FinishFrame();
        }
    }

}

–2020.05.26記錄記錄