bugfix. now correctly shrinking from the outside.

Author: jbehley

src/data/VoxelGrid.cpp

@@ -33,13 +33,13 @@ void VoxelGrid::clear() {
     voxels_[idx].labels.clear();
   }
 
-  for (auto idx : occluded_) {
-    voxels_[idx].count = 0;
-    voxels_[idx].labels.clear();
-  }
+  //  for (auto idx : occluded_) {
+  //    voxels_[idx].count = 0;
+  //    voxels_[idx].labels.clear();
+  //  }
 
   occupied_.clear();
-  occluded_.clear();
+  //  occluded_.clear();
 }
 
 void VoxelGrid::insert(const Eigen::Vector4f& p, uint32_t label) {
@@ -98,7 +98,9 @@ void VoxelGrid::updateOcclusions() {
   uint32_t occludedByCalls = 0;
 
   // move from outer to inner voxels.
-  for (uint32_t o = 0; o < sizex_; ++o) {
+  uint32_t num_shells = std::min<uint32_t>(sizex_, std::ceil(0.5 * sizey_));
+
+  for (uint32_t o = 0; o < num_shells; ++o) {
     uint32_t i = sizex_ - o - 1;
 
     for (uint32_t j = 0; j < sizey_; ++j) {
@@ -109,37 +111,42 @@ void VoxelGrid::updateOcclusions() {
           occludedBy_[idx] = occludedBy(i, j, k);
         }
         occlusions_[idx] = occludedBy_[idx];
-        if (occlusions_[idx] != idx) occluded_.push_back(idx);
+        //        if (occlusions_[idx] != idx) occluded_.push_back(idx);
       }
     }
 
     uint32_t j = o;
-    for (uint32_t i = 0; i < sizex_ - o; ++i) {
+    for (uint32_t i = 0; i < sizex_ - o - 1; ++i) {
       for (uint32_t k = 0; k < sizez_; ++k) {
         int32_t idx = index(i, j, k);
         if (occludedBy_[idx] == -2) {
           occludedByCalls += 1;
           occludedBy_[idx] = occludedBy(i, j, k);
         }
         occlusions_[idx] = occludedBy_[idx];
-        if (occlusions_[idx] != idx) occluded_.push_back(idx);
+        //          if (occlusions_[idx] != idx) occluded_.push_back(idx);
       }
     }
 
     j = sizey_ - o - 1;
-    for (uint32_t i = 0; i < sizex_ - o; ++i) {
+    for (uint32_t i = 0; i < sizex_ - o - 1; ++i) {
       for (uint32_t k = 0; k < sizez_; ++k) {
         int32_t idx = index(i, j, k);
         if (occludedBy_[idx] == -2) {
           occludedByCalls += 1;
           occludedBy_[idx] = occludedBy(i, j, k);
         }
         occlusions_[idx] = occludedBy_[idx];
-        if (occlusions_[idx] != idx) occluded_.push_back(idx);
+        //          if (occlusions_[idx] != idx) occluded_.push_back(idx);
       }
     }
   }
 
+  // sanity check:
+  for (uint32_t i = 0; i < occludedBy_.size(); ++i) {
+    if (occludedBy_[i] == -2) std::cout << "occludedBy == -2" << std::endl;
+  }
+
   //  for (uint32_t i = 0; i < sizex_; ++i) {
   //    for (uint32_t j = 0; j < sizey_; ++j) {
   //      for (uint32_t k = 0; k < sizez_; ++k) {
@@ -166,7 +173,9 @@ void VoxelGrid::updateInvalid(const Eigen::Vector3f& position) {
   std::fill(occludedBy_.begin(), occludedBy_.end(), -2);
 
   // move from outer to inner voxels.
-  for (uint32_t o = 0; o < sizex_; ++o) {
+  uint32_t num_shells = std::min<uint32_t>(sizex_, std::ceil(0.5 * sizey_));
+
+  for (uint32_t o = 0; o < num_shells; ++o) {
     uint32_t i = sizex_ - o - 1;
 
     for (uint32_t j = 0; j < sizey_; ++j) {
@@ -182,7 +191,7 @@ void VoxelGrid::updateInvalid(const Eigen::Vector3f& position) {
     }
 
     uint32_t j = o;
-    for (uint32_t i = 0; i < sizex_ - o; ++i) {
+    for (uint32_t i = 0; i < sizex_ - o - 1; ++i) {
       for (uint32_t k = 0; k < sizez_; ++k) {
         int32_t idx = index(i, j, k);
         if (invalid_[idx] == -1) continue;  // just skip. invalid cannot be less then -1.
@@ -195,7 +204,7 @@ void VoxelGrid::updateInvalid(const Eigen::Vector3f& position) {
     }
 
     j = sizey_ - o - 1;
-    for (uint32_t i = 0; i < sizex_ - o; ++i) {
+    for (uint32_t i = 0; i < sizex_ - o - 1; ++i) {
       for (uint32_t k = 0; k < sizez_; ++k) {
         int32_t idx = index(i, j, k);
         if (invalid_[idx] == -1) continue;  // just skip. invalid cannot be less then -1.
@@ -208,6 +217,8 @@ void VoxelGrid::updateInvalid(const Eigen::Vector3f& position) {
     }
   }
 
+
+
   //  for (uint32_t x = 0; x < sizex_; ++x) {
   //    for (uint32_t y = 0; y < sizey_; ++y) {
   //      for (uint32_t z = 0; z < sizez_; ++z) {

src/data/VoxelGrid.h

@@ -2,6 +2,7 @@
 #define SRC_DATA_VOXELGRID_H_
 
 #include <eigen3/Eigen/Dense>
+#include <iostream>
 #include <map>
 #include <vector>
 
@@ -40,7 +41,7 @@ class VoxelGrid {
   uint32_t num_elements() const { return sizex_ * sizey_ * sizez_; }
 
   /** \brief get size in specific dimension **/
-  uint32_t size(uint32_t dim) const { return (&sizex_)[std::max<uint32_t>(std::min<uint32_t>(dim, 3), 0)]; }
+  uint32_t size(uint32_t dim) const { return (&sizex_)[std::max<uint32_t>(std::min<uint32_t>(dim, 2), 0)]; }
 
   /** \brief resolutions aka sidelength of a voxel **/
   float resolution() const { return resolution_; }
@@ -100,7 +101,21 @@ class VoxelGrid {
                            (pos[2] - offset_[2]) / resolution_);
   }
 
-  inline int32_t index(int32_t i, int32_t j, int32_t k) const { return i + j * sizex_ + k * sizex_ * sizey_; }
+  inline int32_t index(int32_t i, int32_t j, int32_t k) const {
+    if (i >= sizex_ || j >= sizey_ || k >= sizez_) {
+      std::cout << sizex_ << ", " << sizey_ << ", " << sizez_ << std::endl;
+      std::cout << i << ", " << j << ", " << k << std::endl;
+
+      std::cout << "indexes to large." << std::endl;
+    }
+    if (i < 0 || j < 0 || k < 0) {
+
+      std::cout << i << ", " << j << ", " << k << std::endl;
+
+      std::cout << "indexes too small" << std::endl;
+    }
+    return i + j * sizex_ + k * sizex_ * sizey_;
+  }
 
  protected:
   float resolution_;
@@ -114,7 +129,7 @@ class VoxelGrid {
   std::vector<int32_t> occlusions_;  // filled by updateOcclusions.
   std::vector<int32_t> invalid_;
   bool occlusionsValid_{false};
-  std::vector<uint32_t> occluded_;  // filled by updateOcclusions.
+//  std::vector<uint32_t> occluded_;  // filled by updateOcclusions.
 
   std::vector<int32_t> occludedBy_;
 };

src/widget/Mainframe.cpp

@@ -243,30 +243,45 @@ void Mainframe::buildVoxelGrids() {
   if (priorPoints_.size() > 0) {
     Eigen::Matrix4f anchor_pose = priorPoints_.back()->pose;
 
+    std::cout << "fill grids..." << std::flush;
     fillVoxelGrid(anchor_pose, priorPoints_, priorLabels_, priorVoxelGrid_, config);
 
     fillVoxelGrid(anchor_pose, priorPoints_, priorLabels_, pastVoxelGrid_, config);
     fillVoxelGrid(anchor_pose, pastPoints_, pastLabels_, pastVoxelGrid_, config);
+    std::cout << "finished." << std::endl;
 
     // updating occlusions.
-    //    std::cout << "updating occlusions." << std::endl;
+    std::cout << "updating occlusions..." << std::flush;
+
     priorVoxelGrid_.updateOcclusions();
     pastVoxelGrid_.updateOcclusions();
+    std::cout << "finished." << std::endl;
 
+    std::cout << "insert occlusion labels..." << std::flush;
     priorVoxelGrid_.insertOcclusionLabels();
     pastVoxelGrid_.insertOcclusionLabels();
+    std::cout << std::endl;
 
+    std::cout << "update invalid..." << std::flush;
     for (uint32_t i = 0; i < pastPoints_.size(); ++i) {
       Eigen::Vector3f endpoint = (anchor_pose.inverse() * pastPoints_[i]->pose).col(3).head(3);
       pastVoxelGrid_.updateInvalid(endpoint);
     }
+    std::cout << "finished." << std::endl;
 
+    std::cout << "Updating visualized voxels..." << std::flush;
     // only visualization code.
     priorVoxels_.clear();
     pastVoxels_.clear();
     // extract voxels and labels.
     extractLabeledVoxels(priorVoxelGrid_, priorVoxels_);
     extractLabeledVoxels(pastVoxelGrid_, pastVoxels_);
+    std::cout << "finished." << std::endl;
+
+    std::cout << "Used " << (priorVoxelGrid_.voxels().size() * sizeof(LabeledVoxel)) / (1000 * 1000)
+              << " MB for visualization." << std::endl;
+    std::cout << "Used " << (pastVoxelGrid_.voxels().size() * sizeof(LabeledVoxel)) / (1000 * 1000)
+              << " MB for visualization." << std::endl;
 
     //    std::cout << "end" << std::endl;
   }