Add velodyne HDL (32,64) support

[laboshinl]

We need to test and merge the following forks: HDL32 https://github.com/smori/loam_velodyne/commit/774d5117ffc7662cd30ef831be9ce82b6985d5df HDL64 https://github.com/YANG-H/loam_velodyne/commit/ba1d606d2ec62d48be484c28f71d8f29a08b9ab5 [vlp16, hdl32, hdl64] should be added as a param to .launch file

[saimanoj18]

Hi, Where can I get the LOAM that supports HDL 32 and 64? Did anyone work on it?

[ClementLeBihan]

You can add the feature by editing scanRegistration.cpp by :

std::vector<int> scanStartInd(N_SCANS, 0);
  std::vector<int> scanEndInd(N_SCANS, 0);
  double timeScanCur = laserCloudMsg->header.stamp.toSec();
  pcl::PointCloud<pcl::PointXYZI> laserCloudIn;
  pcl::PointCloud<velodyne_pointcloud::PointXYZIR> laserCloudIn_vel;

  pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
  pcl::fromROSMsg(*laserCloudMsg, laserCloudIn_vel);

  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);

  int cloudSize = laserCloudIn.points.size();
  float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
  float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                        laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;

  if (endOri - startOri > 3 * M_PI) {
    endOri -= 2 * M_PI;
  } else if (endOri - startOri < M_PI) {
    endOri += 2 * M_PI;
  }
  bool halfPassed = false;
  int count = cloudSize;
  PointType point;
  std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);
  for (int i = 0; i < cloudSize; i++) {
    point.x = laserCloudIn.points[i].y;
    point.y = laserCloudIn.points[i].z;
    point.z = laserCloudIn.points[i].x;

    int scanID;
   /* float angle = atan2(point.y,sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;
    int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); 
    std::cout << angle << std::endl;
    if (roundedAngle > 0){
      scanID = roundedAngle;
    }
    else {
      scanID = roundedAngle + (N_SCANS - 1);
    }*/
    scanID = laserCloudIn_vel.points[indices[i]].ring;
    if (scanID > (N_SCANS - 1) || scanID < 0 ){
      count--;
      continue;
    }

You also have to increase the size of some array : (max number of points during a sweep)

float cloudCurvature[XXX];
int cloudSortInd[XXX];
int cloudNeighborPicked[XXX];
int cloudLabel[XXX];

Don't forget to add the velodyne_pointcloud in the CMakeLists and in the package.xml ;)

[mattforestyoung]

The code above fails to compile when copied as-is. Does this work on your system and is that all the code required? E.g. "velodyne_pointcloud" isn't defined.

On my system it fails with the following error:

/home/user/repo/workspace/src/loam_velodyne/src/scanRegistration.cpp: In function ‘void laserCloudHandler(const PointCloud2ConstPtr&)’:
/home/user/repo/workspace/src/loam_velodyne/src/scanRegistration.cpp:280:19: error: ‘velodyne_pointcloud’ was not declared in this scope
   pcl::PointCloud<velodyne_pointcloud::PointXYZIR> laserCloudIn_vel;
                   ^
/home/user/repo/workspace/src/loam_velodyne/src/scanRegistration.cpp:280:50: error: template argument 1 is invalid
   pcl::PointCloud<velodyne_pointcloud::PointXYZIR> laserCloudIn_vel;
                                                  ^
/home/user/repo/workspace/src/loam_velodyne/src/scanRegistration.cpp:280:68: error: invalid type in declaration before ‘;’ token
   pcl::PointCloud<velodyne_pointcloud::PointXYZIR> laserCloudIn_vel;
                                                                    ^
/home/user/repo/workspace/src/loam_velodyne/src/scanRegistration.cpp:283:51: error: no matching function for call to ‘fromROSMsg(const sensor_msgs::PointCloud2_<std::allocator<void> >&, int&)’
   pcl::fromROSMsg(*laserCloudMsg, laserCloudIn_vel);
                                                   ^
/home/user/repo/workspace/src/loam_velodyne/src/scanRegistration.cpp:283:51: note: candidate is:
In file included from /home/user/repo/workspace/src/loam_velodyne/src/scanRegistration.cpp:40:0:
/opt/ros/indigo/include/pcl_conversions/pcl_conversions.h:547:8: note: template<class T> void pcl::fromROSMsg(const PointCloud2&, pcl::PointCloud<PointT>&)
   void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud)
        ^
/opt/ros/indigo/include/pcl_conversions/pcl_conversions.h:547:8: note:   template argument deduction/substitution failed:
/home/user/repo/workspace/src/loam_velodyne/src/scanRegistration.cpp:283:51: note:   mismatched types ‘pcl::PointCloud<PointT>’ and ‘int’
   pcl::fromROSMsg(*laserCloudMsg, laserCloudIn_vel);
                                                   ^
/home/user/repo/workspace/src/loam_velodyne/src/scanRegistration.cpp:317:31: error: request for member ‘points’ in ‘laserCloudIn_vel’, which is of non-class type ‘int’
     scanID = laserCloudIn_vel.points[indices[i]].ring;
                               ^
make[2]: *** [loam_velodyne/CMakeFiles/scanRegistration.dir/src/scanRegistration.cpp.o] Error 1
make[1]: *** [loam_velodyne/CMakeFiles/scanRegistration.dir/all] Error 2
make: *** [all] Error 2
Invoking "make -j8 -l8" failed

[ClementLeBihan]

