plane_determination

Author: Zale-Liu

README.md

@@ -2,6 +2,8 @@
 
 ## Efficient and Consistent Bundle Adjustment on Lidar Point Clouds
 
+**Notation**: If your initial pose error is too large in real-world datasets, the plane detection module may not find enough planes (point association) to BA optimization. Our plane determination criteria may be strict for huge initial pose errors. So before optimization, **please to check the planes** (which are displayed in our implementation). If you really cannot provide better initial poses and want to get accurate results, you can try the process of "**coarse-to-fine**", namely from **large voxel size** and **loose plane determination criteria**, to **small voxel size** and **strict plane determination criteria**, using multiple optimization with different point associations. 
+
 **BALM 2.0** is a basic and simple system to use bundle adjustment (BA) in lidar mapping. It includes three experiments in the paper. We try to keep the code as concise as possible, to avoid confusing the readers. It is notable that this package does not include the application experiments, which will be open-sourced in other projects. The paper  is available on Arxiv and more experiments details can be found in the [video](https://youtu.be/MDrIAyhQ-9E).
 
 <div align="center">

launch/benchmark_realworld.launch

@@ -1,7 +1,7 @@
 <launch>
 	<arg name="rviz" default="true" />
 
-	<param name="voxel_size" type="double" value="1" />
+	<param name="voxel_size" type="double" value="2" />
 	<param name="file_path" type="string" value="$(find balm2)" />
 	<node pkg="balm2" type="benchmark_realworld" name="benchmark_realworld" output="screen" />
 

rviz_cfg/back.rviz

@@ -8,8 +8,8 @@ Panels:
         - /full1/Autocompute Value Bounds1
         - /full__curr1/Autocompute Value Bounds1
         - /Test1/Autocompute Value Bounds1
-        - /show1
         - /show1/Autocompute Value Bounds1
+        - /cute1
       Splitter Ratio: 0.49705880880355835
     Tree Height: 865
   - Class: rviz/Selection
@@ -225,6 +225,34 @@ Visualization Manager:
       Topic: /poseAray
       Unreliable: false
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: cute
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /map_cute
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 0; 0; 0
@@ -253,25 +281,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 62.94883728027344
+      Distance: 51.05740737915039
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -11.844158172607422
-        Y: -2.3019261360168457
-        Z: -10.72812271118164
+        X: -3.909618616104126
+        Y: -3.380894660949707
+        Z: -1.017249584197998
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.3403979539871216
+      Pitch: 1.285398006439209
       Target Frame: <Fixed Frame>
-      Yaw: 1.5653976202011108
+      Yaw: 2.7353944778442383
     Saved: ~
 Window Geometry:
   Displays:

src/benchmark/bavoxel.hpp

@@ -8,7 +8,7 @@
 int layer_limit = 2;
 int layer_size[] = {30, 30, 30, 30};
 // float eigen_value_array[] = {1.0/4.0, 1.0/4.0, 1.0/4.0};
-float eigen_value_array[4] = {1.0/36, 1.0/25, 1.0/25, 1.0/25};
+float eigen_value_array[4] = {1.0/16, 1.0/16, 1.0/16, 1.0/16};
 int min_ps = 15;
 double one_three = (1.0 / 3.0);
 
@@ -665,34 +665,34 @@ class OCTO_TREE_NODE
     decision = saes.eigenvalues()[0] / saes.eigenvalues()[1];
     // decision = saes.eigenvalues()[0];
 
-    double eva0 = saes.eigenvalues()[0];
-    center += 3 * sqrt(eva0) * direct;
-    vector<PointCluster> covMats(8);
-    for(int i=0; i<win_count; i++)
-    {
-      for(Eigen::Vector3d &pvec: vec_tran[i])
-      {
-        int xyz[3] = {0, 0, 0};
-        for(int k=0; k<3; k++)
-          if(pvec[k] > center[k])
-            xyz[k] = 1;
-        int leafnum = 4*xyz[0] + 2*xyz[1] + xyz[2];
-        covMats[leafnum].push(pvec);
-      }
-    }
+    // double eva0 = saes.eigenvalues()[0];
+    // center += 3 * sqrt(eva0) * direct;
+    // vector<PointCluster> covMats(8);
+    // for(int i=0; i<win_count; i++)
+    // {
+    //   for(Eigen::Vector3d &pvec: vec_tran[i])
+    //   {
+    //     int xyz[3] = {0, 0, 0};
+    //     for(int k=0; k<3; k++)
+    //       if(pvec[k] > center[k])
+    //         xyz[k] = 1;
+    //     int leafnum = 4*xyz[0] + 2*xyz[1] + xyz[2];
+    //     covMats[leafnum].push(pvec);
+    //   }
+    // }
 
-    double ratios[2] = {1.0/(3.0*3.0), 2.0*2.0}; 
-    int num_all = 0, num_qua = 0;
-    for(int i=0; i<8; i++)
-    {
-      if(covMats[i].N < 10) continue;
-      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMats[i].cov());
-      double child_eva0 = (saes.eigenvalues()[0]);
-      if(child_eva0 > ratios[0]*eva0 && child_eva0 < ratios[1]*eva0)
-        num_qua++;
-      num_all++;
-    }
-    double prop = 1.0 * num_qua / num_all;
+    // double ratios[2] = {1.0/(3.0*3.0), 2.0*2.0}; 
+    // int num_all = 0, num_qua = 0;
+    // for(int i=0; i<8; i++)
+    // {
+    //   if(covMats[i].N < 10) continue;
+    //   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMats[i].cov());
+    //   double child_eva0 = (saes.eigenvalues()[0]);
+    //   if(child_eva0 > ratios[0]*eva0 && child_eva0 < ratios[1]*eva0)
+    //     num_qua++;
+    //   num_all++;
+    // }
+    // double prop = 1.0 * num_qua / num_all;
 
     return (decision < eigen_value_array[layer]);
     // return (decision < eigen_value_array[layer] && prop > 0.5);
