fix BoundingBox to work when points are adde one by one and the box does not surround the origin.

VirtualRobot/BoundingBox.cpp

@@ -5,7 +5,7 @@ namespace VirtualRobot
 
     BoundingBox::BoundingBox()
     {
-        min = max = Eigen::Vector3f::Zero();
+        clear();
     }
 
     bool BoundingBox::planeGoesThrough(const MathTools::Plane& p)
@@ -38,12 +38,10 @@ namespace VirtualRobot
     {
         if (p.size() == 0)
         {
-            min = max = Eigen::Vector3f::Zero();
+            clear();
         }
         else
         {
-            min = p[0];
-            max = p[0];
             addPoints(p);
         }
     }
@@ -80,7 +78,7 @@ namespace VirtualRobot
         for (size_t i = 0; i < p.size(); i++)
         {
             addPoint(p[i]);
-        }
+        }q
     }
 
     void BoundingBox::addPoints(const BoundingBox& bbox)
@@ -93,12 +91,12 @@ namespace VirtualRobot
     {
         for (int j = 0; j < 3; j++)
         {
-            if (p(j) < min(j))
+            if (std::isnan(min(j)) || p(j) < min(j))
             {
                 min(j) = p(j);
             }
 
-            if (p(j) > max(j))
+            if (std::isnan(max(j)) || p(j) > max(j))
             {
                 max(j) = p(j);
             }
@@ -118,12 +116,22 @@ namespace VirtualRobot
 
     void BoundingBox::clear()
     {
-        min.setZero();
-        max.setZero();
+        min(0) = NAN;
+        min(1) = NAN;
+        min(2) = NAN;
+        max(0) = NAN;
+        max(1) = NAN;
+        max(2) = NAN;
     }
 
     void BoundingBox::transform(Eigen::Matrix4f& pose)
     {
+        if (std::isnan(min(0)) || std::isnan(min(1)) || std::isnan(min(2)) ||
+                std::isnan(max(0)) || std::isnan(max(1)) || std::isnan(max(2)))
+        {
+            return;
+        }
+
         Eigen::Vector3f result[8];
         std::vector<Eigen::Vector3f> result3;
         result[0] << min(0), min(1), min(2);
@@ -167,6 +175,12 @@ namespace VirtualRobot
 
     void BoundingBox::scale(Eigen::Vector3f& scaleFactor)
     {
+        if (std::isnan(min(0)) || std::isnan(min(1)) || std::isnan(min(2)) ||
+                std::isnan(max(0)) || std::isnan(max(1)) || std::isnan(max(2)))
+        {
+            return;
+        }
+
         for (int i = 0; i < 3; i++)
         {
             min(i) *= scaleFactor(i);

VirtualRobot/BoundingBox.h

@@ -81,7 +81,7 @@ namespace VirtualRobot
         //! The axis oriented maximum value
         Eigen::Vector3f getMax() const;
 
-        //! set min/max to zero.
+        //! set min/max to NAN.
         void clear();
 
         /*!