Added params as gflags

Author: smauq

console-plugins/dense-reconstruction-plugin/include/dense-reconstruction/balm/bavoxel.h

@@ -3,6 +3,7 @@
 
 #include <Eigen/Core>
 #include <aslam/common/pose-types.h>
+#include <gflags/gflags.h>
 #include <kindr/minimal/common.h>
 #include <maplab-common/threading-helpers.h>
 #include <resources-common/point-cloud.h>
@@ -19,12 +20,12 @@
       Eigen::Matrix<double, a, 1>, \
       Eigen::aligned_allocator<Eigen::Matrix<double, a, 1>>>
 
-size_t layer_limit = 3;
-float eigen_value_array[4] = {1.0 / 40, 1.0 / 40, 1.0 / 40, 1.0 / 40};
-size_t min_ps = 15;
+DECLARE_double(balm_voxel_size);
+DECLARE_uint32(balm_max_layers);
+DECLARE_uint32(balm_min_plane_points);
+DECLARE_double(balm_max_eigen_value);
 
-double voxel_size = 1;
-int win_size = 20;
+int win_size;
 
 class OctoTreeNode;
 typedef std::unordered_map<resources::VoxelPosition, OctoTreeNode*> SurfaceMap;
@@ -275,17 +276,16 @@ class OctoTreeNode {
     }
   }
 
