world coordinates float => double

Turn all world coordinates from float into double.
Float precision is not good enough with 32-bit keys when operating
nearer the edge of the map.
Double precision is actually faster on most modern processors.

Fix broken test cases by this change, due to differences in float
rounding giving different OcTreeKey's. Move such coordinates away
from the resolution boundary. Relax some tree size constraints for
radially constructed trees to allow for small variations in compilers
and architecture. Use FLOAT_EQ where appropriate in test cases.

Also, fix the ray test to not project a ray from the center of the
world to the edge, which on a 32-bit map is a long way off. Now that
there is double precision, adjust the coordinates from the edge of
the world slightly towards the center and test casting the ray into
the boundary from near the boundary, resulting in a test with
reasonable runtime.

Finally, fix the test macros to keep order of operations by adding
parenthesis around the macro arguments.

octomap/include/octomap/ColorOcTree.h

@@ -129,8 +129,8 @@ namespace octomap {
     ColorOcTreeNode* setNodeColor(const OcTreeKey& key, uint8_t r, 
                                  uint8_t g, uint8_t b);
 
-    ColorOcTreeNode* setNodeColor(float x, float y, 
-                                 float z, uint8_t r, 
+    ColorOcTreeNode* setNodeColor(double x, double y, 
+                                 double z, uint8_t r, 
                                  uint8_t g, uint8_t b) {
       OcTreeKey key;
       if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
@@ -141,8 +141,8 @@ namespace octomap {
     ColorOcTreeNode* averageNodeColor(const OcTreeKey& key, uint8_t r, 
                                   uint8_t g, uint8_t b);
 
-    ColorOcTreeNode* averageNodeColor(float x, float y, 
-                                      float z, uint8_t r, 
+    ColorOcTreeNode* averageNodeColor(double x, double y, 
+                                      double z, uint8_t r, 
                                       uint8_t g, uint8_t b) {
       OcTreeKey key;
       if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;
@@ -153,8 +153,8 @@ namespace octomap {
     ColorOcTreeNode* integrateNodeColor(const OcTreeKey& key, uint8_t r, 
                                   uint8_t g, uint8_t b);
 
-    ColorOcTreeNode* integrateNodeColor(float x, float y, 
-                                      float z, uint8_t r, 
+    ColorOcTreeNode* integrateNodeColor(double x, double y, 
+                                      double z, uint8_t r, 
                                       uint8_t g, uint8_t b) {
       OcTreeKey key;
       if (!this->coordToKeyChecked(point3d(x,y,z), key)) return NULL;

octomap/include/octomap/MapCollection.h

@@ -54,7 +54,7 @@ namespace octomap {
     MAPNODE* queryNode(const point3d& p);
 
     bool isOccupied(const point3d& p) const;
-    bool isOccupied(float x, float y, float z) const;
+    bool isOccupied(double x, double y, double z) const;
 
     double getOccupancy(const point3d& p);
 

octomap/include/octomap/MapCollection.hxx

@@ -93,7 +93,7 @@ namespace octomap {
       std::string poseStr;
       ok = readTagValue("MAPNODEPOSE", infile, &poseStr);
       std::istringstream poseStream(poseStr);
-      float x,y,z;
+      double x,y,z;
       poseStream >> x >> y >> z;
       double roll,pitch,yaw;
       poseStream >> roll >> pitch >> yaw;
@@ -161,7 +161,7 @@ namespace octomap {
   }
 
   template <class MAPNODE>
-  bool MapCollection<MAPNODE>::isOccupied(float x, float y, float z) const {
+  bool MapCollection<MAPNODE>::isOccupied(double x, double y, double z) const {
     point3d q(x,y,z);
     return this->isOccupied(q);
   }

octomap/include/octomap/OcTreeBaseImpl.h

@@ -607,13 +607,13 @@ namespace octomap {
     /// converts from an addressing key at the lowest tree level into a coordinate
     /// corresponding to the key's center
     inline point3d keyToCoord(const OcTreeKey& key) const{
-      return point3d(float(keyToCoord(key[0])), float(keyToCoord(key[1])), float(keyToCoord(key[2])));
+      return point3d(keyToCoord(key[0]), keyToCoord(key[1]), keyToCoord(key[2]));
     }
 
     /// converts from an addressing key at a given depth into a coordinate
     /// corresponding to the key's center
     inline point3d keyToCoord(const OcTreeKey& key, unsigned depth) const{
-      return point3d(float(keyToCoord(key[0], depth)), float(keyToCoord(key[1], depth)), float(keyToCoord(key[2], depth)));
+      return point3d(keyToCoord(key[0], depth), keyToCoord(key[1], depth), keyToCoord(key[2], depth));
     }
 
  protected:

octomap/include/octomap/OcTreeBaseImpl.hxx

@@ -168,7 +168,7 @@ namespace octomap {
     resolution_factor = 1. / resolution;
 
     tree_center(0) = tree_center(1) = tree_center(2)
-      = (float) (((double) tree_max_val) / resolution_factor);
+      = (((double) tree_max_val) / resolution_factor);
 
     // init node size lookup table:
     sizeLookupTable.resize(tree_depth+1);
@@ -817,7 +817,7 @@ namespace octomap {
     // Initialization phase -------------------------------------------------------
 
     point3d direction = (end - origin);
-    float length = (float) direction.norm();
+    double length = direction.norm();
     direction /= length; // normalize vector
 
     int    step[3];
@@ -836,7 +836,7 @@ namespace octomap {
       if (step[i] != 0) {
         // corner point of voxel (in direction of ray)
         double voxelBorder = this->keyToCoord(current_key[i]);
-        voxelBorder += (float) (step[i] * this->resolution * 0.5);
+        voxelBorder += (step[i] * this->resolution * 0.5);
 
         tMax[i] = ( voxelBorder - origin(i) ) / direction(i);
         tDelta[i] = this->resolution / fabs( direction(i) );
@@ -1440,9 +1440,9 @@ namespace octomap {
     point3d pmin_clamped = this->keyToCoord(this->coordToKey(pmin, depth), depth);
     point3d pmax_clamped = this->keyToCoord(this->coordToKey(pmax, depth), depth);
 
-    float diff[3];
+    double diff[3];
     unsigned int steps[3];
-    float step_size = this->resolution * pow(2, tree_depth-depth);
+    double step_size = this->resolution * pow(2, tree_depth-depth);
     for (int i=0;i<3;++i) {
       diff[i] = pmax_clamped(i) - pmin_clamped(i);
       steps[i] = static_cast<unsigned int>(floor(diff[i] / step_size));

octomap/include/octomap/OccupancyOcTreeBase.hxx

@@ -209,7 +209,7 @@ namespace octomap {
           }
         } else { // user set a maxrange and length is above
           point3d direction = (p - origin).normalized ();
-          point3d new_end = origin + direction * (float) maxrange;
+          point3d new_end = origin + direction * maxrange;
           if (this->computeRayKeys(origin, new_end, *keyray)){
 #ifdef _OPENMP
             #pragma omp critical (free_insert)
@@ -1173,12 +1173,12 @@ namespace octomap {
     octomap::point3d normalZ(0, 0, 1);
 
     // One point on each plane, let them be the center for simplicity
-    octomap::point3d pointXNeg(center(0) - float(this->resolution / 2.0), center(1), center(2));
-    octomap::point3d pointXPos(center(0) + float(this->resolution / 2.0), center(1), center(2));
-    octomap::point3d pointYNeg(center(0), center(1) - float(this->resolution / 2.0), center(2));
-    octomap::point3d pointYPos(center(0), center(1) + float(this->resolution / 2.0), center(2));
-    octomap::point3d pointZNeg(center(0), center(1), center(2) - float(this->resolution / 2.0));
-    octomap::point3d pointZPos(center(0), center(1), center(2) + float(this->resolution / 2.0));
+    octomap::point3d pointXNeg(center(0) - double(this->resolution / 2.0), center(1), center(2));
+    octomap::point3d pointXPos(center(0) + double(this->resolution / 2.0), center(1), center(2));
+    octomap::point3d pointYNeg(center(0), center(1) - double(this->resolution / 2.0), center(2));
+    octomap::point3d pointYPos(center(0), center(1) + double(this->resolution / 2.0), center(2));
+    octomap::point3d pointZNeg(center(0), center(1), center(2) - double(this->resolution / 2.0));
+    octomap::point3d pointZPos(center(0), center(1), center(2) + double(this->resolution / 2.0));
 
     double lineDotNormal = 0.0;
     double d = 0.0;
@@ -1192,15 +1192,15 @@ namespace octomap {
     // if yes keep only the closest (smallest distance to sensor origin).
     if((lineDotNormal = normalX.dot(direction)) != 0.0){   // Ensure lineDotNormal is non-zero (assign and test)
       d = (pointXNeg - origin).dot(normalX) / lineDotNormal;
-      intersect = direction * float(d) + origin;
+      intersect = direction * double(d) + origin;
       if(!(intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6) ||
          intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
         outD = std::min(outD, d);
         found = true;
       }
 
       d = (pointXPos - origin).dot(normalX) / lineDotNormal;
-      intersect = direction * float(d) + origin;
+      intersect = direction * double(d) + origin;
       if(!(intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6) ||
          intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
         outD = std::min(outD, d);
@@ -1210,15 +1210,15 @@ namespace octomap {
 
     if((lineDotNormal = normalY.dot(direction)) != 0.0){   // Ensure lineDotNormal is non-zero (assign and test)
       d = (pointYNeg - origin).dot(normalY) / lineDotNormal;
-      intersect = direction * float(d) + origin;
+      intersect = direction * double(d) + origin;
       if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
          intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
         outD = std::min(outD, d);
         found = true;
       }
 
       d = (pointYPos - origin).dot(normalY) / lineDotNormal;
-      intersect = direction * float(d) + origin;
+      intersect = direction * double(d) + origin;
       if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
          intersect(2) < (pointZNeg(2) - 1e-6) || intersect(2) > (pointZPos(2) + 1e-6))){
         outD = std::min(outD, d);
@@ -1228,15 +1228,15 @@ namespace octomap {
 
     if((lineDotNormal = normalZ.dot(direction)) != 0.0){   // Ensure lineDotNormal is non-zero (assign and test)
       d = (pointZNeg - origin).dot(normalZ) / lineDotNormal;
-      intersect = direction * float(d) + origin;
+      intersect = direction * double(d) + origin;
       if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
          intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6))){
         outD = std::min(outD, d);
         found = true;
       }
 
       d = (pointZPos - origin).dot(normalZ) / lineDotNormal;
-      intersect = direction * float(d) + origin;
+      intersect = direction * double(d) + origin;
       if(!(intersect(0) < (pointXNeg(0) - 1e-6) || intersect(0) > (pointXPos(0) + 1e-6) ||
          intersect(1) < (pointYNeg(1) - 1e-6) || intersect(1) > (pointYPos(1) + 1e-6))){
         outD = std::min(outD, d);
@@ -1247,7 +1247,7 @@ namespace octomap {
     // Substract (add) a fraction to ensure no ambiguity on the starting voxel
     // Don't start on a boundary.
     if(found)
-      intersection = direction * float(outD + delta) + origin;
+      intersection = direction * double(outD + delta) + origin;
 
     return found;
   }
@@ -1274,7 +1274,7 @@ namespace octomap {
     if ((maxrange > 0) && ((end - origin).norm () > maxrange))
       {
         point3d direction = (end - origin).normalized ();
-        point3d new_end = origin + direction * (float) maxrange;
+        point3d new_end = origin + direction * maxrange;
         return integrateMissOnRay(origin, new_end,lazy_eval);
       }
     // insert complete ray

octomap/include/octomap/Pointcloud.h

@@ -58,7 +58,7 @@ namespace octomap {
     void clear();
     inline void reserve(size_t size) {points.reserve(size); }
 
-    inline void push_back(float x, float y, float z) {
+    inline void push_back(double x, double y, double z) {
       points.push_back(point3d(x,y,z));
     }
     inline void push_back(const point3d& p) {

octomap/include/octomap/math/Pose6D.h

@@ -66,7 +66,7 @@ namespace octomath {
      * its x, y, z-values and a rotation represented by its
      * Tait-Bryan angles roll, pitch, and yaw
      */
-    Pose6D(float x, float y, float z, double roll, double pitch, double yaw);
+    Pose6D(double x, double y, double z, double roll, double pitch, double yaw);
 
     Pose6D(const Pose6D& other);
     Pose6D& operator= (const Pose6D& other);
@@ -99,12 +99,12 @@ namespace octomath {
     const Quaternion& rot() const { return rotation; }
 
 
-    inline float& x() { return translation(0); }
-    inline float& y() { return translation(1); }
-    inline float& z() { return translation(2); }
-    inline const float& x() const { return translation(0); }
-    inline const float& y() const { return translation(1); }
-    inline const float& z() const { return translation(2); }
+    inline double& x() { return translation(0); }
+    inline double& y() { return translation(1); }
+    inline double& z() { return translation(2); }
+    inline const double& x() const { return translation(0); }
+    inline const double& y() const { return translation(1); }
+    inline const double& z() const { return translation(2); }
 
     inline double roll()  const {return (rotation.toEuler())(0); }
     inline double pitch() const {return (rotation.toEuler())(1); }

octomap/include/octomap/math/Quaternion.h

@@ -76,7 +76,7 @@ namespace octomath {
      * Constructs a Quaternion from four single
      * values
      */
-    Quaternion(float u, float x, float y, float z);
+    Quaternion(double u, double x, double y, double z);
 
     /*!
      * \brief Constructor
@@ -113,15 +113,15 @@ namespace octomath {
     void toRotMatrix(std::vector <double>& rot_matrix_3_3) const;
 
 
-    inline const float& operator() (unsigned int i) const { return data[i]; }
-    inline float& operator() (unsigned int i) { return data[i]; }
+    inline const double& operator() (unsigned int i) const { return data[i]; }
+    inline double& operator() (unsigned int i) { return data[i]; }
 
-    float norm () const;
+    double norm () const;
     Quaternion  normalized () const;
     Quaternion& normalize ();
 
 
-    void operator/= (float x);
+    void operator/= (double x);
     Quaternion& operator= (const Quaternion& other);
     bool operator== (const Quaternion& other) const;
 
@@ -175,23 +175,23 @@ namespace octomath {
      */
     Vector3 rotate(const Vector3 &v) const;
 
-    inline float& u() { return data[0]; }
-    inline float& x() { return data[1]; }
-    inline float& y() { return data[2]; }
-    inline float& z() { return data[3]; }
+    inline double& u() { return data[0]; }
+    inline double& x() { return data[1]; }
+    inline double& y() { return data[2]; }
+    inline double& z() { return data[3]; }
 
-    inline const float& u() const { return data[0]; }
-    inline const float& x() const { return data[1]; }
-    inline const float& y() const { return data[2]; }
-    inline const float& z() const { return data[3]; }
+    inline const double& u() const { return data[0]; }
+    inline const double& x() const { return data[1]; }
+    inline const double& y() const { return data[2]; }
+    inline const double& z() const { return data[3]; }
 
     std::istream& read(std::istream &s);
     std::ostream& write(std::ostream &s) const;
     std::istream& readBinary(std::istream &s);
     std::ostream& writeBinary(std::ostream &s) const;
 
   protected:
-    float data[4];
+    double data[4];
 
   };