Fix angular part of covariance matrix (#417)

Compute covariance matrix using orientation as RPY

include/mcl_3dl/pf.h

@@ -80,6 +80,10 @@ class ParticleBase
     }
     return noise;
   }
+  virtual size_t covDimension() const
+  {
+    return size();
+  }
 };
 
 template <typename T, typename FLT_TYPE = float>
@@ -303,8 +307,6 @@ class ParticleFilter
   {
     T e = expectation(pass_ratio);
     FLT_TYPE p_sum = 0;
-    std::vector<T> cov;
-    cov.resize(e.size());
 
     size_t p_num = 0;
     for (auto& p : particles_)
@@ -330,22 +332,25 @@ class ParticleFilter
       indices.resize(sample_num);
     }
 
+    std::vector<T> cov;
+    cov.resize(ie_.covDimension());
+
     p_sum = 0.0;
     for (size_t i : indices)
     {
       auto& p = particles_[i];
       p_sum += p.probability_;
-      for (size_t j = 0; j < ie_.size(); j++)
+      for (size_t j = 0; j < ie_.covDimension(); j++)
       {
-        for (size_t k = j; k < ie_.size(); k++)
+        for (size_t k = j; k < ie_.covDimension(); k++)
         {
           cov[k][j] = cov[j][k] += p.state_.covElement(e, j, k) * p.probability_;
         }
       }
     }
