moved implementation to cpp

CMakeLists.txt

@@ -135,10 +135,14 @@ file(GLOB_RECURSE dbot_HEADERS
 # Build dbot library
 set(dbot_SOURCES
     ${dbot_SOURCE_DIR}/util/camera_data.cpp
+    ${dbot_SOURCE_DIR}/util/object_model.cpp
     ${dbot_SOURCE_DIR}/util/object_file_reader.cpp
     ${dbot_SOURCE_DIR}/util/rigid_body_renderer.cpp
     ${dbot_SOURCE_DIR}/util/object_resource_identifier.cpp
-    ${dbot_SOURCE_DIR}/tracker/rbc_particle_filter_object_tracker.cpp)
+    ${dbot_SOURCE_DIR}/util/simple_camera_data_provider.cpp
+    ${dbot_SOURCE_DIR}/util/simple_wavefront_object_loader.cpp
+    ${dbot_SOURCE_DIR}/tracker/rbc_particle_filter_object_tracker.cpp
+    ${dbot_SOURCE_DIR}/tracker/builder/rbc_particle_filter_tracker_builder.cpp)
 
 add_library(${dbot_LIBRARY} SHARED
     ${dbot_HEADERS}

include/dbot/tracker/builder/rbc_particle_filter_tracker_builder.hpp

@@ -17,96 +17,91 @@
 
 namespace dbot
 {
+
+/**
+ * \brief Represents an Rbc Particle filter based tracker builder
+ */
 class RbcParticleFilterTrackerBuilder
 {
 public:
     typedef osr::FreeFloatingRigidBodiesState<> State;
     typedef Eigen::VectorXd Input;
 
     typedef fl::StateTransitionFunction<State, State, Input> StateTransition;
-    typedef dbot::RbObservationModel<State> ObservationModel;
+    typedef RbObservationModel<State> ObservationModel;
     typedef typename ObservationModel::Observation Obsrv;
 
-    typedef dbot::RBCoordinateParticleFilter<StateTransition, ObservationModel>
+    typedef RBCoordinateParticleFilter<StateTransition, ObservationModel>
         Filter;
 
-    typedef typename Eigen::Transform<fl::Real, 3, Eigen::Affine> Affine;
+    struct Parameters
+    {
+        bool use_gpu;
+        int evaluation_count;
+        double max_kl_divergence;
 
-    typedef RbcParticleFilterObjectTracker::Parameters Parameters;
+        ObjectResourceIdentifier ori;
+        RbObservationModelBuilder<State>::Parameters obsrv;
+        BrownianMotionModelBuilder<State, Input>::Parameters process;
+    };
 
 public:
+    /**
+     * \brief Creates a RbcParticleFilterTrackerBuilder
+     * \param param			Builder and sub-builder parameters
+     * \param camera_data	Tracker camera data object
+     */
     RbcParticleFilterTrackerBuilder(const Parameters& param,
-                                    const dbot::CameraData& camera_data)
-        : param_(param), camera_data_(camera_data)
-    {
-    }
+                                    const CameraData& camera_data);
 
-    std::shared_ptr<RbcParticleFilterObjectTracker> build()
-    {
-        ObjectModel object_model;
-        object_model.load_from(
-            std::shared_ptr<ObjectModelLoader>(
-                new SimpleWavefrontObjectModelLoader(param_.ori)),
-            true);
-
-        std::shared_ptr<ObservationModel> obsrv_model;
-        if (!param_.use_gpu)
-        {
-            obsrv_model = RbObservationModelCpuBuilder<State>(
-                              param_.obsrv, object_model, camera_data_)
-                              .build();
-        }
-        else
-        {
-#ifdef BUILD_GPU
-            obsrv_model = RbObservationModelGpuBuilder<State>(
-                              param_.obsrv, object_model, camera_data_)
-                              .build();
-#else
-            ROS_FATAL("Tracker has not been compiled with GPU support!");
-            exit(1);
-#endif
-        }
-
-        BrownianMotionModelBuilder<State, Input> process_builder(
-            param_.process);
-        std::shared_ptr<StateTransition> process = process_builder.build();
-
-        auto filter = std::shared_ptr<Filter>(new Filter(
-            process,
-            obsrv_model,
-            create_sampling_blocks(
-                param_.ori.count_meshes(),
-                process->noise_dimension() / param_.ori.count_meshes()),
-            param_.max_kl_divergence));
-
-        auto tracker = std::make_shared<RbcParticleFilterObjectTracker>(
-            filter, param_, object_model, camera_data_);
-
-        return tracker;
-    }
+    /**
+     * \brief Builds the Rbc PF tracker
+     */
+    std::shared_ptr<RbcParticleFilterObjectTracker> build();
 
 private:
+    /**
+     * \brief Creates an instance of the Rbc particle filter
+     */
+    std::shared_ptr<Filter> create_filter(const ObjectModel& object_model,
+                                          double max_kl_divergence);
+
+    /**
+     * \brief Creates a Brownian motion state transition function used in the
+     *        filter
+     */
+    std::shared_ptr<StateTransition> create_state_transition_model(
+        const BrownianMotionModelBuilder<State, Input>& param) const;
+
+    /**
+     * \brief Creates the Rbc particle filter observation model. This can either
+     *        be CPU or GPU based
+     */
+    std::shared_ptr<ObservationModel> create_obsrv_model(
+        bool use_gpu,
+        const ObjectModel& object_model,
+        const CameraData& camera_data,
+        const RbObservationModelBuilder<State>::Parameters& param) const;
+
+    /**
+     * \brief Loads and creates an object model represented by the specified
+     *        resource identifier
+     */
+    ObjectModel create_object_model(const ObjectResourceIdentifier& ori) const;
+
+    /**
+     * \brief Creates a sampling block definition used by the coordinate
+     *        particle filter
+     *
+     * \param blocks		Number of objects or object parts
+     * \param block_size	State dimension of each part
+     */
     std::vector<std::vector<size_t>> create_sampling_blocks(
         int blocks,
-        int block_size) const
-    {
-        std::vector<std::vector<size_t>> sampling_blocks(
-            param_.ori.count_meshes());
-        for (int i = 0; i < blocks; ++i)
-        {
-            for (int k = 0; k < block_size; ++k)
-            {
-                sampling_blocks[i].push_back(i * block_size + k);
-            }
-        }
-
-        return sampling_blocks;
-    }
+        int block_size) const;
 
 private:
     Parameters param_;
     dbot::CameraData camera_data_;
 };
