fix pcl version 1.10 compatibility with ndt (#190)

This PR resolves usdot-fhwa-stol/carma-platform#1559 by updating the NDT optimization parameters to reflect the new PCL 1.9+ API changes.

ROS Noetic targets PCL v 1.10 whereas ROS Kinetic target PCL v 1.7.2
It seems that in v1.9 the API for the error epsilons was changed (see PointCloudLibrary/pcl#1724) Now the translation epsilon is actually a squared error and there is an additional epsilon which is the rotation angle error in an axis-angle rotation representation. This PR updates the translation error setting in NDT to therefore be 0.001 which makes the optimization selectivity closer to the old value (TODO I'm still evaluating if 0.001 or 0.0001 (which is 0.01*0.01) is better) (No longer investigating, see comment below). The rotation error is computed as 1.0-translation error which is the default used technique in the ICP matching algorithm (seems they didn't add a default for NDT matching). This may seem strange but actually makes sense as the input is cos(angle error). And arccos(0.999) =0.044 rad or 2.5 deg error.

This PR also removes the --sequential build argument from the docker image to hopefully speed up build times.


I performed some comparison runs using translation epsilons of 0.001 and 0.0001 respectively. Both gave similar over all NDT score values. The first value required fewer iterations overall and hit the max less often while the second value more often hit the max. Both ran within reasonable time frames though the second value did go above 100ms more frequently.

Since I did not see a noticeable improvement in NDT score, I plan to continue with 0.001 for now as it stayed under the 100ms execution time target more reliably. However, I have left a comment describing that 0.0001 can work as documentation of its feasibility.

core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp

@@ -130,8 +130,9 @@ static pcl_omp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> omp_n
 static int max_iter = 30;        // Maximum iterations
 static float ndt_res = 1.0;      // Resolution
 static double step_size = 0.1;   // Step size
-static double trans_eps = 0.01;  // Transformation epsilon
-
+static double trans_eps = 0.001;  // Transformation epsilon. In PCLv1.10 (ros noetic) this value is squared error not base epsilon 
+                                  // NOTE: A value of 0.0001 can work as well. 
+                                  // This will increase the required iteration count (and therefore execution time) but might increase accuracy. 
 static ros::Publisher predict_pose_pub;
 static geometry_msgs::PoseStamped predict_pose_msg;
 
@@ -322,18 +323,27 @@ static void param_callback(const autoware_config_msgs::ConfigNDT::ConstPtr& inpu
   if (input->trans_epsilon != trans_eps)
   {
     trans_eps = input->trans_epsilon;
+    double rot_threshold = 1.0 - trans_eps;
+
 
-    if (_method_type == MethodType::PCL_GENERIC)
+    if (_method_type == MethodType::PCL_GENERIC) {
+      ROS_INFO_STREAM("Using translation threshold of " << trans_eps);
+      ROS_INFO_STREAM("Using rotation threshold of " << rot_threshold);
       ndt.setTransformationEpsilon(trans_eps);
-    else if (_method_type == MethodType::PCL_ANH)
+      ndt.setTransformationRotationEpsilon(rot_threshold);
+    }
+    else if (_method_type == MethodType::PCL_ANH) {
       anh_ndt.setTransformationEpsilon(trans_eps);
+    }
 #ifdef CUDA_FOUND
-    else if (_method_type == MethodType::PCL_ANH_GPU)
+    else if (_method_type == MethodType::PCL_ANH_GPU) {
       anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
+    }
 #endif
 #ifdef USE_PCL_OPENMP
-    else if (_method_type == MethodType::PCL_OPENMP)
+    else if (_method_type == MethodType::PCL_OPENMP) {
       omp_ndt.setTransformationEpsilon(ndt_res);
+    }
 #endif
   }
 
@@ -460,16 +470,21 @@ static void map_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
 
     pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(map));
 
+    double rot_threshold = 1.0 - trans_eps;
+
     // Setting point cloud to be aligned to.
     if (_method_type == MethodType::PCL_GENERIC)
     {
+      ROS_INFO_STREAM("Using translation threshold of " << trans_eps);
+      ROS_INFO_STREAM("Using rotation threshold of " << rot_threshold);
       pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_ndt;
       pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
       new_ndt.setResolution(ndt_res);
       new_ndt.setInputTarget(map_ptr);
       new_ndt.setMaximumIterations(max_iter);
       new_ndt.setStepSize(step_size);
       new_ndt.setTransformationEpsilon(trans_eps);
+      new_ndt.setTransformationRotationEpsilon(rot_threshold);
 
       new_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
 

docker/install.sh

@@ -25,4 +25,4 @@ echo "Build with CUDA"
 sudo mkdir /opt/autoware.ai # Create install directory
 sudo chown carma /opt/autoware.ai # Set owner to expose permissions for build
 sudo chgrp carma /opt/autoware.ai # Set group to expose permissions for build
-AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --install-base /opt/autoware.ai/ros/install --executor sequential --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_LIBRARY_PATH=/usr/local/cuda/lib64/stubs -DCMAKE_CXX_FLAGS=-Wall -DCMAKE_C_FLAGS=-Wall
+AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --install-base /opt/autoware.ai/ros/install --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_LIBRARY_PATH=/usr/local/cuda/lib64/stubs -DCMAKE_CXX_FLAGS=-Wall -DCMAKE_C_FLAGS=-Wall