time back particle filter

Author: yukkysaito

CMakeLists.txt

@@ -179,20 +179,14 @@ src/node.cpp
 src/mcl.cpp
 src/particle_filter.cpp
 src/xorshift128.cpp
+src/time.cpp
 src/resample_noise/normal_distribution.cpp
 src/measurement_model/lidar_measurement_model.cpp
+src/measurement_model/ndt_pose_measurement_model.cpp
 src/prediction_model/foo_prediction_model.cpp
 src/prediction_model/foo_prediction_model_node.cpp
 )
-add_executable(test_amcl_3d
-src/test.cpp
-src/mcl.cpp
-src/particle_filter.cpp
-src/xorshift128.cpp
-src/resample_noise/normal_distribution.cpp
-src/measurement_model/lidar_measurement_model.cpp
-src/prediction_model/foo_prediction_model.cpp
-)
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
@@ -202,15 +196,11 @@ src/prediction_model/foo_prediction_model.cpp
 ## Add cmake target dependencies of the executable
 ## same as for the library above
 add_dependencies(amcl_3d_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(test_amcl_3d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(amcl_3d_node
   ${catkin_LIBRARIES}
 )
-target_link_libraries(test_amcl_3d
-  ${catkin_LIBRARIES}
-)
 
 #############
 ## Install ##

include/amcl_3d/mcl.hpp

@@ -1,6 +1,6 @@
 #pragma once
-#include "amcl_3d/particle_filter_interface.hpp"
 #include "amcl_3d/type.hpp"
+#include "amcl_3d/time.hpp"
 #include "amcl_3d/particle_filter.hpp"
 #include <pcl/point_cloud.h>
 #include <pcl/kdtree/kdtree_flann.h>
@@ -20,16 +20,19 @@ class Amcl
   AmclParam getParam();
   bool setMap(const pcl::PointCloud<pcl::PointXYZ>::Ptr map);
   bool getParticles(std::shared_ptr<const Particles> &particles_ptr);
-  bool measureLidar(const pcl::PointCloud<pcl::PointXYZ>::Ptr measuement);
+  bool measureLidar(const Time &time, const pcl::PointCloud<pcl::PointXYZ>::Ptr measuement);
+  bool measureNdtPose(std::shared_ptr<const Particles> particles_ptr, const Position &position, const Quat &quat, const PoseCovariance &covariance);
   bool predict(std::shared_ptr<PredictionModelInterface> model);
-  bool setInitialPose(const Position &position);
-  bool setInitialPose(const Position &position, const Quat &quat);
+  bool predict(std::shared_ptr<PredictionModelInterface> model, const Time &time);
+  bool predict(std::shared_ptr<Particles> particles_ptr, std::shared_ptr<PredictionModelInterface> model, const Time &time);
   bool setInitialPose(const Position &position, const Quat &quat, const PoseCovariance &covariance);
   bool setInitialPose(const Position &position, const Quat &quat, const PoseCovariance &covariance, const size_t particle_num);
   State getMMSE();
   State getMAP();
 
 private:
+  bool checkResample(const MeasurementState &measurement_state);
+
   pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kd_map_ptr_;
   std::shared_ptr<ParticleFilter> pf_ptr_;
   AmclParam param_;

include/amcl_3d/measurement_model/lidar_measurement_model.hpp

@@ -14,7 +14,9 @@ class LidarMeasurementModel : public MeasurementModelInterface
                         const size_t random_sample_num,
                         /* max limit[m] to calc likelihood */ const double max_dist,
                         /* sigma[m] to calc likelihood */ const double sigma);
-  bool measure(std::shared_ptr<Particles> particles_ptr, MeasurementState &measuremnt_state) override;
+  bool measure(std::shared_ptr<const Particles> mesurement_point_particles_ptr,
+               std::shared_ptr<Particles> particles_ptr,
+               MeasurementState &measurement_state) override;
   void setKdMap(pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kd_map_ptr);
   void setMeasuement(pcl::PointCloud<pcl::PointXYZ>::Ptr measurement_ptr);
   bool isKdMap();

include/amcl_3d/measurement_model/measurement_model_interface.hpp

@@ -5,8 +5,10 @@ namespace amcl_3d
 {
 class MeasurementModelInterface
 {
-  public:
-    virtual bool measure(std::shared_ptr<Particles> particles_ptr, MeasurementState &measuremnt_state) = 0; // don't normalize weight because of augmented mcl
+public:
+  virtual bool measure(std::shared_ptr<const Particles> mesurement_point_particles_ptr,
+                       std::shared_ptr<Particles> particles_ptr,
+                       MeasurementState &measurement_state) = 0; // don't normalize weight because of augmented mcl
 };
 
 } // namespace amcl_3d

include/amcl_3d/measurement_model/ndt_pose_measurement_model.hpp

@@ -0,0 +1,20 @@
+#pragma once
+#include "amcl_3d/measurement_model/measurement_model_interface.hpp"
+
+namespace amcl_3d
+{
+class NdtPoseMeasurementModel : public MeasurementModelInterface
+{
+public:
+  NdtPoseMeasurementModel(const Position &position, const Quat &quat, const PoseCovariance &covariance);
+  bool measure(std::shared_ptr<const Particles> mesurement_point_particles_ptr,
+               std::shared_ptr<Particles> particles_ptr,
+               MeasurementState &measurement_state) override;
+
+private:
+  Position position_;
+  Quat quat_;
+  PoseCovariance covariance_;
+};
+
+} // namespace amcl_3d

include/amcl_3d/particle_filter.hpp

@@ -1,37 +1,69 @@
 #pragma once
-#include "amcl_3d/particle_filter_interface.hpp"
+#include <memory>
+#include "amcl_3d/type.hpp"
+#include "amcl_3d/time.hpp"
+#include "amcl_3d/resample_noise/resample_noise_interface.hpp"
+#include "amcl_3d/prediction_model/prediction_model_interface.hpp"
+#include "amcl_3d/measurement_model/measurement_model_interface.hpp"
 #include "amcl_3d/xorshift128.hpp"
 #include <random>
 #include <limits>
 
 namespace amcl_3d
 {
-class ParticleFilter : public ParticleFilterInterface
+class ParticleFilter
 {
+public:
+  struct NoiseGenerators
+  {
+    NoiseGenerators(std::shared_ptr<ResampleNoiseInterface> x_noise_gen,
+                    std::shared_ptr<ResampleNoiseInterface> y_noise_gen,
+                    std::shared_ptr<ResampleNoiseInterface> z_noise_gen,
+                    std::shared_ptr<ResampleNoiseInterface> roll_noise_gen,
+                    std::shared_ptr<ResampleNoiseInterface> pitch_noise_gen,
+                    std::shared_ptr<ResampleNoiseInterface> yaw_noise_gen) : x(x_noise_gen), y(y_noise_gen), z(z_noise_gen), roll(roll_noise_gen), pitch(pitch_noise_gen), yaw(yaw_noise_gen) {}
+    std::shared_ptr<ResampleNoiseInterface> x;
+    std::shared_ptr<ResampleNoiseInterface> y;
+    std::shared_ptr<ResampleNoiseInterface> z;
+    std::shared_ptr<ResampleNoiseInterface> roll;
+    std::shared_ptr<ResampleNoiseInterface> pitch;
+    std::shared_ptr<ResampleNoiseInterface> yaw;
+  };
+
 public:
   ParticleFilter();
 
   virtual ~ParticleFilter(){};
-  bool init(const Position &position, const Quat &quat, const NoiseGenerators &noise_ge, const size_t particle_num) override;
+  bool init(const Position &position, const Quat &quat, const NoiseGenerators &noise_ge, const size_t particle_num);
   bool resample(const KLDSamplingParam &param, const double random_particle_ratio,
                 const NoiseGenerators &random_particle_noise_gen); // kld resample and random noised resample
   bool resample(const size_t particle_num,
                 const double random_particle_ratio, const NoiseGenerators &random_particle_noise_gen); // random noised resample
   bool resample(const size_t particle_num);                                                            // normal resample
-  bool predict(std::shared_ptr<PredictionModelInterface> model) override;
-  bool measure(std::shared_ptr<MeasurementModelInterface> model, MeasurementState &measuremnt_state) override;
-  bool getParticles(std::shared_ptr<const Particles> &particles_ptr) override;
+                                                                                                       //  bool predict(std::shared_ptr<PredictionModelInterface> model);
+  bool predict(std::shared_ptr<PredictionModelInterface> model, const Time &time);
+  bool predict(std::shared_ptr<Particles> particles_ptr,
+               std::shared_ptr<PredictionModelInterface> model,
+               const Time &time,
+               const bool update_time = true);
+  bool measure(std::shared_ptr<MeasurementModelInterface> model, MeasurementState &measuremnt_state);
+  bool measure(std::shared_ptr<const Particles> measurement_point_particles_ptr,
+               std::shared_ptr<MeasurementModelInterface> model,
+               MeasurementState &measurement_state);
+  bool getParticles(std::shared_ptr<const Particles> &particles_ptr);
   void normalizeWeight();
   State getMMSE(); // minimum mean square error
-  State getMAP(); // maximum a posteriori
+  State getMAP();  // maximum a posteriori
   size_t getParticleNum();
   double getESS(const bool normalized = false); // effective sample size
 
 private:
   bool resample(std::shared_ptr<Particles> new_particles_ptr, const size_t particle_num,
                 const double random_particle_ratio, const NoiseGenerators &random_particle_noise_gen);
   void normalizeWeight(std::shared_ptr<Particles> particles_ptr);
-  double getNormalCDFQuantile(const double u); 
+  double getNormalCDFQuantile(const double u);
   XorShift128 rand_;
+  std::shared_ptr<Particles> particles_ptr_;
+  Time last_prediction_time_;
 };
 } // namespace amcl_3d

include/amcl_3d/particle_filter_interface.hpp

@@ -2,7 +2,6 @@
 #include "amcl_3d/prediction_model/prediction_model_interface.hpp"
 #include "amcl_3d/xorshift128.hpp"
 #include "amcl_3d/mcl.hpp"
-#include <ros/ros.h>
 
 namespace amcl_3d
 {
@@ -11,16 +10,13 @@ class FooPredictionModel : public PredictionModelInterface
 public:
   FooPredictionModel();
   FooPredictionModel(const Eigen::Vector3d &vel, const Eigen::Vector3d &omega);
-  bool predict(State &state, bool rising_edge = false, bool falling_edge = false) override;
-  bool predict(State &state, const double dt_sec);
+  bool predict(State &state, const double dt_sec) override;
   bool measumentLinearVelocity(const Eigen::Vector3d &vel);
   bool measumentAngularVelocity(const Eigen::Vector3d &omega);
   Eigen::Vector3d getLinearVelocity();
   Eigen::Vector3d getAngularVelocity();
 
 private:
-  ros::Time current_time_;
-  ros::Time last_prediction_time_;
   Eigen::Vector3d vel_;
   Eigen::Vector3d omega_;
   static XorShift128 rand_;

include/amcl_3d/prediction_model/foo_prediction_model.hpp

@@ -6,6 +6,6 @@ namespace amcl_3d
 class PredictionModelInterface
 {
 public:
-  virtual bool predict(State &state, bool rising_edge = false, bool falling_edge = false) = 0;
+  virtual bool predict(State &state, const double st_sec) = 0;
 };
 } // namespace amcl_3d

include/amcl_3d/prediction_model/prediction_model_interface.hpp

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <ros/ros.h>
+
+namespace amcl_3d
+{
+struct Time
+{
+    Time(const ros::Time &ros_time);
+    Time();
+    ros::Time toROSTime();
+    static Time fromROSTime(const ros::Time &ros_time);
+    static double getDiff(const Time &start, const Time &end);
+    static Time getTimeNow();
+    unsigned int sec;
+    unsigned int nsec;
+};
+
+} // namespace amcl_3d

include/amcl_3d/time.hpp

@@ -1,5 +1,6 @@
 #include "amcl_3d/mcl.hpp"
 #include "amcl_3d/measurement_model/lidar_measurement_model.hpp"
+#include "amcl_3d/measurement_model/ndt_pose_measurement_model.hpp"
 #include "amcl_3d/resample_noise/normal_distribution.hpp"
 
 namespace amcl_3d
@@ -33,8 +34,9 @@ bool Amcl::getParticles(std::shared_ptr<const Particles> &particles_ptr)
     return true;
 }
 