@@ -826,8 +826,8 @@ class OCTO_TREE_NODE
   {
     if(octo_state != 1)
     {
-      // if(push_state != 1)
-      //   return;
+      if(push_state != 1)
+        return;
 
       PointType ap;
       ap.intensity = ref;
@@ -844,10 +844,10 @@ class OCTO_TREE_NODE
         // ap.normal_x = sqrt(value_vector[1] / value_vector[0]);
         // ap.normal_y = sqrt(value_vector[2] / value_vector[0]);
         // ap.normal_z = sqrt(value_vector[0]);
-        ap.normal_x = voxel_center[0];
-        ap.normal_y = voxel_center[1];
-        ap.normal_z = voxel_center[2];
-        ap.curvature = quater_length * 4;
+        // ap.normal_x = voxel_center[0];
+        // ap.normal_y = voxel_center[1];
+        // ap.normal_z = voxel_center[2];
+        // ap.curvature = quater_length * 4;
 
         pl_feat.push_back(ap);
       }
@@ -1032,6 +1032,22 @@ class BALM2
 
   void damping_iter(vector<IMUST> &x_stats, VOX_HESS &voxhess)
   {
+    vector<int> planes(x_stats.size(), 0);
+    for(int i=0; i<voxhess.plvec_voxels.size(); i++)
+    {
+      for(int j=0; j<voxhess.plvec_voxels[i]->size(); j++)
+        if(voxhess.plvec_voxels[i]->at(j).N != 0)
+          planes[j]++;
+    }
+    sort(planes.begin(), planes.end());
+    if(planes[0] < 20)
+    {
+      printf("Initial error too large.\n");
+      printf("Please loose plane determination criteria for more planes.\n");
+      printf("The optimization is terminated.\n");
+      exit(0);
+    }
+
     double u = 0.01, v = 2;
     Eigen::MatrixXd D(6*win_size, 6*win_size), Hess(6*win_size, 6*win_size);
     Eigen::VectorXd JacT(6*win_size), dxi(6*win_size);
@@ -1092,7 +1108,10 @@ class BALM2
       }
 
       // if(iter_stop(dxi2, 1e-4))
-      if(iter_stop(dxi, 1e-6))
+      // if(iter_stop(dxi, 1e-6))
+      //   break;
+
+      if(fabs(residual1-residual2)/residual1 < 1e-6)
         break;
     }
 