-    for (size_t j = 0; j < ie_.size(); j++)
+    for (size_t j = 0; j < ie_.covDimension(); j++)
     {
-      for (size_t k = 0; k < ie_.size(); k++)
+      for (size_t k = 0; k < ie_.covDimension(); k++)
       {
         cov[k][j] /= p_sum;
       }

include/mcl_3dl/state_6dof.h

@@ -155,6 +155,33 @@ class State6DOF : public mcl_3dl::pf::ParticleBase<float>
   {
     rot_.normalize();
   }
+  size_t covDimension() const override
+  {
+    return 6;
+  }
+  float covElement(const State6DOF& e, const size_t& j, const size_t& k)
+  {
+    const mcl_3dl::Vec3 exp_rpy = e.isDiff() ? e.rpy_.v_ : e.rot_.getRPY();
+    const mcl_3dl::Vec3 rpy = isDiff() ? rpy_.v_ : rot_.getRPY();
+    float val = 1.0f, diff = 0.0f;
+    for (size_t i : {j, k})
+    {
+      if (i < 3)
+      {
+        diff = (*this)[i] - e[i];
+      }
+      else
+      {
+        diff = rpy[i - 3] - exp_rpy[i - 3];
+        while (diff > M_PI)
+          diff -= 2 * M_PI;
+        while (diff < -M_PI)
+          diff += 2 * M_PI;
+      }
+      val *= diff;
+    }
+    return val;
+  }
   State6DOF()
   {
     diff_ = false;

src/mcl_3dl.cpp

@@ -704,7 +704,7 @@ class MCL3dlNode
 
     // Calculate covariance from sampled particles to reduce calculation cost on global localization.
     // Use the number of original particles or at least 10% of full particles.
-    auto cov = pf_->covariance(
+    const auto cov = pf_->covariance(
         1.0,
         std::max(
             0.1f, static_cast<float>(params_.num_particles_) / pf_->getParticleSize()));
@@ -727,9 +727,9 @@ class MCL3dlNode
 
     if (!global_localization_fix_cnt_)
     {
-      if (std::sqrt(cov[0][0] + cov[1][1]) > params_.std_warn_thresh_[0] ||
-          std::sqrt(cov[2][2]) > params_.std_warn_thresh_[1] ||
-          std::sqrt(cov[5][5]) > params_.std_warn_thresh_[2])
+      if (std::sqrt(pose.pose.covariance[0] + pose.pose.covariance[1 * 6 + 1]) > params_.std_warn_thresh_[0] ||
+          std::sqrt(pose.pose.covariance[2 * 6 + 2]) > params_.std_warn_thresh_[1] ||
+          std::sqrt(pose.pose.covariance[5 * 6 + 5]) > params_.std_warn_thresh_[2])
       {
         status_.convergence_status = mcl_3dl_msgs::Status::CONVERGENCE_STATUS_LARGE_STD_VALUE;
       }
@@ -738,8 +738,10 @@ class MCL3dlNode
     if (status_.convergence_status != mcl_3dl_msgs::Status::CONVERGENCE_STATUS_LARGE_STD_VALUE)
     {
       Vec3 fix_axis;
-      const float fix_ang = std::sqrt(cov[3][3] + cov[4][4] + cov[5][5]);
-      const float fix_dist = std::sqrt(cov[0][0] + cov[1][1] + cov[2][2]);
+      const float fix_ang = std::sqrt(
+          pose.pose.covariance[3 * 6 + 3] + pose.pose.covariance[4 * 6 + 4] + pose.pose.covariance[5 * 6 + 5]);
+      const float fix_dist = std::sqrt(
+          pose.pose.covariance[0] + pose.pose.covariance[1 * 6 + 1] + pose.pose.covariance[2 * 6 + 2]);
       ROS_DEBUG("cov: lin %0.3f ang %0.3f", fix_dist, fix_ang);
       if (fix_dist < params_.fix_dist_ &&
           fabs(fix_ang) < params_.fix_ang_)

test/src/compare_pose.cpp

@@ -33,6 +33,7 @@
 
 #include <nav_msgs/Path.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <tf2/utils.h>
 
 #include <gtest/gtest.h>
 
@@ -61,17 +62,25 @@ TEST(ComparePose, Compare)
       const float x_error = path.poses[i_path].pose.position.x - msg->pose.pose.position.x;
       const float y_error = path.poses[i_path].pose.position.y - msg->pose.pose.position.y;
       const float z_error = path.poses[i_path].pose.position.z - msg->pose.pose.position.z;
+      float yaw_error =
+          tf2::getYaw(path.poses[i_path].pose.orientation) - tf2::getYaw(msg->pose.pose.orientation);
+      while (yaw_error > M_PI)
+        yaw_error -= 2 * M_PI;
+      while (yaw_error < -M_PI)
+        yaw_error += 2 * M_PI;
       const float error = std::sqrt(std::pow(x_error, 2) + std::pow(y_error, 2) + std::pow(z_error, 2));
       const float x_sigma = std::sqrt(msg->pose.covariance[0 * 6 + 0]);
       const float y_sigma = std::sqrt(msg->pose.covariance[1 * 6 + 1]);
       const float z_sigma = std::sqrt(msg->pose.covariance[2 * 6 + 2]);
+      const float yaw_sigma = std::sqrt(msg->pose.covariance[5 * 6 + 5]);
 
       fprintf(stderr, "compare_pose[%lu/%lu]:\n",
               i_path, path.poses.size());
       fprintf(stderr, "  position error/limit=%0.3f/%0.3f\n", error, error_limit);
       fprintf(stderr, "  x error/3sigma=%0.3f/%0.3f\n", x_error, x_sigma * 3.0);
       fprintf(stderr, "  y error/3sigma=%0.3f/%0.3f\n", y_error, y_sigma * 3.0);
       fprintf(stderr, "  z error/3sigma=%0.3f/%0.3f\n", z_error, z_sigma * 3.0);
+      fprintf(stderr, "  yaw error/3sigma=%0.3f/%0.3f\n", yaw_error, yaw_sigma * 3.0);
 
       i_path++;
       if (i_path >= path.poses.size())
@@ -85,6 +94,8 @@ TEST(ComparePose, Compare)
           << "Estimated variance is too small to continue tracking. (y)";
       ASSERT_FALSE(fabs(z_error) > z_sigma * 3.0)
           << "Estimated variance is too small to continue tracking. (z)";
+      ASSERT_FALSE(fabs(yaw_error) > yaw_sigma * 3.0)
+          << "Estimated variance is too small to continue tracking. (yaw)";
     }
   };
 

test/src/test_state_6dof.cpp

@@ -27,6 +27,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <array>
 #include <cstddef>
 #include <cmath>
 
@@ -111,6 +112,62 @@ TEST(State6DOF, Adder)
   ASSERT_FLOAT_EQ(sub.rot_.w_, sub_rot.w_);
 }
 
+TEST(State6DOF, CovarianceMatrix)
+{
+  const mcl_3dl::State6DOF e(
+      mcl_3dl::Vec3(1.0, 2.0, 3.0), mcl_3dl::Quat(mcl_3dl::Vec3(0.0, 0.0, 1.0), 0.1));
+
+  ASSERT_EQ(e.covDimension(), 6);
+  std::array<float, 36> cov{};
+
+  std::mt19937 mt(42);
+  std::normal_distribution<float> x_dis(e.pos_.x_, 0.1);
+  std::normal_distribution<float> y_dis(e.pos_.y_, 0.2);
+  std::normal_distribution<float> z_dis(e.pos_.z_, 0.3);
+  std::normal_distribution<float> yaw_dis(e.rot_.getRPY().z_, 0.4);
+
+  const auto generate_state = [&mt, &x_dis, &y_dis, &z_dis, &yaw_dis](const bool use_rpy) -> mcl_3dl::State6DOF
+  {
+    if (use_rpy)
+    {
+      return mcl_3dl::State6DOF(mcl_3dl::Vec3(x_dis(mt), y_dis(mt), z_dis(mt)),
+                                mcl_3dl::Vec3(0, 0, yaw_dis(mt)));
+    }
+    else
+    {
+      return mcl_3dl::State6DOF(mcl_3dl::Vec3(x_dis(mt), y_dis(mt), z_dis(mt)),
+                                mcl_3dl::Quat(mcl_3dl::Vec3(0.0, 0.0, 1.0), yaw_dis(mt)));
+    }
+  };
+
+  const std::size_t N = 500;
+  for (std::size_t n = 0; n < N; n++)
+  {
+    mcl_3dl::State6DOF s = generate_state(n % 2 == 0);
+    for (std::size_t j = 0; j < 6; j++)
+    {
+      for (std::size_t k = j; k < 6; k++)
+      {
+        cov[k * 6 + j] = cov[j * 6 + k] += s.covElement(e, j, k);
+      }
+    }
+  }
+
+  for (size_t j = 0; j < 6; j++)
+  {
+    for (size_t k = 0; k < 6; k++)
+    {
+      cov[k * 6 + j] /= (N - 1);
+    }
+  }
+
+  const float tolerance = 1e-2;
+  ASSERT_NEAR(std::sqrt(cov[0]), 0.1, tolerance);
+  ASSERT_NEAR(std::sqrt(cov[7]), 0.2, tolerance);
+  ASSERT_NEAR(std::sqrt(cov[14]), 0.3, tolerance);
+  ASSERT_NEAR(std::sqrt(cov[35]), 0.4, tolerance);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);