Review comments.

seulbinHwang · Sep 19, 2018 · 69d49a3 · 69d49a3
1 parent 4d5a6f5
commit 69d49a3
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 11 deletions.
diff --git a/voxblox/include/voxblox/integrator/esdf_integrator.h b/voxblox/include/voxblox/integrator/esdf_integrator.h
@@ -45,7 +45,8 @@ class EsdfIntegrator {
     float min_weight = 1e-6;
     // Number of buckets for the bucketed priority queue.
     int num_buckets = 20;
-    // Whether to push stuff to the queue multiple times.
+    // Whether to push stuff to the open queue multiple times, with updated
+    // distances.
     bool multi_queue = false;
     // Whether to add an outside layer of occupied voxels. Basically just sets
     // all unknown voxels in the allocated blocks to occupied.

diff --git a/voxblox/src/integrator/esdf_integrator.cc b/voxblox/src/integrator/esdf_integrator.cc
@@ -148,10 +148,6 @@ void EsdfIntegrator::updateFromTsdfBlocks(const BlockIndexList& tsdf_blocks,
         if (!incremental && config_.add_occupied_crust) {
           // Create a little crust of occupied voxels around.
           EsdfVoxel& esdf_voxel = esdf_block->getVoxelByLinearIndex(lin_index);
-          VoxelIndex voxel_index =
-              esdf_block->computeVoxelIndexFromLinearIndex(lin_index);
-          GlobalIndex global_index = getGlobalVoxelIndexFromBlockAndVoxelIndex(
-              block_index, voxel_index, voxels_per_side_);
           esdf_voxel.distance = -config_.default_distance_m;
           esdf_voxel.observed = true;
           esdf_voxel.hallucinated = true;
@@ -166,7 +162,7 @@ void EsdfIntegrator::updateFromTsdfBlocks(const BlockIndexList& tsdf_blocks,
       GlobalIndex global_index = getGlobalVoxelIndexFromBlockAndVoxelIndex(
           block_index, voxel_index, voxels_per_side_);
 
-      bool tsdf_fixed = isFixed(tsdf_voxel.distance);
+      const bool tsdf_fixed = isFixed(tsdf_voxel.distance);
       // If there was nothing there before:
       if (!esdf_voxel.observed || esdf_voxel.hallucinated) {
         // Two options: ESDF is in the fixed truncation band, or outside.
@@ -296,7 +292,7 @@ void EsdfIntegrator::processRaiseSet() {
   GlobalIndexVector neighbors;
 
   while (!raise_.empty()) {
-    GlobalIndex global_index = raise_.front();
+    const GlobalIndex global_index = raise_.front();
     raise_.pop();
 
     EsdfVoxel* voxel = esdf_layer_->getVoxelPtrByGlobalIndex(global_index);
@@ -436,7 +432,7 @@ void EsdfIntegrator::processOpenSet() {
         }
         // Final case is if the signs are different.
       } else {
-        FloatingPoint potential_distance =
+        const FloatingPoint potential_distance =
             voxel->distance - signum(voxel->distance) * distance;
         if (std::abs(potential_distance - neighbor_voxel->distance) >
             distance) {

diff --git a/voxblox/src/simulation/simulation_world.cc b/voxblox/src/simulation/simulation_world.cc
@@ -100,7 +100,11 @@ void SimulationWorld::getPointcloudFromViewpoint(
           }
         }
       }
-      if (ray_valid && !ray_intersect.array().isNaN().any()) {
+      if (ray_valid) {
+        if (ray_intersect.array().isNaN().any()) {
+          LOG(ERROR) << "Simulation ray intersect is NaN!";
+          continue;
+        }
         ptcloud->push_back(ray_intersect);
         colors->push_back(ray_color);
       }
@@ -164,7 +168,11 @@ void SimulationWorld::getNoisyPointcloudFromViewpoint(
           }
         }
       }
-      if (ray_valid && !ray_intersect.array().isNaN().any()) {
+      if (ray_valid) {
+        if (ray_intersect.array().isNaN().any()) {
+          LOG(ERROR) << "Simulation ray intersect is NaN!";
+          continue;
+        }
         // Apply noise now!
         FloatingPoint noise = getNoise(noise_sigma);
         ray_dist += noise;

diff --git a/voxblox_ros/src/tsdf_server.cc b/voxblox_ros/src/tsdf_server.cc
@@ -278,7 +278,7 @@ void TsdfServer::processPointCloudMessageAndInsert(
         icp_->runICP(tsdf_map_->getTsdfLayer(), points_C,
                      icp_corrected_transform_ * T_G_C, &T_G_C_refined);
     if (verbose_) {
-      ROS_INFO("ICP refinement performed %u successful update steps",
+      ROS_INFO("ICP refinement performed %zu successful update steps",
                num_icp_updates);
     }
     icp_corrected_transform_ = T_G_C_refined * T_G_C.inverse();