Merge pull request iris-ua#4 from UbiquityRobotics/feature/calc_covar

Add interface to calculate position covariance

include/lama/hybrid_pf_slam2d.h

@@ -44,6 +44,7 @@
 #include "sdm/dynamic_distance_map.h"
 #include "sdm/frequency_occupancy_map.h"
 
+#include "lama/kdtree.h"
 #include "lama/kld_sampling.h"
 #include "lama/simple_landmark2d_map.h"
 
@@ -97,6 +98,25 @@ class HybridPFSlam2D {
 
     };
 
+    struct Cluster {
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        // Number of samples in the cluster
+        int32_t count = 0;
+        // The weight of the cluster
+        double weight = 0.0;
+
+        // Pose of the cluster
+        Pose2D pose;
+
+        // Covariance of the pose
+        Matrix3d covar = Matrix3d::Zero();
+
+        // Workspace used to calculate the covariance
+        Vector4d m = Vector4d::Zero();
+        Matrix2d c = Matrix2d::Zero();
+    };
+
     struct Summary {
         /// When it happend.
         DynamicArray<double> timestamp;
@@ -220,6 +240,7 @@ class HybridPFSlam2D {
     size_t getBestParticleIdx() const;
 
     Pose2D getPose() const;
+    Matrix3d getCovar() const;
 
     inline const std::deque<double>& getTimestamps() const
     { return timestamps_; }
@@ -286,12 +307,18 @@ class HybridPFSlam2D {
     void normalize();
     void resample(bool reset_weight = false);
 
+    void clusterStats();
+
 private:
     Options options_;
     SolverOptions solver_options_;
 
     std::vector<Particle> particles_[2];
     uint8_t current_particle_set_;
+
+    DynamicArray<Cluster> clusters_;
+
+    KDTree      kdtree_;
     KLDSampling kld_;
 
     Pose2D odom_;

include/lama/kdtree.h

@@ -0,0 +1,116 @@
+/*
+ * IRIS Localization and Mapping (LaMa)
+ *
+ * Copyright (c) 2021, Eurico Pedrosa, University of Aveiro - Portugal
+ * Copyright (c) 2021, Ubiquity Robotics
+ * All rights reserved.
+ * License: New BSD
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include <memory>
+
+#include "types.h"
+#include "pose2d.h"
+
+namespace lama {
+
+// A k-d tree designed for 2D poses.
+//
+// This implementation is based on Players' AMCL source code.
+// https://github.com/ros-planning/navigation/blob/noetic-devel/amcl/include/amcl/pf/pf_kdtree.h
+struct KDTree {
+
+    struct Node {
+        // Node key, used for equality comparison.
+        Array<int, 3> key;
+        // Is this node a leaf?
+        bool leaf;
+        // Pivot dimension and value
+        int32_t pivot_dim;
+        double  pivot_value;
+        // Cluster label
+        int32_t cluster;
+        // Child nodes
+        Array<int32_t, 2> children;
+
+        // Two node are equal if they have the same key.
+        inline bool operator==(const Array<int32_t, 3>& other) const
+        {
+            return (key[0] == other[0]) &&
+                   (key[1] == other[1]) &&
+                   (key[2] == other[2]);
+        }
+    };
+
+    // Cell size (or resolution)
+    Array<double, 3> size;
+
+    // Pointer to the tree's root node
+    int32_t root;
+
+    // All Nodes
+    DynamicArray<Node> nodes;
+    // Current number of node
+    int32_t num_nodes;
+    // Maximum number of expected nodes
+    int32_t max_nodes;
+
+    // Number of leafs in the tree
+    int32_t num_leafs;
+    // Number of clusters in the tree
+    int32_t num_clusters;
+
+    // Constructor
+    KDTree();
+
+    // Reset the k-d tree
+    void reset(int32_t max_size);
+
+    // Insert a pose into the kdtree
+    void insert(const Pose2D& pose);
+
+    // Insert a node into de kdtree
+    int32_t insertNode(int32_t parent, int32_t node, const Array<int32_t, 3>& key);
+
+    // Recursive search of a node
+    int32_t findNode(int32_t node, const Array<int32_t, 3>& key);
+
+    // Cluster the leafs in the k-d tree
+    void cluster();
+
+    // Recursively label the node
+    void clusterNode(int32_t node);
+
+    // Get the cluster label for the pose
+    int32_t getCluster(const Pose2D& pose);
+
+};
+
+}// namespace lama

include/lama/kld_sampling.h

@@ -32,6 +32,8 @@
  *
  */
 
+#pragma once
+
 #include "types.h"
 
 namespace lama {
@@ -80,6 +82,9 @@ struct KLDSampling {
     // Calculate the resample limit, i.e. the adequate number of samples).
     uint32_t resample_limit(const Array<double, 3>& sample);
 
+    // Calculate the resample limit given the number of bins k
+    uint32_t resample_limit(uint32_t k);
+
 };
 
 } // namespace lama

include/lama/landmark_pf_slam2d.h

@@ -37,6 +37,7 @@
 #include <memory>
 #include <vector>
 
+#include "lama/kdtree.h"
 #include "lama/kld_sampling.h"
 #include "lama/simple_landmark2d_map.h"
 
@@ -72,6 +73,25 @@ class LandmarkPFSlam2D {
         //DynamicArray<Pose2D> poses;
     };
 
+    struct Cluster {
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        // Number of samples in the cluster
+        int32_t count = 0;
+        // The weight of the cluster
+        double weight = 0.0;
+
+        // Pose of the cluster
+        Pose2D pose;
+
+        // Covariance of the pose
+        Matrix3d covar = Matrix3d::Zero();
+
+        // Workspace used to calculate the covariance
+        Vector4d m = Vector4d::Zero();
+        Matrix2d c = Matrix2d::Zero();
+    };
+
     // All SLAM options are in one place for easy access and passing.
     struct Options {
         Options(){}
@@ -122,6 +142,8 @@ class LandmarkPFSlam2D {
     // Get the pose of the best particle.
     Pose2D getPose() const;
 
+    Matrix3d getCovar() const;
+
     inline const std::vector<Particle>& getParticles() const
     { return particles_[current_particle_set_]; }
 
@@ -141,11 +163,17 @@ class LandmarkPFSlam2D {
     void normalize();
     void resample(bool reset_weight = true);
 
+    void clusterStats();
+
 private:
     Options options_;
 
     DynamicArray<Particle> particles_[2];
     uint8_t current_particle_set_;
+
+    DynamicArray<Cluster> clusters_;
+
+    KDTree      kdtree_;
     KLDSampling kld_;
 
     Pose2D odom_;

include/lama/pf_slam2d.h

@@ -40,6 +40,8 @@
 #include "pose2d.h"
 #include "nlls/solver.h"
 
+#include "lama/kdtree.h"
+#include "lama/kld_sampling.h"
 #include "sdm/dynamic_distance_map.h"
 #include "sdm/frequency_occupancy_map.h"
 
@@ -85,6 +87,26 @@ class PFSlam2D {
 
     };
 
+    struct Cluster {
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        // Number of samples in the cluster
+        int32_t count = 0;
+        // The weight of the cluster
+        double weight = 0.0;
+
+        // Pose of the cluster
+        Pose2D pose;
+
+        // Covariance of the pose
+        Matrix3d covar = Matrix3d::Zero();
+
+        // Workspace used to calculate the covariance
+        Vector4d m = Vector4d::Zero();
+        Matrix2d c = Matrix2d::Zero();
+    };
+
+
     struct Summary {
         /// When it happend.
         DynamicArray<double> timestamp;
@@ -133,7 +155,9 @@ class PFSlam2D {
         Options(){}
 
         /// The number of particles to use
-        uint32_t particles;
+        uint32_t particles = 20;
+        /// The number of maximum particles to use
+        uint32_t max_particles = 40;
         /// How much the rotation affects rotation.
         double srr = 0.1;
         /// How much the translation affects rotation.
@@ -201,6 +225,7 @@ class PFSlam2D {
     size_t getBestParticleIdx() const;
 
     Pose2D getPose() const;
+    Matrix3d getCovar() const;
 
     inline const std::deque<double>& getTimestamps() const
     { return timestamps_; }
@@ -257,13 +282,20 @@ class PFSlam2D {
     void normalize();
     void resample();
 
+    void clusterStats();
+
 private:
     Options options_;
     SolverOptions solver_options_;
 
     std::vector<Particle> particles_[2];
     uint8_t current_particle_set_;
 
+    DynamicArray<Cluster> clusters_;
+
+    KDTree      kdtree_;
+    KLDSampling kld_;
+
     Pose2D odom_;
     Pose2D pose_;
 

include/lama/slam2d.h

@@ -146,6 +146,9 @@ class Slam2D {
     inline Pose2D getPose() const
     { return pose_; }
 
+    inline const Matrix3d& getCovar() const
+    { return covar_; }
+
     const FrequencyOccupancyMap* getOccupancyMap() const
     { return occupancy_map_; }
 
@@ -186,6 +189,7 @@ class Slam2D {
 
     Pose2D odom_;
     Pose2D pose_;
+    Matrix3d covar_;
 
     double trans_thresh_;
     double rot_thresh_;

src/CMakeLists.txt

@@ -52,6 +52,7 @@ set(lama_SOURCES
     landmark_pf_slam2d.cpp
     hybrid_pf_slam2d.cpp
     kld_sampling.cpp
+    kdtree.cpp
 )
 
 add_library(${PROJECT_NAME} ${lama_SOURCES})