-
 }

include/dbot/tracker/rbc_particle_filter_object_tracker.hpp

@@ -58,45 +58,27 @@ class RbcParticleFilterObjectTracker
     typedef RBCoordinateParticleFilter<StateTransition, ObservationModel>
         Filter;
 
-    typedef typename Eigen::Transform<fl::Real, 3, Eigen::Affine> Affine;
-
-    struct Parameters
-    {
-        dbot::ObjectResourceIdentifier ori;
-
-        bool use_gpu;
-
-        int evaluation_count;
-        double max_kl_divergence;
-
-        RbObservationModelBuilder<State>::Parameters obsrv;
-        BrownianMotionModelBuilder<State, Input>::Parameters process;
-    };
-
 public:
-
     RbcParticleFilterObjectTracker(const std::shared_ptr<Filter>& filter,
-                                   const Parameters& param,
                                    const ObjectModel& object_model,
                                    const CameraData& camera_data);
 
     State track(const Obsrv& image);
 
-    void initialize(const std::vector<State>& initial_states);
+    void initialize(const std::vector<State>& initial_states,
+                    int evaluation_count);
     State to_center_coordinate_system(const State& state);
-    State to_model_coordinate_system(const State &state);
+    State to_model_coordinate_system(const State& state);
 
-    const Parameters& param() { return param_; }
     const CameraData& camera_data() const { return camera_data_; }
-
 private:
     Input zero_input() const;
 
 private:
     std::shared_ptr<Filter> filter_;
-    Parameters param_;
     ObjectModel object_model_;
     CameraData camera_data_;
+    int evaluation_count_;
 
     std::mutex mutex_;
 };

include/dbot/util/object_model.hpp

@@ -27,66 +27,33 @@ namespace dbot
 class ObjectModel
 {
 public:
-    typedef typename Eigen::Transform<fl::Real, 3, Eigen::Affine> Affine;
-
     typedef std::vector<std::vector<Eigen::Vector3d>> Vertices;
     typedef std::vector<std::vector<std::vector<int>>> TriangleIndecies;
 
+public:
     ObjectModel() = default;
 
-    ObjectModel(const std::shared_ptr<ObjectModelLoader>& loader, bool center)
-    {
-        load_from(loader, center);
-    }
+    ObjectModel(const std::shared_ptr<ObjectModelLoader>& loader, bool center);
 
     void load_from(const std::shared_ptr<ObjectModelLoader>& loader,
-                     bool center)
-    {
-        loader->load(vertices_, triangle_indices_);
-        compute_centers(centers_);
+                   bool center);
+
+    const Vertices& vertices() const;
 
-        if (center) center_vertices(centers_, vertices_);
-    }
+    const TriangleIndecies& triangle_indices() const;
 
