I am unable to drive LeGo-loam using pandar40

[robustfieldautonomylab]

I am unable to drive LeGo-loam using pandar40

[TixiaoShan]

try this
extern const int N_SCAN = 40;
extern const int Horizon_SCAN = 1800;
extern const float ang_res_x = 0.2;
extern const float ang_res_y = 0.33;
extern const float ang_bottom = 16;
extern const int groundScanInd = 25;

[TixiaoShan]

I just notice that the vertical resolution o pandar is not consistent, You need to make more modifications to ensure the point cloud is correctly projected onto the range image.

[hunkyu]

@TixiaoShan

[TixiaoShan]

@hunkyu
The package is optimized for Velodyne lidar. If you could share me a bag file and let me take a closer look, I may be able to point you to the right direction to customize it for your lidar.

[hunkyu]

@TixiaoShan
I am test package :
https://drive.google.com/open?id=1mYqc6BX8cwTu82p3lkWplGs9NY5oG-gu

[TixiaoShan]

@hunkyu
Thanks for sharing the bag file. I noticed that the point cloud drops a lot frames. I believe the pandar 40 frame is very different from velodyne frame. However, I can't find any useful information as this lidar is pooly documented. You can make changes in projectPointCloud() function to make it work with your lidar.

[hunkyu]

@TixiaoShan
The package I used was recorded again, using rosbag record, and there might be a lot of frame dropping. It was no problem to run with pandar_loam before, and the topic of the package was /panda_points. Do you need a complete bag? About 3.6G

[TixiaoShan]

@hunkyu
A complete bad would be great.

[hunkyu]

@TixiaoShan
https://drive.google.com/open?id=1UA7B0BfDNW5M4yQdzakJ98xOaPApuK2p
You can try it.

[TixiaoShan]

@hunkyu
I am able to get a map after making a few changes to the code. Set world frame in Rviz to "camera_init"

extern const int N_SCAN = 40;
extern const int Horizon_SCAN = 1800;
extern const float ang_res_x = 0.2;
extern const float ang_res_y = 0.33;
extern const float ang_bottom = 16;
extern const int groundScanInd = 25;

extern const float globalMapVisualizationSearchRadius = 10000.0;

void projectPointCloud(){
        float verticalAngle, horizonAngle, range;
        size_t rowIdn, columnIdn, index, cloudSize; 
        PointType thisPoint;

        cloudSize = laserCloudIn->points.size();

        for (size_t i = 0; i < cloudSize; ++i){

            thisPoint.x = laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = laserCloudIn->points[i].z;

            verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
            
            float y_res;
            if (verticalAngle >= -6 + 0.1 && verticalAngle <= 2 + 0.1)
                y_res = 0.33;
            else
                y_res = 1.0;

            rowIdn = (verticalAngle + ang_bottom) / y_res;
            if (rowIdn < 0 || rowIdn >= N_SCAN)
                continue;

            horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

            if (horizonAngle <= -90)
                columnIdn = -int(horizonAngle / ang_res_x) - 450; 
            else if (horizonAngle >= 0)
                columnIdn = -int(horizonAngle / ang_res_x) + 1350;
            else
                columnIdn = 1350 - int(horizonAngle / ang_res_x);

            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
            rangeMat.at<float>(rowIdn, columnIdn) = range;

            thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;

            index = columnIdn  + rowIdn * Horizon_SCAN;
            fullCloud->points[index] = thisPoint;

            fullInfoCloud->points[index].intensity = range;
        }
    }

However, I am still not sure the image projection is 100% correct.

[hunkyu]

@TixiaoShan awesome! There was no concern at the time that the vertical angle was a problem of inhomogeneity

[TixiaoShan]

@hunkyu
There is a bug in the code above. The projection function should look like this:

void projectPointCloud(){
        float verticalAngle, horizonAngle, range;
        size_t rowIdn, columnIdn, index, cloudSize; 
        PointType thisPoint;

        cloudSize = laserCloudIn->points.size();

        for (size_t i = 0; i < cloudSize; ++i){

            thisPoint.x = laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = laserCloudIn->points[i].z;

            verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;

            if (verticalAngle < -6)
                rowIdn = (verticalAngle + 16) / 1.0;
            else if (verticalAngle >= -6 && verticalAngle < 2)
                rowIdn = (verticalAngle + 6) / 0.33 + 10;
            else
                rowIdn = (verticalAngle - 2) / 1.0 + 34;

            if (rowIdn < 0 || rowIdn >= N_SCAN)
                continue;

            horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

            if (horizonAngle <= -90)
                columnIdn = -int(horizonAngle / ang_res_x) - 450; 
            else if (horizonAngle >= 0)
                columnIdn = -int(horizonAngle / ang_res_x) + 1350;
            else
                columnIdn = 1350 - int(horizonAngle / ang_res_x);

            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
            rangeMat.at<float>(rowIdn, columnIdn) = range;

            thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;

            index = columnIdn  + rowIdn * Horizon_SCAN;
            fullCloud->points[index] = thisPoint;

            fullInfoCloud->points[index].intensity = range;
        }
    }

The mapping result will be much better.

[hunkyu]

@TixiaoShan Set world frame in Rviz to "camera_init"