-bool Amcl::measureLidar(const pcl::PointCloud<pcl::PointXYZ>::Ptr measuement)
+bool Amcl::measureLidar(const Time &time, const pcl::PointCloud<pcl::PointXYZ>::Ptr measuement)
 {
+#if 0
     if (kd_map_ptr_ == nullptr || measuement == nullptr)
         return false;
     MeasurementState measurement_state;
@@ -43,8 +45,25 @@ bool Amcl::measureLidar(const pcl::PointCloud<pcl::PointXYZ>::Ptr measuement)
     const double sigma = 1.0;
     std::shared_ptr<MeasurementModelInterface> model =
         std::make_shared<LidarMeasurementModel>(kd_map_ptr_, measuement, random_sample_num, max_dist, sigma);
-    pf_ptr_->measure(model, measurement_state);
+    pf_ptr_->measure(model, time, measurement_state);
 
+    checkResample(measurement_state);
+#endif
+    return true;
+}
+
+bool Amcl::measureNdtPose(std::shared_ptr<const Particles> particles_ptr, const Position &position, const Quat &quat, const PoseCovariance &covariance)
+{
+    MeasurementState measurement_state;
+    std::shared_ptr<MeasurementModelInterface> model =
+        std::make_shared<NdtPoseMeasurementModel>(position, quat, covariance);
+    pf_ptr_->measure(particles_ptr, model, measurement_state);
+    checkResample(measurement_state);
+    return true;
+}
+
+bool Amcl::checkResample(const MeasurementState &measurement_state)
+{
     param_.augmented_mcl.w_fast =
         param_.augmented_mcl.w_fast +
         param_.augmented_mcl.alpha_fast * (measurement_state.raw_weight_avg - param_.augmented_mcl.w_fast);
@@ -78,18 +97,28 @@ bool Amcl::measureLidar(const pcl::PointCloud<pcl::PointXYZ>::Ptr measuement)
             std::make_shared<NormalDistribution>(/*avg*/ 0.0, /*var*/ param_.augmented_mcl.noise_pitch_var);
         std::shared_ptr<ResampleNoiseInterface> yaw_noise_ptr =
             std::make_shared<NormalDistribution>(/*avg*/ 0.0, /*var*/ param_.augmented_mcl.noise_yaw_var);
-        ParticleFilterInterface::NoiseGenerators
+        ParticleFilter::NoiseGenerators
             noise_gens(x_noise_ptr, y_noise_ptr, z_noise_ptr, roll_noise_ptr, pitch_noise_ptr, yaw_noise_ptr);
         // pf_ptr_->resample(pf_ptr_->getParticleNum(), 0.3, noise_gens); // random resample
         // pf_ptr_->resample(pf_ptr_->getParticleNum(), random_sampling_ratio, noise_gens); // random resample
         pf_ptr_->resample(param_.kld_sampling, random_sampling_ratio, noise_gens); // random resample and kld sample
     }