Please read the error ;) you forgot to add velodyne_pointcloud in the CMakeList and in your package.xml. Moreover, donc forget to build velodyne_pointcloud (http://wiki.ros.org/velodyne_pointcloud)...

[mattforestyoung]

Ah, I'm sorry. I didn't realize velodyne_pointcloud was a separate package. I did also need to include the header files to make LOAM compile properly, so these lines also need to be added at the top of scanRegistration.cpp:

#include <velodyne_pointcloud/point_types.h> 
#include <velodyne_pointcloud/rawdata.h>

Now everything compiles and I can run LOAM over a bagfile! So thank you very much for your help @ClementLeBihan , I really appreciate it!

I do have to ask though, when LOAM runs, the topic /velodyne_cloud_registered is perpendicular to the normal odom or ground plane (as shown below), and it occasionally flips upside-down when it's running... is this behaviour normal?

[ClementLeBihan]

I’m really happy you succeed ;) For the frame problem, the point cloud advertise on LOAM topics are in camera’s frame ! And the camera’s frame is z forward. So you can add a tf from camera_init to map if you want to solve this. For the flip upside down, you can change the point of view of the camera in rviz, and chose a better one manually. After that, feel free to save your rviz config to load this one each time you launch LOAM ;)

[saimanoj18]

@msy22 Can you share the code that worked for you?

[saimanoj18]

With the proposed changes, do we also have to increase N_SCANS to 32 or 64 respectively?

[ClementLeBihan]

Yes of course, to 32 or 64 ;)

[saimanoj18]

Thanks, I got it to working as well! Any thoughts on saving the high resolution map as PCD? My thoughts are to save the /after_mapped_to_init to a file and then play the bag file and create an accumulated map by projecting each point cloud with the estimated odometry.

Is there any other easier way ?

[ClementLeBihan]

You can save all the registered cloud in laserMapping.cpp in the camera_init frame ;)

        std::ostringstream titlePC;
        titlePC << boost::lexical_cast<std::string>(timeLaserCloudFullRes) << ".ply";
        pcl::io::savePLYFile(titlePC.str().c_str(), *laserCloudFullRes);

[mattforestyoung]

@saimanoj18 You can also use the command rosrun pcl_ros pointcloud_to_pcd input:=/velodyne_cloud_registered to save point clouds. Note that this will generate a new .pcd file every time the /velodyne_cloud_registered topic is updated, so you will end up with quite a few files.

@ClementLeBihan That looks useful. What file should we put that code in and where?

The topic of this issue is drifting away from its original subject (HDL-32 support) but it looks like saving the map is a common issue people are having (me included), so would it be better to document all this in a separate issue or in the readme.md file?

[lowmee]

@ClementLeBihan I have modified codes as add HDL64 support #11 I also add the velodyne_pointcloud in the CMakeLists and in the package.xml and modified N_SCANS to 64.But I got a bad result.(It plays a data set of a room)

[Xpeipei]

@lowmee hi ， Is your problem solved? I seem to have encountered the same problem with VLP-16.

[LiShuaixin]

Hi, I tried to test it with KITTI dataset, but failed. I debugged the node and found that KITTI dataset just provide 4 values (x y z intensity) without rings. Do you have any idea that how to put points into different rings as what does with VLP-16 Lidar? Looking forward to your reply!

[Xpeipei]

@ClementLeBihan Hi , I Imodified the code as you provided . However, when i use 'rosbag play xxx.bag',which is a velodyne-64 bag, it failed as follows ：

process[rviz-7]: started with pid [14273] Failed to find match for field 'ring'. [pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud! [pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud! Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'.

Do you have any idea about it ? Looking forward to your reply.

[lowmee]

@Xpeipei NO，I'm not

[zjuluolun]

@LiShuaixin HI, do you succeed testing with KITTI? I‘m trying to test LOAM with KITTI recently. To get the code usable with KITTI , I sort the points into 64 scans just like the code did but the intervals of the scans are changed according to the Velodyn64E manual. However, I havn't get a good result, the error is much more than 0.68% as it showed on the ranking list. Do you have any idea about it ?

[seanM29]

@zjuluolun hi,what result you get? I used master branch code, and test it on the KITTI, but the result is much worse, the rotation error is 0.2 deg/m, it seems the algorithm failed

[zjuluolun]

@seanM29 you should make some modify on the source code. My translation error on the test set is 0.86% now.

[saimanoj18]

@zjuluolun Can you share what changes did you make ?

[zjuluolun]

@saimanoj18 I dont know which step you have reached now, but in general, you should first sort 64 lines precisely from the lidar data to guarantee the correct feature extraction. When you evaluate your result, you should align the lidar frame to the camera frame according to the calibration file.

[CtfChan]

Hi does anyone have it working with the hdl64 and is willing to share their code?

[claydergc]

@CtfChan I am trying to reproduce the same results of the KITTI Vision Benchmark, but I am not getting it. You can checkout my modified version of loam_velodyne to run with the KITTI dataset here: https://github.com/claydergc/loam_velodyne_kitti

[jianrui1]

@zjuluolun I‘m trying to test LOAM with KITTI recently.But did not get a good result.Can you share what change did you make? Or uploading your code in your github.

[michaelczhou]

@ClementLeBihan Hi , I Imodified the code as you provided . However, when i use 'rosbag play xxx.bag',which is a velodyne-64 bag, it failed as follows ：

process[rviz-7]: started with pid [14273] Failed to find match for field 'ring'. [pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud! [pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud! Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'. Failed to find match for field 'ring'.

Do you have any idea about it ? Looking forward to your reply.

did you fix it?I met the same problem...