-  bool judge_eigen(Eigen::Vector3d& normal) {
+  bool judge_eigen() {
     PointCluster covMat;
     for (const PointCluster& p : sig_tran) {
       covMat += p;
     }
 
     Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat.cov());
     double decision = saes.eigenvalues()[0] / saes.eigenvalues()[1];
-    normal = saes.eigenvectors().col(0);
 
-    return (decision < eigen_value_array[layer]);
+    return (decision < FLAGS_balm_max_eigen_value);
   }
 
   void cut_func() {
@@ -326,21 +326,18 @@ class OctoTreeNode {
     vec_tran = std::vector<PLV(3)>();
   }
 
-  bool recut(
-      VoxHess* vox_opt, std::vector<PLV(3)>* normals,
-      resources::PointCloud* points_G) {
+  bool recut(VoxHess* vox_opt, resources::PointCloud* points_G) {
     size_t point_size = 0;
     for (const PointCluster& p : sig_tran) {
       point_size += p.N;
     }
 
     const size_t num_pointclouds = index.size();
-    if (num_pointclouds < 2 || point_size <= min_ps) {
+    if (num_pointclouds < 2 || point_size <= FLAGS_balm_min_plane_points) {
       return false;
     }
 
-    Eigen::Vector3d normal;
-    if (judge_eigen(normal)) {
+    if (judge_eigen()) {
       // Push planes into visualization if requested.
       if (points_G != nullptr) {
         const float intensity = 255.0 * rand() / (RAND_MAX + 1.0f);
@@ -360,16 +357,11 @@ class OctoTreeNode {
       vec_tran = std::vector<PLV(3)>();
       sig_tran = std::vector<PointCluster>();
 
-      // Return the normal of the plane we found.
-      for (const size_t i : index) {
-        (*normals)[i].emplace_back(normal);
-      }
-
       // Push plane into optimization.
       vox_opt->push_voxel(&index, &sig_orig, &fix_point);
 
       return true;
-    } else if (layer == layer_limit) {
+    } else if (layer == FLAGS_balm_max_layers) {
       return false;
     }
 
@@ -380,7 +372,7 @@ class OctoTreeNode {
     bool keep = false;
     for (size_t i = 0; i < 8; ++i) {
       if (leaves[i] != nullptr) {
-        if (leaves[i]->recut(vox_opt, normals, points_G)) {
+        if (leaves[i]->recut(vox_opt, points_G)) {
           keep = true;
         } else {
           delete leaves[i];
@@ -454,20 +446,6 @@ class BALM2 {
   }
 
   void damping_iter(aslam::TransformationVector& x_stats, VoxHess& voxhess) {
-    /*std::vector<int> planes(x_stats.size(), 0);
-    for (size_t i = 0; i < voxhess.plvec_voxels.size(); i++) {
-      for (size_t 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);
@@ -538,7 +516,7 @@ void cut_voxel(
     const Eigen::Vector3d pvec_orig = xyz_S.col(i);
     const Eigen::Vector3d pvec_tran = xyz_G.col(i);
 
-    resources::VoxelPosition position(pvec_tran, voxel_size);
+    resources::VoxelPosition position(pvec_tran, FLAGS_balm_voxel_size);
     auto iter = surface_map.find(position);
     if (iter != surface_map.end()) {
       if (iter->second->index.back() != index) {
@@ -562,10 +540,10 @@ void cut_voxel(
       node->sig_orig.back().push(pvec_orig);
       node->sig_tran.back().push(pvec_tran);
 
-      node->voxel_center[0] = (0.5 + position.x) * voxel_size;
-      node->voxel_center[1] = (0.5 + position.y) * voxel_size;
-      node->voxel_center[2] = (0.5 + position.z) * voxel_size;
-      node->quater_length = voxel_size / 4.0;
+      node->voxel_center[0] = (0.5 + position.x) * FLAGS_balm_voxel_size;
+      node->voxel_center[1] = (0.5 + position.y) * FLAGS_balm_voxel_size;
+      node->voxel_center[2] = (0.5 + position.z) * FLAGS_balm_voxel_size;
+      node->quater_length = FLAGS_balm_voxel_size / 4.0;
       node->layer = 0;
 
       surface_map[position] = node;

console-plugins/dense-reconstruction-plugin/src/dense-reconstruction-plugin.cc

@@ -79,6 +79,20 @@ DEFINE_double(
     balm_kf_time_threshold_s, 1.0,
     "BALM force a keyframe at fixed time intervals [s].");
 
+DEFINE_double(
+    balm_voxel_size, 1.0, "BALM voxel size to use to look for planes in.");
+DEFINE_uint32(
+    balm_max_layers, 3,
+    "BALM maximum number of subdividing of a voxel when looking for a plane.");
+DEFINE_uint32(
+    balm_min_plane_points, 15,
+    "BALM minimum number of points needed when looking for a plane.");
+DEFINE_double(
+    balm_max_eigen_value, 0.05,
+    "BALM maximum least significant eigen value, when looking for a plane in a "
+    "voxel. Smaller values will result in flatter planes, but will need better "
+    "initial poses.");
+
 DEFINE_double(
     balm_vis_voxel_size, 0.2,
     "Voxel size to use for visualization when publishing pointclouds from "
@@ -837,73 +851,17 @@ DenseReconstructionPlugin::DenseReconstructionPlugin(
         }
 
         VoxHess voxhess;
-        std::vector<PLV(3)> normals(win_size);
         resources::PointCloud planes_G;
         for (auto iter = surface_map.begin(); iter != surface_map.end();) {
-          if (iter->second->recut(&voxhess, &normals, &planes_G)) {
+          if (iter->second->recut(&voxhess, &planes_G)) {
             ++iter;
           } else {
             delete iter->second;
             iter = surface_map.erase(iter);
           }
         }
 
-        // Check if all poses are well constrained.
-        size_t num_under_constrained = 0;
-        std::vector<bool> under_constrained(win_size);
-        for (size_t i = 0; i < win_size; ++i) {
-          under_constrained[i] = false;
-          // First check we have enough planes.
-          if (normals[i].size() < FLAGS_balm_min_plane_count) {
-            ++num_under_constrained;
-            under_constrained[i] = true;
-            continue;
-          }
-
-          // Perform SVD on the normals that constrain one point cloud.
-          Eigen::Matrix3Xd index_normals(3, normals[i].size());
-          // index_normals.resize(Eigen::NoChange, normals[i].size());
-          for (size_t j = 0; j < normals[i].size(); ++j) {
-            index_normals.col(j) = normals[i][j];
-          }
-
-          Eigen::JacobiSVD<Eigen::Matrix3Xd> svd;
-          svd.compute(index_normals, Eigen::ComputeThinU | Eigen::ComputeThinV);
-
-          // If one singular value is very small it means that direction is not
-          // well constrained. If the ratio of singular values is very
-          // imbalanced it means one direction is much better constrained and
-          // might cause the optimization to ignore the other ones.
-          const double sv0 = svd.singularValues()[0];
-          const double sv1 = svd.singularValues()[1];
-          const double sv2 = svd.singularValues()[2];
-          if ((sv2 < FLAGS_balm_min_normal_svd) ||
-              (sv2 < sv1 / FLAGS_balm_min_normal_svd_ratio) ||
-              (sv1 < sv0 / FLAGS_balm_min_normal_svd_ratio)) {
-            ++num_under_constrained;
-            under_constrained[i] = true;
-          }
-        }
-
-        LOG(INFO) << "Found " << num_under_constrained << " poses that are "
-                  << "under constrained. Removing them from the optimization "
-                  << "and proceeding.";
-
-        // Visualize the under constrained poses.
-        {
-          Eigen::Matrix3Xd positions_G(3, num_under_constrained);
-          int eigen_i = 0;
-          for (size_t i = 0; i < under_constrained.size(); ++i) {
-            if (under_constrained[i]) {
-              positions_G.col(eigen_i++) = poses_G_S[i].getPosition();
-            }
-          }
-          visualization::publish3DPointsAsPointCloud(
-              positions_G, visualization::kCommonRed, 0.7, FLAGS_tf_map_frame,
-              "balm_path_under_constrained");
-        }
-
-        // Publish planes computed by BALM ro rviz.
+        // Publish planes computed by BALM to rviz.
         sensor_msgs::PointCloud2 ros_planes_G;
         backend::convertPointCloudType(planes_G, &ros_planes_G);
         ros_planes_G.header.frame_id = FLAGS_tf_map_frame;