src/benchmark/benchmark_realworld.cpp

@@ -26,7 +26,7 @@ void pub_pl_func(T &pl, ros::Publisher &pub)
   pub.publish(output);
 }
 
-ros::Publisher pub_path, pub_test, pub_show;
+ros::Publisher pub_path, pub_test, pub_show, pub_cute;
 
 int read_pose(vector<double> &tims, PLM(3) &rots, PLV(3) &poss, string prename)
 {
@@ -148,6 +148,7 @@ int main(int argc, char **argv)
   pub_test = n.advertise<sensor_msgs::PointCloud2>("/map_test", 100);
   pub_path = n.advertise<sensor_msgs::PointCloud2>("/map_path", 100);
   pub_show = n.advertise<sensor_msgs::PointCloud2>("/map_show", 100);
+  pub_cute = n.advertise<sensor_msgs::PointCloud2>("/map_cute", 100);
 
   string prename, ofname;
   vector<IMUST> x_buf;
@@ -169,6 +170,11 @@ int main(int argc, char **argv)
   win_size = x_buf.size();
   printf("The size of poses: %d\n", win_size);
 
+  data_show(x_buf, pl_fulls);
+  printf("Check the point cloud with the initial poses.\n");
+  printf("If no problem, input '1' to continue or '0' to exit...\n");
+  int a; cin >> a; if(a==0) exit(0);
+
   pcl::PointCloud<PointType> pl_full, pl_surf, pl_path, pl_send;
   for(int iterCount=0; iterCount<1; iterCount++)
   { 
@@ -177,19 +183,33 @@ int main(int argc, char **argv)
     for(int i=0; i<win_size; i++)
       cut_voxel(surf_map, *pl_fulls[i], x_buf[i], i);
 
+    pcl::PointCloud<PointType> pl_send;
+    pub_pl_func(pl_send, pub_show);
+
     pcl::PointCloud<PointType> pl_cent; pl_send.clear();
     VOX_HESS voxhess;
     for(auto iter=surf_map.begin(); iter!=surf_map.end() && n.ok(); iter++)
     {
       iter->second->recut(win_size);
       iter->second->tras_opt(voxhess, win_size);
+      iter->second->tras_display(pl_send, win_size);
     }
 
-    data_show(x_buf, pl_fulls);
-    printf("Initial point cloud is published.\n");
-    printf("Input '1' to start optimization...\n");
+    pub_pl_func(pl_send, pub_cute);
+    printf("\nThe planes (point association) cut by adaptive voxelization.\n");
+    printf("If the planes are too few, the optimization will be degenerated and fail.\n");
+    printf("If no problem, input '1' to continue or '0' to exit...\n");
     int a; cin >> a; if(a==0) exit(0);
-    
+    pl_send.clear(); pub_pl_func(pl_send, pub_cute);
+
+    if(voxhess.plvec_voxels.size() < 3 * x_buf.size())
+    {
+      printf("Initial error too large.\n");
+      printf("Please loose plane determination criteria for more planes.\n");
+      printf("The optimization is terminated.\n");
+      exit(0);
+    }
+
     BALM2 opt_lsv;
     opt_lsv.damping_iter(x_buf, voxhess);
 
@@ -203,9 +223,10 @@ int main(int argc, char **argv)
     malloc_trim(0);
   }
 
+  printf("\nRefined point cloud is publishing...\n");
   malloc_trim(0);
   data_show(x_buf, pl_fulls);
-  printf("Refined point cloud is published.\n");
+  printf("\nRefined point cloud is published.\n");
 
   ros::spin();
   return 0;

src/benchmark/benchmark_virtual.cpp

@@ -449,7 +449,9 @@ class BALM2
         is_calc_hess = false;
       }
 
-      if(iter_stop(dxi, 1e-6))
+      // if(iter_stop(dxi, 1e-6))
+      //   break;
+      if(fabs((residual1-residual2))/ residual1 < 1e-6)
         break;
     }
     double tt2 = ros::Time::now().toSec();