-    const Vertices& vertices() const { return vertices_; }
-    const TriangleIndecies& triangle_indices() const
-    {
-        return triangle_indices_;
-    }
+    const std::vector<Eigen::Vector3d>& centers() const;
 
-    const std::vector<Eigen::Vector3d>& centers() const { return centers_; }
+    int count_parts() const;
 
-    int count_parts() const { return vertices_.size(); }
 private:
-    void compute_centers(std::vector<Eigen::Vector3d>& centers)
-    {
-        centers.resize(vertices_.size());
-        for (size_t i = 0; i < vertices_.size(); i++)
-        {
-            centers[i] = Eigen::Vector3d::Zero();
-            for (size_t j = 0; j < vertices_[i].size(); j++)
-            {
-                centers[i] += vertices_[i][j];
-            }
-            centers[i] /= double(vertices_[i].size());
-        }
-    }
+    void compute_centers(std::vector<Eigen::Vector3d>& centers);
 
     void center_vertices(const std::vector<Eigen::Vector3d>& centers,
-                         std::vector<std::vector<Eigen::Vector3d>>& vertices)
-    {
-        for (size_t i = 0; i < vertices.size(); i++)
-        {
-            for (size_t j = 0; j < vertices[i].size(); j++)
-            {
-                vertices[i][j] -= centers[i];
-            }
-        }
-    }
+                         std::vector<std::vector<Eigen::Vector3d>>& vertices);
 
 private:
     std::vector<Eigen::Vector3d> centers_;
-    std::vector<Affine> default_poses_;
 
     std::vector<std::vector<Eigen::Vector3d>> vertices_;
     std::vector<std::vector<std::vector<int>>> triangle_indices_;

include/dbot/util/simple_camera_data_provider.hpp

@@ -24,20 +24,13 @@ namespace dbot
 class SimpleCameraDataProvider : public CameraDataProvider
 {
 public:
-    SimpleCameraDataLoader(const std::string& camera_frame_id,
+    SimpleCameraDataProvider(const std::string& camera_frame_id,
                            const Eigen::Matrix3d& camera_mat,
-                           const CameraData::Resolution& native_res = {640,
-                                                                       480})
-        : frame_id_(camera_frame_id),
-          camera_matrix_(camera_mat),
-          native_res_(native_res)
+                           const CameraData::Resolution& native_res);
 
-    {
-    }
-
-    Eigen::Matrix3d camera_matrix() const { return camera_matrix_; }
-    std::string frame_id() const { return frame_id_; }
-    CameraData::Resolution native_resolution() const { return native_res_; }
+    Eigen::Matrix3d camera_matrix() const;
+    std::string frame_id() const;
+    CameraData::Resolution native_resolution() const;
 
 private:
     std::string frame_id_;

include/dbot/util/simple_wavefront_object_loader.hpp

@@ -22,28 +22,11 @@ namespace dbot
 class SimpleWavefrontObjectModelLoader : public ObjectModelLoader
 {
 public:
-    SimpleWavefrontObjectModelLoader(const ObjectResourceIdentifier& ori)
-        : ori_(ori)
-    {
-    }
+    SimpleWavefrontObjectModelLoader(const ObjectResourceIdentifier& ori);
 
     void load(
         std::vector<std::vector<Eigen::Vector3d>>& vertices,
-        std::vector<std::vector<std::vector<int>>>& triangle_indices) const
-    {
-        vertices.resize(ori_.count_meshes());
-        triangle_indices.resize(ori_.count_meshes());
-
-        for (size_t i = 0; i < ori_.count_meshes(); i++)
-        {
-            ObjectFileReader file_reader;
-            file_reader.set_filename(ori_.mesh_path(i));
-            file_reader.Read();
-
-            vertices[i] = *file_reader.get_vertices();
-            triangle_indices[i] = *file_reader.get_indices();
-        }
-    }
+        std::vector<std::vector<std::vector<int>>>& triangle_indices) const;
 
 private:
     ObjectResourceIdentifier ori_;