correctly adding invalid voxels for visualization.

Author: jbehley

assets/settings.cfg

@@ -1,6 +1,6 @@
 max scans: 100
 min range: 2.5
 max range: 25.0
-ignore: 0,250,251,252,253,254
+ignore: [0,250,251,252,253,254]
 
 

src/data/VoxelGrid.cpp

@@ -63,7 +63,9 @@ bool VoxelGrid::isOccluded(int32_t i, int32_t j, int32_t k) const { return occlu
 bool VoxelGrid::isFree(int32_t i, int32_t j, int32_t k) const { return occlusions_[index(i, j, k)] == -1; }
 
 bool VoxelGrid::isInvalid(int32_t i, int32_t j, int32_t k) const {
-  return (invalid_[index(i, j, k)] > -1) && (invalid_[index(i, j, k)] == index(i, j, k));
+  if (int32_t(invalid_.size()) < index(i, j, k)) return true;
+
+  return (invalid_[index(i, j, k)] > -1) && (invalid_[index(i, j, k)] != index(i, j, k));
 }
 
 void VoxelGrid::insertOcclusionLabels() {
@@ -101,6 +103,8 @@ void VoxelGrid::updateOcclusions() {
 }
 
 void VoxelGrid::updateInvalid(const Eigen::Vector3f& position) {
+  if (!occlusionsValid_) updateOcclusions();
+
   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/widget/Mainframe.cpp

@@ -323,29 +323,27 @@ void Mainframe::updateOccludedVoxels() {
 }
 
 void Mainframe::updateInvalidVoxels() {
-  if (invalidVoxels_.size() > 0) {
-    std::vector<LabeledVoxel> voxels;
+  std::vector<LabeledVoxel> voxels;
 
-    float voxelSize = pastVoxelGrid_.resolution();
-    Eigen::Vector4f offset = pastVoxelGrid_.offset();
+  float voxelSize = pastVoxelGrid_.resolution();
+  Eigen::Vector4f offset = pastVoxelGrid_.offset();
 
-    for (uint32_t x = 0; x < pastVoxelGrid_.size(0); ++x) {
-      for (uint32_t y = 0; y < pastVoxelGrid_.size(1); ++y) {
-        for (uint32_t z = 0; z < pastVoxelGrid_.size(2); ++z) {
-          if (!pastVoxelGrid_.isInvalid(x, y, z)) continue;
+  for (uint32_t x = 0; x < pastVoxelGrid_.size(0); ++x) {
+    for (uint32_t y = 0; y < pastVoxelGrid_.size(1); ++y) {
+      for (uint32_t z = 0; z < pastVoxelGrid_.size(2); ++z) {
+        if (!pastVoxelGrid_.isInvalid(x, y, z)) continue;
 
-          LabeledVoxel lv;
-          Eigen::Vector4f pos =
-              offset + Eigen::Vector4f(x * voxelSize + 0.1, y * voxelSize + 0.1, z * voxelSize + 0.1, 0.0f);
-          lv.position = vec3(pos.x(), pos.y(), pos.z());
-          lv.label = 11;
-          voxels.push_back(lv);
-        }
+        LabeledVoxel lv;
+        Eigen::Vector4f pos =
+            offset + Eigen::Vector4f(x * voxelSize + 0.1, y * voxelSize + 0.1, z * voxelSize + 0.1, 0.0f);
+        lv.position = vec3(pos.x(), pos.y(), pos.z());
+        lv.label = 11;
+        voxels.push_back(lv);
       }
     }
-
-    ui.mViewportXYZ->setInvalidVoxels(voxels);
   }
+
+  ui.mViewportXYZ->setInvalidVoxels(voxels);
 }
 
 void Mainframe::extractLabeledVoxels(const VoxelGrid& grid, std::vector<LabeledVoxel>& labeledVoxels) {