Integrated center_object_frame option

Author: cassinaj

source/dbot/builder/rbc_particle_filter_tracker_builder.hpp

@@ -71,6 +71,7 @@ class RbcParticleFilterTrackerBuilder
         int evaluation_count;
         double moving_average_update_rate;
         double max_kl_divergence;
+        bool center_object_frame;
     };
 
 public:
@@ -101,7 +102,8 @@ class RbcParticleFilterTrackerBuilder
             filter,
             object_model_,
             params_.evaluation_count,
-            params_.moving_average_update_rate);
+            params_.moving_average_update_rate,
+            params_.center_object_frame);        
 
         return tracker;
     }

source/dbot/builder/rms_gaussian_filter_tracker_builder.cpp

@@ -41,7 +41,10 @@ RmsGaussianFilterTrackerBuilder::build()
     auto filter = create_filter(object_model);
 
     auto tracker = std::make_shared<RmsGaussianFilterObjectTracker>(
-        filter, object_model, param_.update_rate);
+        filter,
+        object_model,
+        param_.moving_average_update_rate,
+        param_.center_object_frame);
 
     return tracker;
 }
@@ -52,14 +55,12 @@ auto RmsGaussianFilterTrackerBuilder::create_filter(
     /* ------------------------------ */
     /* - State transition model     - */
     /* ------------------------------ */
-    auto transition =
-        create_object_transition(param_.object_transition);
+    auto transition = create_object_transition(param_.object_transition);
 
     /* ------------------------------ */
     /* - Observation model          - */
     /* ------------------------------ */
-    auto sensor =
-        create_sensor(object_model, camera_data_, param_.observation);
+    auto sensor = create_sensor(object_model, camera_data_, param_.observation);
 
     /* ------------------------------ */
     /* - Quadrature                 - */
@@ -69,8 +70,8 @@ auto RmsGaussianFilterTrackerBuilder::create_filter(
     /* ------------------------------ */
     /* - Filter                     - */
     /* ------------------------------ */
-    auto filter = std::shared_ptr<Filter>(
-        new Filter(transition, sensor, quadrature));
+    auto filter =
+        std::shared_ptr<Filter>(new Filter(transition, sensor, quadrature));
 
     return filter;
 }
@@ -103,14 +104,17 @@ RmsGaussianFilterTrackerBuilder::create_object_model(
     const ObjectResourceIdentifier& ori) const
 {
     auto object_model = std::make_shared<ObjectModel>(
-        std::shared_ptr<ObjectModelLoader>(new SimpleWavefrontObjectModelLoader(ori)), true);
+        std::shared_ptr<ObjectModelLoader>(
+            new SimpleWavefrontObjectModelLoader(ori)),
+        param_.center_object_frame);
 
     return object_model;
 }
 
 auto RmsGaussianFilterTrackerBuilder::create_object_transition(
-    const ObjectTransitionBuilder<RmsGaussianFilterTrackerBuilder::State>::
-        Parameters& param) const -> Transition
+    const ObjectTransitionBuilder<
+        RmsGaussianFilterTrackerBuilder::State>::Parameters& param) const
+    -> Transition
 {
     ObjectTransitionBuilder<State> process_builder(param);
     auto process = process_builder.build_model();

source/dbot/builder/rms_gaussian_filter_tracker_builder.hpp

@@ -56,7 +56,8 @@ class RmsGaussianFilterTrackerBuilder
     struct Parameters
     {
         double ut_alpha;
-        double update_rate;
+        double moving_average_update_rate;
+        bool center_object_frame;
 
         struct Observation
         {

source/dbot/tracker/object_tracker.cpp

@@ -17,9 +17,11 @@
 namespace dbot
 {
 ObjectTracker::ObjectTracker(const std::shared_ptr<ObjectModel> &object_model,
-                             double update_rate)
+                             double update_rate,
+                             bool center_object_frame)
     : object_model_(object_model),
       update_rate_(update_rate),
+      center_object_frame_(center_object_frame),
       moving_average_(object_model_->count_parts())
 {
 }
@@ -80,6 +82,8 @@ auto ObjectTracker::track(const Obsrv& image) -> State
 auto ObjectTracker::to_center_coordinate_system(
     const ObjectTracker::State& state) -> State
 {
+    if (!center_object_frame_) return state;
+
     State centered_state = state;
     for (size_t j = 0; j < state.count(); j++)
     {
@@ -90,10 +94,13 @@ auto ObjectTracker::to_center_coordinate_system(
 
     return centered_state;
 }
+    
 
 auto ObjectTracker::to_model_coordinate_system(
     const ObjectTracker::State& state) -> State
 {
+    if (!center_object_frame_) return state;
+
     State model_state = state;
     for (size_t j = 0; j < state.count(); j++)
     {

source/dbot/tracker/object_tracker.hpp

@@ -58,7 +58,8 @@ class ObjectTracker
      *     Moving average update rate
      */
     ObjectTracker(const std::shared_ptr<ObjectModel>& object_model,
-                  double update_rate);
+                  double update_rate,
+                  bool center_object_frame);
 
     virtual ~ObjectTracker() { }
 
@@ -132,6 +133,7 @@ class ObjectTracker
     std::shared_ptr<ObjectModel> object_model_;
     State moving_average_;
     double update_rate_;
+    bool center_object_frame_;
     std::mutex mutex_;
 };
 }

source/dbot/tracker/rbc_particle_filter_object_tracker.cpp

@@ -29,8 +29,9 @@ RbcParticleFilterObjectTracker::RbcParticleFilterObjectTracker(
     const std::shared_ptr<Filter>& filter,
     const std::shared_ptr<ObjectModel>& object_model,
     int evaluation_count,
-    double update_rate)
-    : ObjectTracker(object_model, update_rate),
+    double update_rate,
+    bool center_object_frame)
+    : ObjectTracker(object_model, update_rate, center_object_frame),
       filter_(filter),
       evaluation_count_(evaluation_count)
 {
@@ -40,7 +41,6 @@ auto RbcParticleFilterObjectTracker::on_initialize(
     const std::vector<State>& initial_states) -> State
 {
     filter_->set_particles(initial_states);
-    // filter_->filter(camera_data_->depth_image_vector(), zero_input());
     filter_->resample(evaluation_count_ / object_model_->count_parts());
 
     State delta_mean = filter_->belief().mean();

source/dbot/tracker/rbc_particle_filter_object_tracker.hpp

@@ -66,7 +66,9 @@ class RbcParticleFilterObjectTracker : public ObjectTracker
         const std::shared_ptr<Filter>& filter,
         const std::shared_ptr<ObjectModel>& object_model,
         int evaluation_count,
-        double update_rate);
+        double update_rate,
+        bool center_object_frame);
+    
 
     virtual ~RbcParticleFilterObjectTracker() { }
 

source/dbot/tracker/rms_gaussian_filter_object_tracker.cpp

@@ -28,8 +28,9 @@ namespace dbot
 RmsGaussianFilterObjectTracker::RmsGaussianFilterObjectTracker(
     const std::shared_ptr<Filter>& filter,
     const std::shared_ptr<ObjectModel>& object_model,
-    double update_rate)
-    : ObjectTracker(object_model, update_rate),
+    double update_rate,
+    bool center_object_frame)
+    : ObjectTracker(object_model, update_rate, center_object_frame),
       filter_(filter),
       belief_(filter_->create_belief())
 {

source/dbot/tracker/rms_gaussian_filter_object_tracker.hpp

@@ -94,7 +94,8 @@ class RmsGaussianFilterObjectTracker : public ObjectTracker
     RmsGaussianFilterObjectTracker(
         const std::shared_ptr<Filter>& filter,
         const std::shared_ptr<ObjectModel>& object_model,
-        double update_rate);
+        double update_rate,
+        bool center_object_frame);
 
     /**
      * \brief perform a single filter step