-    return true;
 }
 
 bool Amcl::predict(std::shared_ptr<PredictionModelInterface> model)
 {
-    pf_ptr_->predict(model);
+    return predict(model, Time::getTimeNow());
+}
+
+bool Amcl::predict(std::shared_ptr<PredictionModelInterface> model, const Time &time)
+{
+    pf_ptr_->predict(model, time);
+    return true;
+}
+
+bool Amcl::predict(std::shared_ptr<Particles> particles_ptr, std::shared_ptr<PredictionModelInterface> model, const Time &time)
+{
+    pf_ptr_->predict(particles_ptr, model, time);
     return true;
 }
 
@@ -112,18 +141,19 @@ bool Amcl::setInitialPose(const Position &position, const Quat &quat, const Pose
         std::make_shared<NormalDistribution>(/*avg*/ 0.0, /*var*/ covariance(4, 4));
     std::shared_ptr<ResampleNoiseInterface> yaw_noise_ptr =
         std::make_shared<NormalDistribution>(/*avg*/ 0.0, /*var*/ covariance(5, 5));
-    ParticleFilterInterface::NoiseGenerators
+    ParticleFilter::NoiseGenerators
         noise_gens(x_noise_ptr, y_noise_ptr, z_noise_ptr, roll_noise_ptr, pitch_noise_ptr, yaw_noise_ptr);
     return pf_ptr_->init(position, quat, noise_gens, particle_num);
 }
 
-State Amcl::getMMSE(){
+State Amcl::getMMSE()
+{
     return pf_ptr_->getMMSE();
 }
 
-State Amcl::getMAP(){
+State Amcl::getMAP()
+{
     return pf_ptr_->getMAP();
 }
 
-
 } // namespace amcl_3d