diff --git a/CMakeLists.txt b/CMakeLists.txt index 2b9e49e..ba70f15 100644 --- a/CMakeLists.txt +++ b/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} diff --git a/include/dbot/tracker/builder/rbc_particle_filter_tracker_builder.hpp b/include/dbot/tracker/builder/rbc_particle_filter_tracker_builder.hpp index 970a6e5..ea899cf 100644 --- a/include/dbot/tracker/builder/rbc_particle_filter_tracker_builder.hpp +++ b/include/dbot/tracker/builder/rbc_particle_filter_tracker_builder.hpp @@ -17,6 +17,10 @@ namespace dbot { + +/** + * \brief Represents an Rbc Particle filter based tracker builder + */ class RbcParticleFilterTrackerBuilder { public: @@ -24,89 +28,80 @@ class RbcParticleFilterTrackerBuilder typedef Eigen::VectorXd Input; typedef fl::StateTransitionFunction StateTransition; - typedef dbot::RbObservationModel ObservationModel; + typedef RbObservationModel ObservationModel; typedef typename ObservationModel::Observation Obsrv; - typedef dbot::RBCoordinateParticleFilter + typedef RBCoordinateParticleFilter Filter; - typedef typename Eigen::Transform Affine; + struct Parameters + { + bool use_gpu; + int evaluation_count; + double max_kl_divergence; - typedef RbcParticleFilterObjectTracker::Parameters Parameters; + ObjectResourceIdentifier ori; + RbObservationModelBuilder::Parameters obsrv; + BrownianMotionModelBuilder::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 build() - { - ObjectModel object_model; - object_model.load_from( - std::shared_ptr( - new SimpleWavefrontObjectModelLoader(param_.ori)), - true); - - std::shared_ptr obsrv_model; - if (!param_.use_gpu) - { - obsrv_model = RbObservationModelCpuBuilder( - param_.obsrv, object_model, camera_data_) - .build(); - } - else - { -#ifdef BUILD_GPU - obsrv_model = RbObservationModelGpuBuilder( - param_.obsrv, object_model, camera_data_) - .build(); -#else - ROS_FATAL("Tracker has not been compiled with GPU support!"); - exit(1); -#endif - } - - BrownianMotionModelBuilder process_builder( - param_.process); - std::shared_ptr process = process_builder.build(); - - auto filter = std::shared_ptr(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( - filter, param_, object_model, camera_data_); - - return tracker; - } + /** + * \brief Builds the Rbc PF tracker + */ + std::shared_ptr build(); private: + /** + * \brief Creates an instance of the Rbc particle filter + */ + std::shared_ptr 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 create_state_transition_model( + const BrownianMotionModelBuilder& param) const; + + /** + * \brief Creates the Rbc particle filter observation model. This can either + * be CPU or GPU based + */ + std::shared_ptr create_obsrv_model( + bool use_gpu, + const ObjectModel& object_model, + const CameraData& camera_data, + const RbObservationModelBuilder::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> create_sampling_blocks( int blocks, - int block_size) const - { - std::vector> 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_; }; - } diff --git a/include/dbot/tracker/rbc_particle_filter_object_tracker.hpp b/include/dbot/tracker/rbc_particle_filter_object_tracker.hpp index 2951019..5e0ff02 100644 --- a/include/dbot/tracker/rbc_particle_filter_object_tracker.hpp +++ b/include/dbot/tracker/rbc_particle_filter_object_tracker.hpp @@ -58,45 +58,27 @@ class RbcParticleFilterObjectTracker typedef RBCoordinateParticleFilter Filter; - typedef typename Eigen::Transform Affine; - - struct Parameters - { - dbot::ObjectResourceIdentifier ori; - - bool use_gpu; - - int evaluation_count; - double max_kl_divergence; - - RbObservationModelBuilder::Parameters obsrv; - BrownianMotionModelBuilder::Parameters process; - }; - public: - RbcParticleFilterObjectTracker(const std::shared_ptr& filter, - const Parameters& param, const ObjectModel& object_model, const CameraData& camera_data); State track(const Obsrv& image); - void initialize(const std::vector& initial_states); + void initialize(const std::vector& 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_; - Parameters param_; ObjectModel object_model_; CameraData camera_data_; + int evaluation_count_; std::mutex mutex_; }; diff --git a/include/dbot/util/object_model.hpp b/include/dbot/util/object_model.hpp index 5f48209..ec33489 100644 --- a/include/dbot/util/object_model.hpp +++ b/include/dbot/util/object_model.hpp @@ -27,66 +27,33 @@ namespace dbot class ObjectModel { public: - typedef typename Eigen::Transform Affine; - typedef std::vector> Vertices; typedef std::vector>> TriangleIndecies; +public: ObjectModel() = default; - ObjectModel(const std::shared_ptr& loader, bool center) - { - load_from(loader, center); - } + ObjectModel(const std::shared_ptr& loader, bool center); void load_from(const std::shared_ptr& 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& centers() const; - const std::vector& centers() const { return centers_; } + int count_parts() const; - int count_parts() const { return vertices_.size(); } private: - void compute_centers(std::vector& 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& centers); void center_vertices(const std::vector& centers, - std::vector>& 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>& vertices); private: std::vector centers_; - std::vector default_poses_; std::vector> vertices_; std::vector>> triangle_indices_; diff --git a/include/dbot/util/simple_camera_data_provider.hpp b/include/dbot/util/simple_camera_data_provider.hpp index 349ab95..17e3c7c 100644 --- a/include/dbot/util/simple_camera_data_provider.hpp +++ b/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_; diff --git a/include/dbot/util/simple_wavefront_object_loader.hpp b/include/dbot/util/simple_wavefront_object_loader.hpp index 3eda89b..097bef5 100644 --- a/include/dbot/util/simple_wavefront_object_loader.hpp +++ b/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>& vertices, - std::vector>>& 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>>& triangle_indices) const; private: ObjectResourceIdentifier ori_; diff --git a/src/dbot/tracker/builder/rbc_particle_filter_tracker_builder.cpp b/src/dbot/tracker/builder/rbc_particle_filter_tracker_builder.cpp new file mode 100644 index 0000000..a82eaae --- /dev/null +++ b/src/dbot/tracker/builder/rbc_particle_filter_tracker_builder.cpp @@ -0,0 +1,126 @@ +/* + * This is part of the Bayesian Object Tracking (bot), + * (https://github.com/bayesian-object-tracking) + * + * Copyright (c) 2015 Max Planck Society, + * Autonomous Motion Department, + * Institute for Intelligent Systems + * + * This Source Code Form is subject to the terms of the GNU General Public + * License License (GNU GPL). A copy of the license can be found in the LICENSE + * file distributed with this source code. + */ + +#include + +namespace dbot +{ +RbcParticleFilterTrackerBuilder::RbcParticleFilterTrackerBuilder( + const Parameters& param, + const CameraData& camera_data) + : param_(param), camera_data_(camera_data) +{ +} + +std::shared_ptr +RbcParticleFilterTrackerBuilder::build() +{ + auto object_model = create_object_model(param_.ori); + auto filter = create_filter(object_model, param_.max_kl_divergence); + + auto tracker = std::make_shared( + filter, object_model, camera_data_); + + return tracker; +} + +auto RbcParticleFilterTrackerBuilder::create_filter( + const ObjectModel& object_model, + double max_kl_divergence) -> std::shared_ptr +{ + auto state_transition_model = create_state_transition_model(param_.process); + + auto obsrv_model = create_obsrv_model( + param_.use_gpu, object_model, camera_data_, param_.obsrv); + + auto sampling_blocks = create_sampling_blocks( + object_model.count_parts(), + state_transition_model->noise_dimension() / object_model.count_parts()); + + auto filter = std::shared_ptr(new Filter(state_transition_model, + obsrv_model, + sampling_blocks, + max_kl_divergence)); + + return filter; +} + +auto RbcParticleFilterTrackerBuilder::create_state_transition_model( + const BrownianMotionModelBuilder& param) const + -> std::shared_ptr +{ + BrownianMotionModelBuilder process_builder(param); + std::shared_ptr process = process_builder.build(); + + return process; +} + +auto RbcParticleFilterTrackerBuilder::create_obsrv_model( + bool use_gpu, + const ObjectModel& object_model, + const CameraData& camera_data, + const RbObservationModelBuilder::Parameters& param) const + -> std::shared_ptr +{ + std::shared_ptr obsrv_model; + + if (!use_gpu) + { + obsrv_model = RbObservationModelCpuBuilder( + param, object_model, camera_data) + .build(); + } + else + { +#ifdef BUILD_GPU + obsrv_model = RbObservationModelGpuBuilder( + param, object_model, camera_data) + .build(); +#else + ROS_FATAL("Tracker has not been compiled with GPU support!"); + exit(1); +#endif + } + + return obsrv_model; +} + +ObjectModel RbcParticleFilterTrackerBuilder::create_object_model( + const ObjectResourceIdentifier& ori) const +{ + ObjectModel object_model; + + object_model.load_from(std::shared_ptr( + new SimpleWavefrontObjectModelLoader(ori)), + true); + + return object_model; +} + +std::vector> +RbcParticleFilterTrackerBuilder::create_sampling_blocks(int blocks, + int block_size) const +{ + std::vector> 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; +} + +} diff --git a/src/dbot/util/object_model.cpp b/src/dbot/util/object_model.cpp new file mode 100644 index 0000000..dd792fe --- /dev/null +++ b/src/dbot/util/object_model.cpp @@ -0,0 +1,78 @@ +/* + * This is part of the Bayesian Object Tracking (bot), + * (https://github.com/bayesian-object-tracking) + * + * Copyright (c) 2015 Max Planck Society, + * Autonomous Motion Department, + * Institute for Intelligent Systems + * + * This Source Code Form is subject to the terms of the GNU General Public + * License License (GNU GPL). A copy of the license can be found in the LICENSE + * file distributed with this source code. + */ + +#include + +namespace dbot +{ +ObjectModel::ObjectModel(const std::shared_ptr& loader, + bool center) +{ + load_from(loader, center); +} + +void ObjectModel::load_from(const std::shared_ptr& loader, + bool center) +{ + loader->load(vertices_, triangle_indices_); + compute_centers(centers_); + + if (center) center_vertices(centers_, vertices_); +} + +auto ObjectModel::vertices() const -> const Vertices & +{ + return vertices_; +} + +auto ObjectModel::triangle_indices() const -> const TriangleIndecies & +{ + return triangle_indices_; +} + +const std::vector& ObjectModel::centers() const +{ + return centers_; +} + +int ObjectModel::count_parts() const +{ + return vertices_.size(); +} +void ObjectModel::compute_centers(std::vector& 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 ObjectModel::center_vertices( + const std::vector& centers, + std::vector>& 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]; + } + } +} +} diff --git a/src/dbot/util/simple_camera_data_provider.cpp b/src/dbot/util/simple_camera_data_provider.cpp new file mode 100644 index 0000000..95f002f --- /dev/null +++ b/src/dbot/util/simple_camera_data_provider.cpp @@ -0,0 +1,43 @@ +/* + * This is part of the Bayesian Object Tracking (bot), + * (https://github.com/bayesian-object-tracking) + * + * Copyright (c) 2015 Max Planck Society, + * Autonomous Motion Department, + * Institute for Intelligent Systems + * + * This Source Code Form is subject to the terms of the GNU General Public + * License License (GNU GPL). A copy of the license can be found in the LICENSE + * file distributed with this source code. + */ + +#include + +namespace dbot +{ +SimpleCameraDataProvider::SimpleCameraDataProvider( + const std::string& camera_frame_id, + const Eigen::Matrix3d& camera_mat, + const CameraData::Resolution& native_res) + : frame_id_(camera_frame_id), + camera_matrix_(camera_mat), + native_res_(native_res) +{ +} + +Eigen::Matrix3d SimpleCameraDataProvider::camera_matrix() const +{ + return camera_matrix_; +} + +std::string SimpleCameraDataProvider::frame_id() const +{ + return frame_id_; +} + +CameraData::Resolution SimpleCameraDataProvider::native_resolution() const +{ + return native_res_; +} + +} diff --git a/src/dbot/util/simple_wavefront_object_loader.cpp b/src/dbot/util/simple_wavefront_object_loader.cpp new file mode 100644 index 0000000..51deb68 --- /dev/null +++ b/src/dbot/util/simple_wavefront_object_loader.cpp @@ -0,0 +1,40 @@ +/* + * this is part of the bayesian object tracking (bot), + * (https://github.com/bayesian-object-tracking) + * + * copyright (c) 2015 max planck society, + * autonomous motion department, + * institute for intelligent systems + * + * this source code form is subject to the terms of the gnu general public + * license license (gnu gpl). a copy of the license can be found in the license + * file distributed with this source code. + */ + +#include +namespace dbot +{ +SimpleWavefrontObjectModelLoader::SimpleWavefrontObjectModelLoader( + const ObjectResourceIdentifier& ori) + : ori_(ori) +{ +} + +void SimpleWavefrontObjectModelLoader::load( + std::vector>& vertices, + std::vector>>& 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(); + } +} +}