From 569ac38a71b13495cbd1fac18a431eee7bda6310 Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 18 Aug 2015 15:27:46 +0200 Subject: [PATCH 001/131] cleaned some stuff in rigid_body_renderer --- include/dbot/utils/rigid_body_renderer.hpp | 7 +++---- src/dbot/utils/rigid_body_renderer.cpp | 22 ++++------------------ 2 files changed, 7 insertions(+), 22 deletions(-) diff --git a/include/dbot/utils/rigid_body_renderer.hpp b/include/dbot/utils/rigid_body_renderer.hpp index 6a76b76..5a56b2c 100644 --- a/include/dbot/utils/rigid_body_renderer.hpp +++ b/include/dbot/utils/rigid_body_renderer.hpp @@ -49,9 +49,9 @@ class RigidBodyRenderer typedef typename Eigen::Transform Affine; - RigidBodyRenderer(const std::vector >& vertices, - const std::vector > >& indices, - const boost::shared_ptr& state_ptr); + RigidBodyRenderer( + const std::vector >& vertices, + const std::vector > >& indices); virtual ~RigidBodyRenderer(); @@ -90,7 +90,6 @@ class RigidBodyRenderer std::vector > > indices_; // state - const boost::shared_ptr state_; std::vector R_; std::vector t_; diff --git a/src/dbot/utils/rigid_body_renderer.cpp b/src/dbot/utils/rigid_body_renderer.cpp index e7027b1..d3453ae 100644 --- a/src/dbot/utils/rigid_body_renderer.cpp +++ b/src/dbot/utils/rigid_body_renderer.cpp @@ -38,12 +38,11 @@ using namespace Eigen; using namespace ff; -RigidBodyRenderer::RigidBodyRenderer(const std::vector >& vertices, - const std::vector > >& indices, - const boost::shared_ptr& state_ptr) -:vertices_(vertices), indices_(indices), state_(state_ptr) +RigidBodyRenderer::RigidBodyRenderer( + const std::vector >& vertices, + const std::vector > >& indices) +:vertices_(vertices), indices_(indices) { -// state(*state_ptr); /// \todo this does not belong here float total_weight = 0; coms_.resize(vertices.size()); @@ -295,19 +294,6 @@ RigidBodyRenderer::Vector RigidBodyRenderer::object_center(const size_t& index) return R_[index]*coms_[index] + t_[index]; } -//// set state -//void RigidBodyRenderer::state(const Eigen::VectorXd& state) -//{ -// *state_ = state; -// R_.resize(state_->body_count()); -// t_.resize(state_->body_count()); -// for(size_t part_index = 0; part_index < state_->body_count(); part_index++) -// { -// R_[part_index] = state_->rotation_matrix(part_index); -// t_[part_index] = state_->position(part_index); -// } -//} - // set state void RigidBodyRenderer::set_poses(const std::vector& rotations, From 882ef15c002fbcc94775bbc98703b0b906629a69 Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 18 Aug 2015 16:50:46 +0200 Subject: [PATCH 002/131] in the process of using new state, still everything working --- .../states/free_floating_rigid_bodies_state.hpp | 13 +++++++++++++ include/dbot/states/rigid_bodies_state.hpp | 10 +++++++++- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/include/dbot/states/free_floating_rigid_bodies_state.hpp b/include/dbot/states/free_floating_rigid_bodies_state.hpp index dc283de..2502ad7 100644 --- a/include/dbot/states/free_floating_rigid_bodies_state.hpp +++ b/include/dbot/states/free_floating_rigid_bodies_state.hpp @@ -88,6 +88,8 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes Affine; + typedef typename Base::PoseVelocityBlock PoseVelocityBlock; + typedef Eigen::VectorBlock Block; typedef Eigen::VectorBlock PoseBlock; typedef Eigen::VectorBlock BodyBlock; @@ -185,6 +187,17 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypesrows()/BODY_SIZE; } + + + // accessors *************************************************************** + virtual fl::PoseVelocityVector component(int index) const + { + return PoseVelocityBlock(*((State*)(this)), index * PoseVelocityBlock::SizeAtCompileTime); + } + int count() const + { + return this->size() / PoseVelocityBlock::SizeAtCompileTime; + } }; } diff --git a/include/dbot/states/rigid_bodies_state.hpp b/include/dbot/states/rigid_bodies_state.hpp index 19bab04..bb37799 100644 --- a/include/dbot/states/rigid_bodies_state.hpp +++ b/include/dbot/states/rigid_bodies_state.hpp @@ -30,6 +30,7 @@ along with this program. If not, see . #include #include +#include namespace ff { @@ -48,6 +49,8 @@ class RigidBodiesState: public Eigen::Matrix typedef Eigen::Matrix RotationMatrix; typedef Eigen::Matrix HomogeneousMatrix; + typedef fl::PoseVelocityBlock PoseVelocityBlock; + // constructor and destructor RigidBodiesState() { } template RigidBodiesState(const Eigen::MatrixBase& state_vector) @@ -73,7 +76,7 @@ class RigidBodiesState: public Eigen::Matrix Scalar angle = euler_vector(object_index).norm(); Vector axis = euler_vector(object_index).normalized(); - if(std::isfinite(axis.norm())) + if(std::isfinite(axis.norm())) { return Quaternion(AngleAxis(angle, axis)); } @@ -93,6 +96,11 @@ class RigidBodiesState: public Eigen::Matrix } virtual unsigned body_count() const = 0; + + + virtual fl::PoseVelocityVector component(int index) const = 0; + + virtual int count() const = 0; }; } From c0384c473dc8e7eb9fc3a0295c8c522c8330c11d Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 18 Aug 2015 17:26:55 +0200 Subject: [PATCH 003/131] in the process of changing state rep, everything working --- .../free_floating_rigid_bodies_state.hpp | 24 ++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/include/dbot/states/free_floating_rigid_bodies_state.hpp b/include/dbot/states/free_floating_rigid_bodies_state.hpp index 2502ad7..b516f9b 100644 --- a/include/dbot/states/free_floating_rigid_bodies_state.hpp +++ b/include/dbot/states/free_floating_rigid_bodies_state.hpp @@ -95,7 +95,6 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes BodyBlock; // give access to base member functions (otherwise it is shadowed) - using Base::quaternion; FreeFloatingRigidBodiesState() { } FreeFloatingRigidBodiesState(unsigned count_bodies): Base(State::Zero(count_bodies * BODY_SIZE)) { } @@ -105,6 +104,10 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes(body_index * POSE_SIZE) = pose(body_index); + poses_.template middleRows(body_index * POSE_SIZE) + = component(body_index).pose(); } return poses_; } @@ -142,7 +146,8 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes(body_index * POSE_SIZE); + component(body_index).pose() + = poses_.template middleRows(body_index * POSE_SIZE); } } Block position(const size_t& body_index = 0) @@ -167,6 +172,9 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypesderived(), body_index * BODY_SIZE); } @@ -198,6 +206,16 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypessize() / PoseVelocityBlock::SizeAtCompileTime; } + + // mutators **************************************************************** + PoseVelocityBlock component(int index) + { + return PoseVelocityBlock(*((State*)(this)), index * PoseVelocityBlock::SizeAtCompileTime); + } + void recount(int new_count) + { + return this->resize(new_count * PoseVelocityBlock::SizeAtCompileTime); + } }; } From a25c8f8907e5bb026a416dcaa3df65c5417816fb Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 18 Aug 2015 18:18:53 +0200 Subject: [PATCH 004/131] still working --- .../brownian_object_motion_model.hpp | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/include/dbot/models/process_models/brownian_object_motion_model.hpp b/include/dbot/models/process_models/brownian_object_motion_model.hpp index 6628431..0756ccc 100644 --- a/include/dbot/models/process_models/brownian_object_motion_model.hpp +++ b/include/dbot/models/process_models/brownian_object_motion_model.hpp @@ -146,14 +146,14 @@ class BrownianObjectMotionModel Eigen::Matrix linear_delta = linear_process_[i].MapStandardGaussian(position_noise); Eigen::Matrix angular_delta = angular_process_[i].MapStandardGaussian(orientation_noise); - new_state.position(i) = state_.position(i) + linear_delta.topRows(3); - Quaternion updated_quaternion(state_.quaternion(i).coeffs() + quaternion_map_[i] * angular_delta.topRows(3)); + new_state.position(i) = state_.component(i).position() + linear_delta.topRows(3); + Quaternion updated_quaternion(state_.component(i).euler_vector().quaternion().coeffs() + quaternion_map_[i] * angular_delta.topRows(3)); new_state.quaternion(updated_quaternion.normalized(), i); new_state.linear_velocity(i) = linear_delta.bottomRows(3); new_state.angular_velocity(i) = angular_delta.bottomRows(3); // transform to external coordinate system - new_state.linear_velocity(i) -= new_state.angular_velocity(i).cross(state_.position(i)); + new_state.linear_velocity(i) -= new_state.angular_velocity(i).cross(state_.component(i).position()); new_state.position(i) -= new_state.rotation_matrix(i)*rotation_center_[i]; } @@ -185,17 +185,17 @@ class BrownianObjectMotionModel // state_ = state; // for(size_t i = 0; i < state_.body_count(); i++) // { -// quaternion_map_[i] = ff::hf::QuaternionMatrix(state_.quaternion(i).coeffs()); +// quaternion_map_[i] = ff::hf::QuaternionMatrix(state_.component(i).euler_vector().quaternion().coeffs()); // // transform the state, which is the pose and velocity with respect to to the origin, // // into internal representation, which is the position and velocity of the center // // and the orientation and angular velocity around the center -// state_.position(i) += state_.rotation_matrix(i)*rotation_center_[i]; -// state_.linear_velocity(i) += state_.angular_velocity(i).cross(state_.position(i)); +// state_.component(i).position() += state_.rotation_matrix(i)*rotation_center_[i]; +// state_.component(i).linear_velocity() += state_.angular_velocity(i).cross(state_.component(i).position()); // Eigen::Matrix linear_state; // linear_state.topRows(3) = Eigen::Vector3d::Zero(); -// linear_state.bottomRows(3) = state_.linear_velocity(i); +// linear_state.bottomRows(3) = state_.component(i).linear_velocity(); // linear_process_[i].Condition(delta_time, // linear_state, // control.template middleRows<3>(i*DIMENSION_PER_OBJECT)); @@ -218,17 +218,18 @@ class BrownianObjectMotionModel state_ = state; for(size_t i = 0; i < state_.body_count(); i++) { - quaternion_map_[i] = ff::hf::QuaternionMatrix(state_.quaternion(i).coeffs()); + quaternion_map_[i] = ff::hf::QuaternionMatrix(state_.component(i).euler_vector().quaternion().coeffs()); // transform the state, which is the pose and velocity with respect to to the origin, // into internal representation, which is the position and velocity of the center // and the orientation and angular velocity around the center - state_.position(i) += state_.rotation_matrix(i)*rotation_center_[i]; - state_.linear_velocity(i) += state_.angular_velocity(i).cross(state_.position(i)); + state_.component(i).position() + += state_.rotation_matrix(i)*rotation_center_[i]; + state_.component(i).linear_velocity() += state_.angular_velocity(i).cross(state_.component(i).position()); Eigen::Matrix linear_state; linear_state.topRows(3) = Eigen::Vector3d::Zero(); - linear_state.bottomRows(3) = state_.linear_velocity(i); + linear_state.bottomRows(3) = state_.component(i).linear_velocity(); linear_process_[i].Condition(linear_state, control.template middleRows<3>(i*DIMENSION_PER_OBJECT)); From 9051842f104995e5484cd67c9e716bb7b82215f4 Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 18 Aug 2015 19:14:14 +0200 Subject: [PATCH 005/131] still working --- .../brownian_object_motion_model.hpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/include/dbot/models/process_models/brownian_object_motion_model.hpp b/include/dbot/models/process_models/brownian_object_motion_model.hpp index 0756ccc..c2f1c9a 100644 --- a/include/dbot/models/process_models/brownian_object_motion_model.hpp +++ b/include/dbot/models/process_models/brownian_object_motion_model.hpp @@ -146,15 +146,15 @@ class BrownianObjectMotionModel Eigen::Matrix linear_delta = linear_process_[i].MapStandardGaussian(position_noise); Eigen::Matrix angular_delta = angular_process_[i].MapStandardGaussian(orientation_noise); - new_state.position(i) = state_.component(i).position() + linear_delta.topRows(3); + new_state.component(i).position() = state_.component(i).position() + linear_delta.topRows(3); Quaternion updated_quaternion(state_.component(i).euler_vector().quaternion().coeffs() + quaternion_map_[i] * angular_delta.topRows(3)); new_state.quaternion(updated_quaternion.normalized(), i); - new_state.linear_velocity(i) = linear_delta.bottomRows(3); - new_state.angular_velocity(i) = angular_delta.bottomRows(3); + new_state.component(i).linear_velocity() = linear_delta.bottomRows(3); + new_state.component(i).angular_velocity() = angular_delta.bottomRows(3); // transform to external coordinate system - new_state.linear_velocity(i) -= new_state.angular_velocity(i).cross(state_.component(i).position()); - new_state.position(i) -= new_state.rotation_matrix(i)*rotation_center_[i]; + new_state.component(i).linear_velocity() -= new_state.component(i).angular_velocity().cross(state_.component(i).position()); + new_state.component(i).position() -= new_state.component(i).euler_vector().rotation_matrix()*rotation_center_[i]; } return new_state; @@ -190,8 +190,8 @@ class BrownianObjectMotionModel // // transform the state, which is the pose and velocity with respect to to the origin, // // into internal representation, which is the position and velocity of the center // // and the orientation and angular velocity around the center -// state_.component(i).position() += state_.rotation_matrix(i)*rotation_center_[i]; -// state_.component(i).linear_velocity() += state_.angular_velocity(i).cross(state_.component(i).position()); +// state_.component(i).position() += state_.component(i).euler_vector().rotation_matrix()*rotation_center_[i]; +// state_.component(i).linear_velocity() += state_.component(i).angular_velocity().cross(state_.component(i).position()); // Eigen::Matrix linear_state; // linear_state.topRows(3) = Eigen::Vector3d::Zero(); @@ -202,7 +202,7 @@ class BrownianObjectMotionModel // Eigen::Matrix angular_state; // angular_state.topRows(3) = Eigen::Vector3d::Zero(); -// angular_state.bottomRows(3) = state_.angular_velocity(i); +// angular_state.bottomRows(3) = state_.component(i).angular_velocity(); // angular_process_[i].Condition(delta_time, // angular_state, // control.template middleRows<3>(i*DIMENSION_PER_OBJECT + 3)); @@ -224,8 +224,8 @@ class BrownianObjectMotionModel // into internal representation, which is the position and velocity of the center // and the orientation and angular velocity around the center state_.component(i).position() - += state_.rotation_matrix(i)*rotation_center_[i]; - state_.component(i).linear_velocity() += state_.angular_velocity(i).cross(state_.component(i).position()); + += state_.component(i).euler_vector().rotation_matrix()*rotation_center_[i]; + state_.component(i).linear_velocity() += state_.component(i).angular_velocity().cross(state_.component(i).position()); Eigen::Matrix linear_state; linear_state.topRows(3) = Eigen::Vector3d::Zero(); @@ -235,7 +235,7 @@ class BrownianObjectMotionModel Eigen::Matrix angular_state; angular_state.topRows(3) = Eigen::Vector3d::Zero(); - angular_state.bottomRows(3) = state_.angular_velocity(i); + angular_state.bottomRows(3) = state_.component(i).angular_velocity(); angular_process_[i].Condition(angular_state, control.template middleRows<3>(i*DIMENSION_PER_OBJECT + 3)); } From 034a0f6c247aaebbe70337575d68976ba07b64cd Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 18 Aug 2015 19:29:21 +0200 Subject: [PATCH 006/131] stripping state, still working --- .../free_floating_rigid_bodies_state.hpp | 62 +++++++++---------- 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/include/dbot/states/free_floating_rigid_bodies_state.hpp b/include/dbot/states/free_floating_rigid_bodies_state.hpp index b516f9b..4f1f1ef 100644 --- a/include/dbot/states/free_floating_rigid_bodies_state.hpp +++ b/include/dbot/states/free_floating_rigid_bodies_state.hpp @@ -118,18 +118,18 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypestemplate middleRows(body_index * BODY_SIZE + ORIENTATION_INDEX); } - virtual Vector linear_velocity(const size_t& body_index = 0) const - { - return this->template middleRows(body_index * BODY_SIZE + LINEAR_VELOCITY_INDEX); - } - virtual Vector angular_velocity(const size_t& body_index = 0) const - { - return this->template middleRows(body_index * BODY_SIZE + ANGULAR_VELOCITY_INDEX); - } - virtual Pose pose(const size_t& body_index = 0) const - { - return this->template middleRows(body_index * BODY_SIZE + POSE_INDEX); - } +// virtual Vector linear_velocity(const size_t& body_index = 0) const +// { +// return this->template middleRows(body_index * BODY_SIZE + LINEAR_VELOCITY_INDEX); +// } +// virtual Vector angular_velocity(const size_t& body_index = 0) const +// { +// return this->template middleRows(body_index * BODY_SIZE + ANGULAR_VELOCITY_INDEX); +// } +// virtual Pose pose(const size_t& body_index = 0) const +// { +// return this->template middleRows(body_index * BODY_SIZE + POSE_INDEX); +// } virtual Poses poses() const { Poses poses_(body_count()*POSE_SIZE); @@ -158,25 +158,25 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypesderived(), body_index * BODY_SIZE + ORIENTATION_INDEX); } - Block linear_velocity(const size_t& body_index = 0) - { - return Block(this->derived(), body_index * BODY_SIZE + LINEAR_VELOCITY_INDEX); - } - Block angular_velocity(const size_t& body_index = 0) - { - return Block(this->derived(), body_index * BODY_SIZE + ANGULAR_VELOCITY_INDEX); - } - PoseBlock pose(const size_t& body_index = 0) - { - return PoseBlock(this->derived(), body_index * BODY_SIZE + POSE_INDEX); - } - BodyBlock operator [](const size_t& body_index) - { - std::cout << "operator shizzle is being used" << std::endl; - - exit(-1); - return BodyBlock(this->derived(), body_index * BODY_SIZE); - } +// Block linear_velocity(const size_t& body_index = 0) +// { +// return Block(this->derived(), body_index * BODY_SIZE + LINEAR_VELOCITY_INDEX); +// } +// Block angular_velocity(const size_t& body_index = 0) +// { +// return Block(this->derived(), body_index * BODY_SIZE + ANGULAR_VELOCITY_INDEX); +// } +// PoseBlock pose(const size_t& body_index = 0) +// { +// return PoseBlock(this->derived(), body_index * BODY_SIZE + POSE_INDEX); +// } +// BodyBlock operator [](const size_t& body_index) +// { +// std::cout << "operator shizzle is being used" << std::endl; + +// exit(-1); +// return BodyBlock(this->derived(), body_index * BODY_SIZE); +// } // other representations virtual void quaternion(const Quaternion& quaternion, const size_t& body_index = 0) From b558c62b49c95c83162d9e43ce9332fd3bb09e27 Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 18 Aug 2015 19:39:07 +0200 Subject: [PATCH 007/131] still working --- .../brownian_object_motion_model.hpp | 2 +- .../free_floating_rigid_bodies_state.hpp | 42 +++++++++++-------- 2 files changed, 25 insertions(+), 19 deletions(-) diff --git a/include/dbot/models/process_models/brownian_object_motion_model.hpp b/include/dbot/models/process_models/brownian_object_motion_model.hpp index c2f1c9a..a76b3ab 100644 --- a/include/dbot/models/process_models/brownian_object_motion_model.hpp +++ b/include/dbot/models/process_models/brownian_object_motion_model.hpp @@ -148,7 +148,7 @@ class BrownianObjectMotionModel new_state.component(i).position() = state_.component(i).position() + linear_delta.topRows(3); Quaternion updated_quaternion(state_.component(i).euler_vector().quaternion().coeffs() + quaternion_map_[i] * angular_delta.topRows(3)); - new_state.quaternion(updated_quaternion.normalized(), i); + new_state.component(i).euler_vector().quaternion(updated_quaternion.normalized()); new_state.component(i).linear_velocity() = linear_delta.bottomRows(3); new_state.component(i).angular_velocity() = angular_delta.bottomRows(3); diff --git a/include/dbot/states/free_floating_rigid_bodies_state.hpp b/include/dbot/states/free_floating_rigid_bodies_state.hpp index 4f1f1ef..90bd7f9 100644 --- a/include/dbot/states/free_floating_rigid_bodies_state.hpp +++ b/include/dbot/states/free_floating_rigid_bodies_state.hpp @@ -118,6 +118,19 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypestemplate middleRows(body_index * BODY_SIZE + ORIENTATION_INDEX); } + + Block position(const size_t& body_index = 0) + { + return Block(this->derived(), body_index * BODY_SIZE + POSITION_INDEX); + } + Block euler_vector(const size_t& body_index = 0) + { + return Block(this->derived(), body_index * BODY_SIZE + ORIENTATION_INDEX); + } + + + + // virtual Vector linear_velocity(const size_t& body_index = 0) const // { // return this->template middleRows(body_index * BODY_SIZE + LINEAR_VELOCITY_INDEX); @@ -150,14 +163,7 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes(body_index * POSE_SIZE); } } - Block position(const size_t& body_index = 0) - { - return Block(this->derived(), body_index * BODY_SIZE + POSITION_INDEX); - } - Block euler_vector(const size_t& body_index = 0) - { - return Block(this->derived(), body_index * BODY_SIZE + ORIENTATION_INDEX); - } + // Block linear_velocity(const size_t& body_index = 0) // { // return Block(this->derived(), body_index * BODY_SIZE + LINEAR_VELOCITY_INDEX); @@ -179,16 +185,16 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes Date: Tue, 18 Aug 2015 19:52:21 +0200 Subject: [PATCH 008/131] still --- include/dbot/states/free_floating_rigid_bodies_state.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/dbot/states/free_floating_rigid_bodies_state.hpp b/include/dbot/states/free_floating_rigid_bodies_state.hpp index 90bd7f9..29a95a1 100644 --- a/include/dbot/states/free_floating_rigid_bodies_state.hpp +++ b/include/dbot/states/free_floating_rigid_bodies_state.hpp @@ -107,7 +107,7 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes Date: Tue, 18 Aug 2015 20:04:05 +0200 Subject: [PATCH 009/131] still working --- .../kinect_image_observation_model_cpu.hpp | 3 +- .../kinect_image_observation_model_gpu.hpp | 6 ++- include/dbot/states/rigid_bodies_state.hpp | 46 +++++++++---------- 3 files changed, 29 insertions(+), 26 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index f5df620..3489cd5 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -141,7 +141,8 @@ class KinectImageObservationModelCPU: std::vector translations(body_count); for(size_t part_index = 0; part_index < body_count; part_index++) { - rotations[part_index] = states[state_index].rotation_matrix(part_index); + rotations[part_index] = + states[state_index].component(part_index).euler_vector().rotation_matrix(); translations[part_index] = states[state_index].position(part_index); } diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index dbe0c06..837d8ac 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -225,12 +225,14 @@ class KinectImageObservationModelGPU: for(size_t state_index = 0; state_index < size_t(n_poses_); state_index++) for(size_t body_index = 0; body_index < states[state_index].body_count(); body_index++) { - const Eigen::Quaternion& quaternion = states[state_index].quaternion(body_index); + const Eigen::Quaternion& quaternion + = states[state_index].component(body_index).euler_vector().quaternion(); states_internal_format[state_index][body_index][0] = quaternion.w(); states_internal_format[state_index][body_index][1] = quaternion.x(); states_internal_format[state_index][body_index][2] = quaternion.y(); states_internal_format[state_index][body_index][3] = quaternion.z(); - const Eigen::Matrix& position = states[state_index].position(body_index); + const Eigen::Matrix& position = + states[state_index].component(body_index).position(); states_internal_format[state_index][body_index][4] = position[0]; states_internal_format[state_index][body_index][5] = position[1]; states_internal_format[state_index][body_index][6] = position[2]; diff --git a/include/dbot/states/rigid_bodies_state.hpp b/include/dbot/states/rigid_bodies_state.hpp index bb37799..3fff31d 100644 --- a/include/dbot/states/rigid_bodies_state.hpp +++ b/include/dbot/states/rigid_bodies_state.hpp @@ -71,29 +71,29 @@ class RigidBodiesState: public Eigen::Matrix virtual Vector euler_vector(const size_t& object_index = 0) const = 0; // other representations - virtual Quaternion quaternion(const size_t& object_index = 0) const - { - Scalar angle = euler_vector(object_index).norm(); - Vector axis = euler_vector(object_index).normalized(); - - if(std::isfinite(axis.norm())) - { - return Quaternion(AngleAxis(angle, axis)); - } - return Quaternion::Identity(); - } - virtual RotationMatrix rotation_matrix(const size_t& object_index = 0) const - { - return RotationMatrix(quaternion(object_index)); - } - virtual HomogeneousMatrix homogeneous_matrix(const size_t& object_index = 0) const - { - HomogeneousMatrix homogeneous_matrix(HomogeneousMatrix::Identity()); - homogeneous_matrix.topLeftCorner(3, 3) = rotation_matrix(object_index); - homogeneous_matrix.topRightCorner(3, 1) = position(object_index); - - return homogeneous_matrix; - } +// virtual Quaternion quaternion(const size_t& object_index = 0) const +// { +// Scalar angle = euler_vector(object_index).norm(); +// Vector axis = euler_vector(object_index).normalized(); + +// if(std::isfinite(axis.norm())) +// { +// return Quaternion(AngleAxis(angle, axis)); +// } +// return Quaternion::Identity(); +// } +// virtual RotationMatrix rotation_matrix(const size_t& object_index = 0) const +// { +// return RotationMatrix(quaternion(object_index)); +// } +// virtual HomogeneousMatrix homogeneous_matrix(const size_t& object_index = 0) const +// { +// HomogeneousMatrix homogeneous_matrix(HomogeneousMatrix::Identity()); +// homogeneous_matrix.topLeftCorner(3, 3) = rotation_matrix(object_index); +// homogeneous_matrix.topRightCorner(3, 1) = position(object_index); + +// return homogeneous_matrix; +// } virtual unsigned body_count() const = 0; From 25abdfa9e89807403528be249ea40786df5467b6 Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 18 Aug 2015 20:18:18 +0200 Subject: [PATCH 010/131] almost stripped down state completely, stuff still working --- .../kinect_image_observation_model_cpu.hpp | 3 +- .../free_floating_rigid_bodies_state.hpp | 32 +++++++++---------- include/dbot/states/rigid_bodies_state.hpp | 8 ++--- 3 files changed, 22 insertions(+), 21 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index 3489cd5..2e74ffe 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -143,7 +143,8 @@ class KinectImageObservationModelCPU: { rotations[part_index] = states[state_index].component(part_index).euler_vector().rotation_matrix(); - translations[part_index] = states[state_index].position(part_index); + translations[part_index] = + states[state_index].component(part_index).position(); } diff --git a/include/dbot/states/free_floating_rigid_bodies_state.hpp b/include/dbot/states/free_floating_rigid_bodies_state.hpp index 29a95a1..e353983 100644 --- a/include/dbot/states/free_floating_rigid_bodies_state.hpp +++ b/include/dbot/states/free_floating_rigid_bodies_state.hpp @@ -110,23 +110,23 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypestemplate middleRows(body_index * BODY_SIZE + POSITION_INDEX); - } - virtual Vector euler_vector(const size_t& body_index = 0) const - { - return this->template middleRows(body_index * BODY_SIZE + ORIENTATION_INDEX); - } +// virtual Vector position(const size_t& body_index = 0) const +// { +// return this->template middleRows(body_index * BODY_SIZE + POSITION_INDEX); +// } +// virtual Vector euler_vector(const size_t& body_index = 0) const +// { +// return this->template middleRows(body_index * BODY_SIZE + ORIENTATION_INDEX); +// } - Block position(const size_t& body_index = 0) - { - return Block(this->derived(), body_index * BODY_SIZE + POSITION_INDEX); - } - Block euler_vector(const size_t& body_index = 0) - { - return Block(this->derived(), body_index * BODY_SIZE + ORIENTATION_INDEX); - } +// Block position(const size_t& body_index = 0) +// { +// return Block(this->derived(), body_index * BODY_SIZE + POSITION_INDEX); +// } +// Block euler_vector(const size_t& body_index = 0) +// { +// return Block(this->derived(), body_index * BODY_SIZE + ORIENTATION_INDEX); +// } diff --git a/include/dbot/states/rigid_bodies_state.hpp b/include/dbot/states/rigid_bodies_state.hpp index 3fff31d..7a2e84e 100644 --- a/include/dbot/states/rigid_bodies_state.hpp +++ b/include/dbot/states/rigid_bodies_state.hpp @@ -66,9 +66,9 @@ class RigidBodiesState: public Eigen::Matrix *((State*)(this)) = state_vector; } - // read - virtual Vector position(const size_t& object_index = 0) const = 0; - virtual Vector euler_vector(const size_t& object_index = 0) const = 0; +// // read +// virtual Vector position(const size_t& object_index = 0) const = 0; +// virtual Vector euler_vector(const size_t& object_index = 0) const = 0; // other representations // virtual Quaternion quaternion(const size_t& object_index = 0) const @@ -95,7 +95,7 @@ class RigidBodiesState: public Eigen::Matrix // return homogeneous_matrix; // } - virtual unsigned body_count() const = 0; +// virtual unsigned body_count() const = 0; virtual fl::PoseVelocityVector component(int index) const = 0; From 6365cf6669453ad7892c731022b8f2b70ca78c4f Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 18 Aug 2015 20:27:28 +0200 Subject: [PATCH 011/131] still working --- .../kinect_image_observation_model_cpu.hpp | 2 +- .../kinect_image_observation_model_gpu.hpp | 4 +- .../brownian_object_motion_model.hpp | 8 +- .../free_floating_rigid_bodies_state.hpp | 82 +------------------ 4 files changed, 11 insertions(+), 85 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index 2e74ffe..3fa5b76 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -135,7 +135,7 @@ class KinectImageObservationModelCPU: std::vector predictions; //TODO: DOES THIS MAKE SENSE? THE OBJECT MODEL SHOULD KNOW ABOUT THE STATE... - int body_count = states[state_index].body_count(); + int body_count = states[state_index].count(); std::vector rotations(body_count); std::vector translations(body_count); diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 837d8ac..9793e27 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -216,14 +216,14 @@ class KinectImageObservationModelGPU: // convert to internal state format std::vector > > states_internal_format( n_poses_, - std::vector >(states[0].body_count(), + std::vector >(states[0].count(), std::vector(7, 0))); MEASURE("gpu: creating state vectors"); for(size_t state_index = 0; state_index < size_t(n_poses_); state_index++) - for(size_t body_index = 0; body_index < states[state_index].body_count(); body_index++) + for(size_t body_index = 0; body_index < states[state_index].count(); body_index++) { const Eigen::Quaternion& quaternion = states[state_index].component(body_index).euler_vector().quaternion(); diff --git a/include/dbot/models/process_models/brownian_object_motion_model.hpp b/include/dbot/models/process_models/brownian_object_motion_model.hpp index a76b3ab..6d0aead 100644 --- a/include/dbot/models/process_models/brownian_object_motion_model.hpp +++ b/include/dbot/models/process_models/brownian_object_motion_model.hpp @@ -138,8 +138,8 @@ class BrownianObjectMotionModel virtual State MapStandardGaussian(const Noise& sample) const { - State new_state(state_.body_count()); - for(size_t i = 0; i < new_state.body_count(); i++) + State new_state(state_.count()); + for(size_t i = 0; i < new_state.count(); i++) { Eigen::Matrix position_noise = sample.template middleRows<3>(i*DIMENSION_PER_OBJECT); Eigen::Matrix orientation_noise = sample.template middleRows<3>(i*DIMENSION_PER_OBJECT + 3); @@ -216,7 +216,7 @@ class BrownianObjectMotionModel const Input& control) { state_ = state; - for(size_t i = 0; i < state_.body_count(); i++) + for(size_t i = 0; i < state_.count(); i++) { quaternion_map_[i] = ff::hf::QuaternionMatrix(state_.component(i).euler_vector().quaternion().coeffs()); @@ -260,7 +260,7 @@ class BrownianObjectMotionModel virtual unsigned NoiseDimension() const { - return state_.body_count() * DIMENSION_PER_OBJECT; + return state_.count() * DIMENSION_PER_OBJECT; } virtual size_t Dimension() diff --git a/include/dbot/states/free_floating_rigid_bodies_state.hpp b/include/dbot/states/free_floating_rigid_bodies_state.hpp index e353983..41c1dc1 100644 --- a/include/dbot/states/free_floating_rigid_bodies_state.hpp +++ b/include/dbot/states/free_floating_rigid_bodies_state.hpp @@ -94,7 +94,6 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes PoseBlock; typedef Eigen::VectorBlock BodyBlock; - // give access to base member functions (otherwise it is shadowed) FreeFloatingRigidBodiesState() { } FreeFloatingRigidBodiesState(unsigned count_bodies): Base(State::Zero(count_bodies * BODY_SIZE)) { } @@ -107,46 +106,11 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypestemplate middleRows(body_index * BODY_SIZE + POSITION_INDEX); -// } -// virtual Vector euler_vector(const size_t& body_index = 0) const -// { -// return this->template middleRows(body_index * BODY_SIZE + ORIENTATION_INDEX); -// } - -// Block position(const size_t& body_index = 0) -// { -// return Block(this->derived(), body_index * BODY_SIZE + POSITION_INDEX); -// } -// Block euler_vector(const size_t& body_index = 0) -// { -// return Block(this->derived(), body_index * BODY_SIZE + ORIENTATION_INDEX); -// } - - - - -// virtual Vector linear_velocity(const size_t& body_index = 0) const -// { -// return this->template middleRows(body_index * BODY_SIZE + LINEAR_VELOCITY_INDEX); -// } -// virtual Vector angular_velocity(const size_t& body_index = 0) const -// { -// return this->template middleRows(body_index * BODY_SIZE + ANGULAR_VELOCITY_INDEX); -// } -// virtual Pose pose(const size_t& body_index = 0) const -// { -// return this->template middleRows(body_index * BODY_SIZE + POSE_INDEX); -// } + virtual Poses poses() const { - Poses poses_(body_count()*POSE_SIZE); - for(size_t body_index = 0; body_index < body_count(); body_index++) + Poses poses_(count()*POSE_SIZE); + for(size_t body_index = 0; body_index < count(); body_index++) { poses_.template middleRows(body_index * POSE_SIZE) = component(body_index).pose(); @@ -157,51 +121,13 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes(body_index * POSE_SIZE); } } -// Block linear_velocity(const size_t& body_index = 0) -// { -// return Block(this->derived(), body_index * BODY_SIZE + LINEAR_VELOCITY_INDEX); -// } -// Block angular_velocity(const size_t& body_index = 0) -// { -// return Block(this->derived(), body_index * BODY_SIZE + ANGULAR_VELOCITY_INDEX); -// } -// PoseBlock pose(const size_t& body_index = 0) -// { -// return PoseBlock(this->derived(), body_index * BODY_SIZE + POSE_INDEX); -// } -// BodyBlock operator [](const size_t& body_index) -// { -// std::cout << "operator shizzle is being used" << std::endl; - -// exit(-1); -// return BodyBlock(this->derived(), body_index * BODY_SIZE); -// } - - // other representations -// virtual void quaternion(const Quaternion& quaternion, const size_t& body_index = 0) -// { -// AngleAxis angle_axis(quaternion.normalized()); -// euler_vector(body_index) = angle_axis.angle()*angle_axis.axis(); -// } -// virtual void pose(const Affine& affine, const size_t& body_index = 0) -// { -// quaternion(Quaternion(affine.rotation()), body_index); -// position(body_index) = affine.translation(); -// } - - - virtual unsigned body_count() const - { - return ((State*)(this))->rows()/BODY_SIZE; - } - // accessors *************************************************************** virtual fl::PoseVelocityVector component(int index) const From d647c030ee1b332a49120b6e934f5935c3dbda20 Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 18 Aug 2015 20:35:01 +0200 Subject: [PATCH 012/131] still working --- .../kinect_image_observation_model_gpu_hack.hpp | 4 ++-- .../models/process_models/brownian_object_motion_model.hpp | 2 +- include/dbot/states/rigid_bodies_state.hpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp index 81835e0..665028f 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp @@ -242,7 +242,7 @@ class KinectImageObservationModelGPUHack: // convert to internal state format std::vector > > states_internal_format( n_poses_, - std::vector >(states[0].body_count(), + std::vector >(states[0].count(), std::vector(7, 0))); MEASURE("gpu: creating state vectors"); @@ -257,7 +257,7 @@ class KinectImageObservationModelGPUHack: kinematics_[tid].InitKDLData(states[state_index]); - for(size_t body_index = 0; body_index < states[state_index].body_count(); body_index++) + for(size_t body_index = 0; body_index < states[state_index].count(); body_index++) { const Eigen::Quaternion& quaternion = kinematics_[tid].GetLinkOrientation(body_index); states_internal_format[state_index][body_index][0] = quaternion.w(); diff --git a/include/dbot/models/process_models/brownian_object_motion_model.hpp b/include/dbot/models/process_models/brownian_object_motion_model.hpp index 6d0aead..e9765b7 100644 --- a/include/dbot/models/process_models/brownian_object_motion_model.hpp +++ b/include/dbot/models/process_models/brownian_object_motion_model.hpp @@ -183,7 +183,7 @@ class BrownianObjectMotionModel // const Input& control) // { // state_ = state; -// for(size_t i = 0; i < state_.body_count(); i++) +// for(size_t i = 0; i < state_.count(); i++) // { // quaternion_map_[i] = ff::hf::QuaternionMatrix(state_.component(i).euler_vector().quaternion().coeffs()); diff --git a/include/dbot/states/rigid_bodies_state.hpp b/include/dbot/states/rigid_bodies_state.hpp index 7a2e84e..4550bf7 100644 --- a/include/dbot/states/rigid_bodies_state.hpp +++ b/include/dbot/states/rigid_bodies_state.hpp @@ -95,7 +95,7 @@ class RigidBodiesState: public Eigen::Matrix // return homogeneous_matrix; // } -// virtual unsigned body_count() const = 0; +// virtual unsigned count() const = 0; virtual fl::PoseVelocityVector component(int index) const = 0; From 7fd7f0e3b6dd38df873fe415f4e499ad73477623 Mon Sep 17 00:00:00 2001 From: manuel Date: Wed, 19 Aug 2015 14:29:19 +0200 Subject: [PATCH 013/131] some renaming --- .../kinect_image_observation_model_cpu.hpp | 2 +- .../kinect_image_observation_model_gpu.hpp | 2 +- .../brownian_object_motion_model.hpp | 14 +++---- .../free_floating_rigid_bodies_state.hpp | 4 -- include/dbot/states/rigid_bodies_state.hpp | 37 ------------------- 5 files changed, 9 insertions(+), 50 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index 3fa5b76..2048a4a 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -142,7 +142,7 @@ class KinectImageObservationModelCPU: for(size_t part_index = 0; part_index < body_count; part_index++) { rotations[part_index] = - states[state_index].component(part_index).euler_vector().rotation_matrix(); + states[state_index].component(part_index).orientation().rotation_matrix(); translations[part_index] = states[state_index].component(part_index).position(); } diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 9793e27..6c2aebc 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -226,7 +226,7 @@ class KinectImageObservationModelGPU: for(size_t body_index = 0; body_index < states[state_index].count(); body_index++) { const Eigen::Quaternion& quaternion - = states[state_index].component(body_index).euler_vector().quaternion(); + = states[state_index].component(body_index).orientation().quaternion(); states_internal_format[state_index][body_index][0] = quaternion.w(); states_internal_format[state_index][body_index][1] = quaternion.x(); states_internal_format[state_index][body_index][2] = quaternion.y(); diff --git a/include/dbot/models/process_models/brownian_object_motion_model.hpp b/include/dbot/models/process_models/brownian_object_motion_model.hpp index e9765b7..4e0367d 100644 --- a/include/dbot/models/process_models/brownian_object_motion_model.hpp +++ b/include/dbot/models/process_models/brownian_object_motion_model.hpp @@ -147,14 +147,14 @@ class BrownianObjectMotionModel Eigen::Matrix angular_delta = angular_process_[i].MapStandardGaussian(orientation_noise); new_state.component(i).position() = state_.component(i).position() + linear_delta.topRows(3); - Quaternion updated_quaternion(state_.component(i).euler_vector().quaternion().coeffs() + quaternion_map_[i] * angular_delta.topRows(3)); - new_state.component(i).euler_vector().quaternion(updated_quaternion.normalized()); + Quaternion updated_quaternion(state_.component(i).orientation().quaternion().coeffs() + quaternion_map_[i] * angular_delta.topRows(3)); + new_state.component(i).orientation().quaternion(updated_quaternion.normalized()); new_state.component(i).linear_velocity() = linear_delta.bottomRows(3); new_state.component(i).angular_velocity() = angular_delta.bottomRows(3); // transform to external coordinate system new_state.component(i).linear_velocity() -= new_state.component(i).angular_velocity().cross(state_.component(i).position()); - new_state.component(i).position() -= new_state.component(i).euler_vector().rotation_matrix()*rotation_center_[i]; + new_state.component(i).position() -= new_state.component(i).orientation().rotation_matrix()*rotation_center_[i]; } return new_state; @@ -185,12 +185,12 @@ class BrownianObjectMotionModel // state_ = state; // for(size_t i = 0; i < state_.count(); i++) // { -// quaternion_map_[i] = ff::hf::QuaternionMatrix(state_.component(i).euler_vector().quaternion().coeffs()); +// quaternion_map_[i] = ff::hf::QuaternionMatrix(state_.component(i).orientation().quaternion().coeffs()); // // transform the state, which is the pose and velocity with respect to to the origin, // // into internal representation, which is the position and velocity of the center // // and the orientation and angular velocity around the center -// state_.component(i).position() += state_.component(i).euler_vector().rotation_matrix()*rotation_center_[i]; +// state_.component(i).position() += state_.component(i).orientation().rotation_matrix()*rotation_center_[i]; // state_.component(i).linear_velocity() += state_.component(i).angular_velocity().cross(state_.component(i).position()); // Eigen::Matrix linear_state; @@ -218,13 +218,13 @@ class BrownianObjectMotionModel state_ = state; for(size_t i = 0; i < state_.count(); i++) { - quaternion_map_[i] = ff::hf::QuaternionMatrix(state_.component(i).euler_vector().quaternion().coeffs()); + quaternion_map_[i] = ff::hf::QuaternionMatrix(state_.component(i).orientation().quaternion().coeffs()); // transform the state, which is the pose and velocity with respect to to the origin, // into internal representation, which is the position and velocity of the center // and the orientation and angular velocity around the center state_.component(i).position() - += state_.component(i).euler_vector().rotation_matrix()*rotation_center_[i]; + += state_.component(i).orientation().rotation_matrix()*rotation_center_[i]; state_.component(i).linear_velocity() += state_.component(i).angular_velocity().cross(state_.component(i).position()); Eigen::Matrix linear_state; diff --git a/include/dbot/states/free_floating_rigid_bodies_state.hpp b/include/dbot/states/free_floating_rigid_bodies_state.hpp index 41c1dc1..9380839 100644 --- a/include/dbot/states/free_floating_rigid_bodies_state.hpp +++ b/include/dbot/states/free_floating_rigid_bodies_state.hpp @@ -82,10 +82,6 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes Poses; - typedef typename Base::AngleAxis AngleAxis; - typedef typename Base::Quaternion Quaternion; - typedef typename Base::RotationMatrix RotationMatrix; - typedef typename Base::HomogeneousMatrix HomogeneousMatrix; typedef typename Eigen::Transform Affine; typedef typename Base::PoseVelocityBlock PoseVelocityBlock; diff --git a/include/dbot/states/rigid_bodies_state.hpp b/include/dbot/states/rigid_bodies_state.hpp index 4550bf7..aea588f 100644 --- a/include/dbot/states/rigid_bodies_state.hpp +++ b/include/dbot/states/rigid_bodies_state.hpp @@ -43,12 +43,6 @@ class RigidBodiesState: public Eigen::Matrix typedef Eigen::Matrix State; typedef Eigen::Matrix Vector; - // rotation types - typedef Eigen::AngleAxis AngleAxis; - typedef Eigen::Quaternion Quaternion; - typedef Eigen::Matrix RotationMatrix; - typedef Eigen::Matrix HomogeneousMatrix; - typedef fl::PoseVelocityBlock PoseVelocityBlock; // constructor and destructor @@ -65,37 +59,6 @@ class RigidBodiesState: public Eigen::Matrix { *((State*)(this)) = state_vector; } - -// // read -// virtual Vector position(const size_t& object_index = 0) const = 0; -// virtual Vector euler_vector(const size_t& object_index = 0) const = 0; - - // other representations -// virtual Quaternion quaternion(const size_t& object_index = 0) const -// { -// Scalar angle = euler_vector(object_index).norm(); -// Vector axis = euler_vector(object_index).normalized(); - -// if(std::isfinite(axis.norm())) -// { -// return Quaternion(AngleAxis(angle, axis)); -// } -// return Quaternion::Identity(); -// } -// virtual RotationMatrix rotation_matrix(const size_t& object_index = 0) const -// { -// return RotationMatrix(quaternion(object_index)); -// } -// virtual HomogeneousMatrix homogeneous_matrix(const size_t& object_index = 0) const -// { -// HomogeneousMatrix homogeneous_matrix(HomogeneousMatrix::Identity()); -// homogeneous_matrix.topLeftCorner(3, 3) = rotation_matrix(object_index); -// homogeneous_matrix.topRightCorner(3, 1) = position(object_index); - -// return homogeneous_matrix; -// } - -// virtual unsigned count() const = 0; virtual fl::PoseVelocityVector component(int index) const = 0; From 10b45d4e6fc10cfad09fbb0726152a2d246ad21a Mon Sep 17 00:00:00 2001 From: manuel Date: Wed, 19 Aug 2015 14:44:46 +0200 Subject: [PATCH 014/131] cleaned up state a little bit --- .../free_floating_rigid_bodies_state.hpp | 79 +++++++------------ include/dbot/states/rigid_bodies_state.hpp | 3 +- 2 files changed, 30 insertions(+), 52 deletions(-) diff --git a/include/dbot/states/free_floating_rigid_bodies_state.hpp b/include/dbot/states/free_floating_rigid_bodies_state.hpp index 9380839..ba89e24 100644 --- a/include/dbot/states/free_floating_rigid_bodies_state.hpp +++ b/include/dbot/states/free_floating_rigid_bodies_state.hpp @@ -32,6 +32,8 @@ along with this program. If not, see . #include #include +#include + #include @@ -45,12 +47,6 @@ struct FreeFloatingRigidBodiesStateTypes { BODY_SIZE = 12, POSE_SIZE = 6, - BLOCK_SIZE = 3, - POSITION_INDEX = 0, - POSE_INDEX = 0, - ORIENTATION_INDEX = 3, - LINEAR_VELOCITY_INDEX = 6, - ANGULAR_VELOCITY_INDEX = 9 }; typedef RigidBodiesState Base; @@ -58,7 +54,8 @@ struct FreeFloatingRigidBodiesStateTypes template -class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes::Base +class FreeFloatingRigidBodiesState: + public FreeFloatingRigidBodiesStateTypes::Base { public: typedef FreeFloatingRigidBodiesStateTypes Types; @@ -67,42 +64,30 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes Pose; - - typedef Eigen::Matrix Poses; - - typedef typename Eigen::Transform Affine; - + typedef Eigen::Matrix Poses; typedef typename Base::PoseVelocityBlock PoseVelocityBlock; - typedef Eigen::VectorBlock Block; - typedef Eigen::VectorBlock PoseBlock; - typedef Eigen::VectorBlock BodyBlock; - + // constructor and destructor ********************************************** FreeFloatingRigidBodiesState() { } - FreeFloatingRigidBodiesState(unsigned count_bodies): Base(State::Zero(count_bodies * BODY_SIZE)) { } - - // constructor with initial value - template FreeFloatingRigidBodiesState(const Eigen::MatrixBase& state_vector): - Base(state_vector) { } + FreeFloatingRigidBodiesState(unsigned count_bodies): + Base(State::Zero(count_bodies * BODY_SIZE)) { } + template + FreeFloatingRigidBodiesState(const Eigen::MatrixBase& state_vector): + Base(state_vector) { } virtual ~FreeFloatingRigidBodiesState() {} - - - + // accessors *************************************************************** + virtual fl::PoseVelocityVector component(int index) const + { + return PoseVelocityBlock(*((State*)(this)), + index * PoseVelocityBlock::SizeAtCompileTime); + } virtual Poses poses() const { Poses poses_(count()*POSE_SIZE); @@ -113,23 +98,6 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes(body_index * POSE_SIZE); - } - } - - - // accessors *************************************************************** - virtual fl::PoseVelocityVector component(int index) const - { - return PoseVelocityBlock(*((State*)(this)), index * PoseVelocityBlock::SizeAtCompileTime); - } int count() const { return this->size() / PoseVelocityBlock::SizeAtCompileTime; @@ -138,7 +106,16 @@ class FreeFloatingRigidBodiesState: public FreeFloatingRigidBodiesStateTypes(body_index * POSE_SIZE); + } } void recount(int new_count) { diff --git a/include/dbot/states/rigid_bodies_state.hpp b/include/dbot/states/rigid_bodies_state.hpp index aea588f..a0249ec 100644 --- a/include/dbot/states/rigid_bodies_state.hpp +++ b/include/dbot/states/rigid_bodies_state.hpp @@ -47,7 +47,8 @@ class RigidBodiesState: public Eigen::Matrix // constructor and destructor RigidBodiesState() { } - template RigidBodiesState(const Eigen::MatrixBase& state_vector) + template + RigidBodiesState(const Eigen::MatrixBase& state_vector) { *this = state_vector; } From 8282635beb981e29e4282de2d6528bdf25e02f52 Mon Sep 17 00:00:00 2001 From: manuel Date: Wed, 19 Aug 2015 20:32:27 +0200 Subject: [PATCH 015/131] cleaned renderer a bit --- include/dbot/utils/rigid_body_renderer.hpp | 12 ----- src/dbot/utils/rigid_body_renderer.cpp | 59 ++++------------------ 2 files changed, 11 insertions(+), 60 deletions(-) diff --git a/include/dbot/utils/rigid_body_renderer.hpp b/include/dbot/utils/rigid_body_renderer.hpp index 5a56b2c..addd103 100644 --- a/include/dbot/utils/rigid_body_renderer.hpp +++ b/include/dbot/utils/rigid_body_renderer.hpp @@ -66,15 +66,7 @@ class RigidBodyRenderer int n_cols, std::vector &depth_image) const; - // get functions std::vector > vertices() const; -// Vector system_center() const; - - /// \todo: this function should not be in this class - Vector object_center(const size_t& index) const; - - // set function -// virtual void state(const Eigen::VectorXd& state); virtual void set_poses(const std::vector& rotations, const std::vector& translations); @@ -92,10 +84,6 @@ class RigidBodyRenderer // state std::vector R_; std::vector t_; - - // cached center of mass - std::vector coms_; - std::vector com_weights_; }; } diff --git a/src/dbot/utils/rigid_body_renderer.cpp b/src/dbot/utils/rigid_body_renderer.cpp index d3453ae..2e29832 100644 --- a/src/dbot/utils/rigid_body_renderer.cpp +++ b/src/dbot/utils/rigid_body_renderer.cpp @@ -41,27 +41,9 @@ using namespace ff; RigidBodyRenderer::RigidBodyRenderer( const std::vector >& vertices, const std::vector > >& indices) -:vertices_(vertices), indices_(indices) + :vertices_(vertices), indices_(indices) { - /// \todo this does not belong here - float total_weight = 0; - coms_.resize(vertices.size()); - com_weights_.resize(vertices.size()); - for(size_t part_index = 0; part_index < indices_.size(); part_index++) - { - com_weights_[part_index] = vertices[part_index].size(); - total_weight += com_weights_[part_index]; - - coms_[part_index] = Vector3d::Zero(); - for(size_t vertex_index = 0; vertex_index < vertices[part_index].size(); vertex_index++) - coms_[part_index] += vertices[part_index][vertex_index]; - coms_[part_index] /= float(vertices[part_index].size()); - } - for(size_t i = 0; i < com_weights_.size(); i++) - com_weights_[i] /= total_weight; - - - // initialize poses + /// initialize poses ******************************************************* R_.resize(vertices_.size()); t_.resize(vertices_.size()); @@ -71,9 +53,7 @@ RigidBodyRenderer::RigidBodyRenderer( t_[i] = Vector::Zero(); } - - - + /// compute normals ******************************************************** normals_.clear(); for(size_t part_index = 0; part_index < indices_.size(); part_index++) { @@ -264,46 +244,29 @@ void RigidBodyRenderer::Render(Matrix camera_matrix, } - -// get functions -std::vector > RigidBodyRenderer::vertices() const +std::vector > + RigidBodyRenderer::vertices() const { vector > trans_vertices(vertices_.size()); - for(int part_index = 0; part_index < int(vertices_.size()); part_index++) + for(int o = 0; o < int(vertices_.size()); o++) { - - trans_vertices[part_index].resize(vertices_[part_index].size()); - for(int point_index = 0; point_index < int(vertices_[part_index].size()); point_index++) - trans_vertices[part_index][point_index] = R_[part_index] * vertices_[part_index][point_index] + t_[part_index]; + trans_vertices[o].resize(vertices_[o].size()); + for(int p = 0; p < int(vertices_[o].size()); p++) + { + trans_vertices[o][p] = R_[o] * vertices_[o][p] + t_[o]; + } } return trans_vertices; } -//RigidBodyRenderer::Vector RigidBodyRenderer::system_center() const -//{ -// Eigen::Vector3d com = Eigen::Vector3d::Zero(); -// for(size_t i = 0; i < coms_.size(); i++) -// com += com_weights_[i] * (R_[i]*coms_[i] + t_[i]); -// com = R_[0].inverse() * (com - t_[0]); -// return com; -//} -RigidBodyRenderer::Vector RigidBodyRenderer::object_center(const size_t& index) const -{ - return R_[index]*coms_[index] + t_[index]; -} - - -// set state void RigidBodyRenderer::set_poses(const std::vector& rotations, const std::vector& translations) { R_ = rotations; t_ = translations; } - -/// \todo: th void RigidBodyRenderer::set_poses(const std::vector& poses) { R_.resize(poses.size()); From 1a77dc7a1939889e73a0a85ec3e817652346584c Mon Sep 17 00:00:00 2001 From: manuel Date: Thu, 20 Aug 2015 18:16:02 +0200 Subject: [PATCH 016/131] cleaned cpf a bit --- ...o_blackwell_coordinate_particle_filter.hpp | 62 ++++++++++--------- 1 file changed, 33 insertions(+), 29 deletions(-) diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index 9c3170c..b44b927 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -37,20 +37,16 @@ along with this program. If not, see . #include #include +#include +#include + #include #include #include -//#include -//#include -//#include #include -//#include -#include -#include - namespace ff { @@ -65,9 +61,6 @@ class RaoBlackwellCoordinateParticleFilter typedef typename ObservationModel::Observation Observation; - // state distribution -// typedef SumOfDeltas StateDistributionType; - typedef fl::DiscreteDistribution Belief; public: @@ -92,34 +85,46 @@ class RaoBlackwellCoordinateParticleFilter observation_model_->SetObservation(observation); loglikes_ = std::vector(samples_.size(), 0); - noises_ = std::vector(samples_.size(), Noise::Zero(process_model_->NoiseDimension())); + noises_ = std::vector(samples_.size(), + Noise::Zero(process_model_->NoiseDimension())); next_samples_ = samples_; - for(size_t block_index = 0; block_index < sampling_blocks_.size(); block_index++) + for(size_t i_block = 0; i_block < sampling_blocks_.size(); i_block++) { INIT_PROFILING; - for(size_t particle_index = 0; particle_index < samples_.size(); particle_index++) + // add noise of this block ----------------------------------------- + for(size_t i_sampl = 0; i_sampl < samples_.size(); i_sampl++) { - for(size_t i = 0; i < sampling_blocks_[block_index].size(); i++) - noises_[particle_index](sampling_blocks_[block_index][i]) = unit_gaussian_.sample()(0); + for(size_t i = 0; i < sampling_blocks_[i_block].size(); i++) + { + noises_[i_sampl](sampling_blocks_[i_block][i]) = + unit_gaussian_.sample()(0); + } } MEASURE("sampling"); - for(size_t particle_index = 0; particle_index < samples_.size(); particle_index++) + + // propagate using partial noise ----------------------------------- + for(size_t i_sampl = 0; i_sampl < samples_.size(); i_sampl++) { - process_model_->Condition(samples_[particle_index], - input); - next_samples_[particle_index] = process_model_->MapStandardGaussian(noises_[particle_index]); + process_model_->Condition(samples_[i_sampl], input); + next_samples_[i_sampl] = + process_model_->MapStandardGaussian(noises_[i_sampl]); } MEASURE("propagation"); - bool update_occlusions = (block_index == sampling_blocks_.size()-1); - std::vector new_loglikes = observation_model_->Loglikes(next_samples_, - indices_, - update_occlusions); + // compute likelihood ---------------------------------------------- + bool update_occlusions = (i_block == sampling_blocks_.size()-1); + std::vector new_loglikes = + observation_model_->Loglikes(next_samples_, + indices_, update_occlusions); MEASURE("evaluation"); + + // update the weights and resample if necessary -------------------- std::vector delta_loglikes(new_loglikes.size()); for(size_t i = 0; i < delta_loglikes.size(); i++) + { delta_loglikes[i] = new_loglikes[i] - loglikes_[i]; + } loglikes_ = new_loglikes; UpdateWeights(delta_loglikes); MEASURE("updating weights"); @@ -128,8 +133,6 @@ class RaoBlackwellCoordinateParticleFilter samples_ = next_samples_; - - belief_.set_uniform(samples_.size()); for(int i = 0; i < belief_.size(); i++) belief_.location(i) = samples_[i]; @@ -177,7 +180,9 @@ class RaoBlackwellCoordinateParticleFilter // belief_.SetDeltas(samples_); // not sure whether this is the right place } + private: + void UpdateWeights(std::vector log_weight_diffs) { for(size_t i = 0; i < log_weight_diffs.size(); i++) @@ -253,13 +258,12 @@ class RaoBlackwellCoordinateParticleFilter std::vector log_weights_; std::vector noises_; std::vector next_samples_; + std::vector loglikes_; - // observation model + // models boost::shared_ptr observation_model_; - - // process model - boost::shared_ptr process_model_; + boost::shared_ptr process_model_; // parameters std::vector > sampling_blocks_; From 0c576af097a4c57287e4ed538ab6b9d4b9396132 Mon Sep 17 00:00:00 2001 From: manuel Date: Thu, 20 Aug 2015 20:07:50 +0200 Subject: [PATCH 017/131] cleaning the cpf --- ...o_blackwell_coordinate_particle_filter.hpp | 55 ++++++++++--------- 1 file changed, 30 insertions(+), 25 deletions(-) diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index b44b927..2b6b28a 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -84,16 +84,19 @@ class RaoBlackwellCoordinateParticleFilter { observation_model_->SetObservation(observation); - loglikes_ = std::vector(samples_.size(), 0); - noises_ = std::vector(samples_.size(), + loglikes_ = std::vector(belief_.size(), 0); + noises_ = std::vector(belief_.size(), Noise::Zero(process_model_->NoiseDimension())); - next_samples_ = samples_; + + next_samples_.resize(belief_.size()); + for(int i = 0; i < belief_.size(); i++) + next_samples_[i] = belief_.location(i); for(size_t i_block = 0; i_block < sampling_blocks_.size(); i_block++) { INIT_PROFILING; // add noise of this block ----------------------------------------- - for(size_t i_sampl = 0; i_sampl < samples_.size(); i_sampl++) + for(size_t i_sampl = 0; i_sampl < belief_.size(); i_sampl++) { for(size_t i = 0; i < sampling_blocks_[i_block].size(); i++) { @@ -104,9 +107,9 @@ class RaoBlackwellCoordinateParticleFilter MEASURE("sampling"); // propagate using partial noise ----------------------------------- - for(size_t i_sampl = 0; i_sampl < samples_.size(); i_sampl++) + for(size_t i_sampl = 0; i_sampl < belief_.size(); i_sampl++) { - process_model_->Condition(samples_[i_sampl], input); + process_model_->Condition(belief_.location(i_sampl), input); next_samples_[i_sampl] = process_model_->MapStandardGaussian(noises_[i_sampl]); } @@ -131,11 +134,10 @@ class RaoBlackwellCoordinateParticleFilter } - samples_ = next_samples_; - - belief_.set_uniform(samples_.size()); + /// \todo this is temporary + belief_.set_uniform(next_samples_.size()); for(int i = 0; i < belief_.size(); i++) - belief_.location(i) = samples_[i]; + belief_.location(i) = next_samples_[i]; // belief_.SetDeltas(samples_); // not sure whether this is the right place @@ -155,26 +157,25 @@ class RaoBlackwellCoordinateParticleFilter { size_t index = sampler.Sample(); - samples[i] = samples_[index]; + samples[i] = belief_.location(index); indices[i] = indices_[index]; noises[i] = noises_[index]; next_samples[i] = next_samples_[index]; loglikes[i] = loglikes_[index]; } - samples_ = samples; indices_ = indices; noises_ = noises; next_samples_ = next_samples; loglikes_ = loglikes; - log_weights_ = std::vector(samples_.size(), 0.); + log_weights_ = std::vector(sample_count, 0.); - belief_.set_uniform(samples_.size()); + belief_.set_uniform(samples.size()); for(int i = 0; i < belief_.size(); i++) - belief_.location(i) = samples_[i]; + belief_.location(i) = samples[i]; // belief_.SetDeltas(samples_); // not sure whether this is the right place @@ -208,16 +209,21 @@ class RaoBlackwellCoordinateParticleFilter } if(kl_divergence > max_kl_divergence_) - Resample(samples_.size()); + Resample(belief_.size()); } public: // set void Samples(const std::vector& samples) { - samples_ = samples; - indices_ = std::vector(samples_.size(), 0); observation_model_->Reset(); - log_weights_ = std::vector(samples_.size(), 0); + belief_.set_uniform(samples.size()); + for(int i = 0; i < belief_.size(); i++) + belief_.location(i) = samples[i]; + + log_weights_ = std::vector(samples.size(), 0); + + indices_ = std::vector(samples.size(), 0); + observation_model_->Reset(); } void SamplingBlocks(const std::vector >& sampling_blocks) { @@ -238,11 +244,11 @@ class RaoBlackwellCoordinateParticleFilter } } - // get - const std::vector& Samples() const - { - return samples_; - } +// // get +// const std::vector& Samples() const +// { +// return samples_; +// } Belief& StateDistribution() { @@ -253,7 +259,6 @@ class RaoBlackwellCoordinateParticleFilter // internal state TODO: THIS COULD BE MADE MORE COMPACT!! Belief belief_; - std::vector samples_; std::vector indices_; std::vector log_weights_; std::vector noises_; From b232a79def694160348e9d7ad7cc2568ae94e152 Mon Sep 17 00:00:00 2001 From: manuel Date: Fri, 21 Aug 2015 19:01:33 +0200 Subject: [PATCH 018/131] using now the discrete distribution properly inside of cpf --- ...o_blackwell_coordinate_particle_filter.hpp | 61 +++---------------- 1 file changed, 10 insertions(+), 51 deletions(-) diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index 2b6b28a..4f70ffc 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -134,51 +134,34 @@ class RaoBlackwellCoordinateParticleFilter } - /// \todo this is temporary - belief_.set_uniform(next_samples_.size()); for(int i = 0; i < belief_.size(); i++) belief_.location(i) = next_samples_[i]; - -// belief_.SetDeltas(samples_); // not sure whether this is the right place - } void Resample(const size_t& sample_count) { - std::vector samples(sample_count); std::vector indices(sample_count); std::vector noises(sample_count); std::vector next_samples(sample_count); std::vector loglikes(sample_count); - ff::hf::DiscreteSampler sampler(log_weights_); + Belief belief(sample_count); for(size_t i = 0; i < sample_count; i++) { - size_t index = sampler.Sample(); + int index; + belief.location(i) = belief_.sample(index); - samples[i] = belief_.location(index); indices[i] = indices_[index]; noises[i] = noises_[index]; next_samples[i] = next_samples_[index]; loglikes[i] = loglikes_[index]; } + belief_ = belief; indices_ = indices; noises_ = noises; next_samples_ = next_samples; loglikes_ = loglikes; - - log_weights_ = std::vector(sample_count, 0.); - - - - - belief_.set_uniform(samples.size()); - for(int i = 0; i < belief_.size(); i++) - belief_.location(i) = samples[i]; - - -// belief_.SetDeltas(samples_); // not sure whether this is the right place } @@ -186,30 +169,16 @@ class RaoBlackwellCoordinateParticleFilter void UpdateWeights(std::vector log_weight_diffs) { - for(size_t i = 0; i < log_weight_diffs.size(); i++) - log_weights_[i] += log_weight_diffs[i]; - - std::vector weights = log_weights_; - ff::hf::Sort(weights, 1); + typename Belief::Function delta_weights(log_weight_diffs.size()); + for(size_t i = 0; i < delta_weights.size(); i++) + delta_weights[i] = log_weight_diffs[i]; - for(int i = weights.size() - 1; i >= 0; i--) - weights[i] -= weights[0]; + belief_.delta_log_prob_mass(delta_weights); - weights = ff::hf::Apply(weights, std::exp); - weights = ff::hf::SetSum(weights, Scalar(1)); - - // compute KL divergence to uniform distribution KL(p|u) - Scalar kl_divergence = std::log(Scalar(weights.size())); - for(size_t i = 0; i < weights.size(); i++) + if(belief_.kl_given_uniform() > max_kl_divergence_) { - Scalar information = - std::log(weights[i]) * weights[i]; - if(!std::isfinite(information)) - information = 0; // the limit for weight -> 0 is equal to 0 - kl_divergence -= information; - } - - if(kl_divergence > max_kl_divergence_) Resample(belief_.size()); + } } public: @@ -220,8 +189,6 @@ class RaoBlackwellCoordinateParticleFilter for(int i = 0; i < belief_.size(); i++) belief_.location(i) = samples[i]; - log_weights_ = std::vector(samples.size(), 0); - indices_ = std::vector(samples.size(), 0); observation_model_->Reset(); } @@ -244,23 +211,15 @@ class RaoBlackwellCoordinateParticleFilter } } -// // get -// const std::vector& Samples() const -// { -// return samples_; -// } - Belief& StateDistribution() { return belief_; } private: - // internal state TODO: THIS COULD BE MADE MORE COMPACT!! Belief belief_; std::vector indices_; - std::vector log_weights_; std::vector noises_; std::vector next_samples_; From 8d91b361ecb25f379c3294b5c9dcbbd10bb055b6 Mon Sep 17 00:00:00 2001 From: manuel Date: Fri, 21 Aug 2015 19:16:48 +0200 Subject: [PATCH 019/131] some more clean up in cpf --- ...o_blackwell_coordinate_particle_filter.hpp | 67 ++++++++++--------- 1 file changed, 36 insertions(+), 31 deletions(-) diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index 4f70ffc..42409ba 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -36,7 +36,7 @@ along with this program. If not, see . #include -#include +#include #include #include @@ -54,7 +54,6 @@ template class RaoBlackwellCoordinateParticleFilter { public: - typedef typename internal::Traits::Scalar Scalar; typedef typename internal::Traits::State State; typedef typename internal::Traits::Input Input; typedef typename internal::Traits::Noise Noise; @@ -65,10 +64,10 @@ class RaoBlackwellCoordinateParticleFilter public: RaoBlackwellCoordinateParticleFilter( - const boost::shared_ptr process_model, - const boost::shared_ptr observation_model, - const std::vector >& sampling_blocks, - const Scalar& max_kl_divergence = 0): + const boost::shared_ptr process_model, + const boost::shared_ptr observation_model, + const std::vector >& sampling_blocks, + const fl::Real& max_kl_divergence = 0): observation_model_(observation_model), process_model_(process_model), max_kl_divergence_(max_kl_divergence) @@ -84,7 +83,7 @@ class RaoBlackwellCoordinateParticleFilter { observation_model_->SetObservation(observation); - loglikes_ = std::vector(belief_.size(), 0); + loglikes_ = std::vector(belief_.size(), 0); noises_ = std::vector(belief_.size(), Noise::Zero(process_model_->NoiseDimension())); @@ -117,13 +116,13 @@ class RaoBlackwellCoordinateParticleFilter // compute likelihood ---------------------------------------------- bool update_occlusions = (i_block == sampling_blocks_.size()-1); - std::vector new_loglikes = + std::vector new_loglikes = observation_model_->Loglikes(next_samples_, indices_, update_occlusions); MEASURE("evaluation"); // update the weights and resample if necessary -------------------- - std::vector delta_loglikes(new_loglikes.size()); + std::vector delta_loglikes(new_loglikes.size()); for(size_t i = 0; i < delta_loglikes.size(); i++) { delta_loglikes[i] = new_loglikes[i] - loglikes_[i]; @@ -131,33 +130,40 @@ class RaoBlackwellCoordinateParticleFilter loglikes_ = new_loglikes; UpdateWeights(delta_loglikes); MEASURE("updating weights"); - } for(int i = 0; i < belief_.size(); i++) + { belief_.location(i) = next_samples_[i]; + } + } + + Belief& belief() + { + return belief_; } + void Resample(const size_t& sample_count) { std::vector indices(sample_count); std::vector noises(sample_count); std::vector next_samples(sample_count); - std::vector loglikes(sample_count); + std::vector loglikes(sample_count); - Belief belief(sample_count); + Belief new_belief(sample_count); for(size_t i = 0; i < sample_count; i++) { int index; - belief.location(i) = belief_.sample(index); + new_belief.location(i) = belief_.sample(index); indices[i] = indices_[index]; noises[i] = noises_[index]; next_samples[i] = next_samples_[index]; loglikes[i] = loglikes_[index]; } - belief_ = belief; + belief_ = new_belief; indices_ = indices; noises_ = noises; next_samples_ = next_samples; @@ -167,7 +173,7 @@ class RaoBlackwellCoordinateParticleFilter private: - void UpdateWeights(std::vector log_weight_diffs) + void UpdateWeights(std::vector log_weight_diffs) { typename Belief::Function delta_weights(log_weight_diffs.size()); for(size_t i = 0; i < delta_weights.size(); i++) @@ -181,17 +187,6 @@ class RaoBlackwellCoordinateParticleFilter } } -public: - // set - void Samples(const std::vector& samples) - { - belief_.set_uniform(samples.size()); - for(int i = 0; i < belief_.size(); i++) - belief_.location(i) = samples[i]; - - indices_ = std::vector(samples.size(), 0); - observation_model_->Reset(); - } void SamplingBlocks(const std::vector >& sampling_blocks) { sampling_blocks_ = sampling_blocks; @@ -211,11 +206,21 @@ class RaoBlackwellCoordinateParticleFilter } } - Belief& StateDistribution() +public: + // set + void Samples(const std::vector& samples) { - return belief_; + belief_.set_uniform(samples.size()); + for(int i = 0; i < belief_.size(); i++) + belief_.location(i) = samples[i]; + + indices_ = std::vector(samples.size(), 0); + observation_model_->Reset(); } + + + private: Belief belief_; @@ -223,7 +228,7 @@ class RaoBlackwellCoordinateParticleFilter std::vector noises_; std::vector next_samples_; - std::vector loglikes_; + std::vector loglikes_; // models boost::shared_ptr observation_model_; @@ -231,10 +236,10 @@ class RaoBlackwellCoordinateParticleFilter // parameters std::vector > sampling_blocks_; - Scalar max_kl_divergence_; + fl::Real max_kl_divergence_; // distribution for sampling - fl::Gaussian > unit_gaussian_; + fl::Gaussian > unit_gaussian_; }; } From bd93b9f887e21f75e8b9dd1bb12f0e55c3f2a855 Mon Sep 17 00:00:00 2001 From: manuel Date: Fri, 21 Aug 2015 19:23:49 +0200 Subject: [PATCH 020/131] some renaming --- ...o_blackwell_coordinate_particle_filter.hpp | 92 ++++++++----------- 1 file changed, 37 insertions(+), 55 deletions(-) diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index 42409ba..e419837 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -72,13 +72,26 @@ class RaoBlackwellCoordinateParticleFilter process_model_(process_model), max_kl_divergence_(max_kl_divergence) { - SamplingBlocks(sampling_blocks); - } + sampling_blocks_ = sampling_blocks; + // make sure sizes are consistent + size_t dimension = 0; + for(size_t i = 0; i < sampling_blocks_.size(); i++) + for(size_t j = 0; j < sampling_blocks_[i].size(); j++) + dimension++; + + if(dimension != process_model_->NoiseDimension()) + { + std::cout << "the dimension of the sampling blocks is " << dimension + << " while the dimension of the noise is " + << process_model_->NoiseDimension() << std::endl; + exit(-1); + } + } virtual ~RaoBlackwellCoordinateParticleFilter() {} public: - void Filter(const Observation& observation, + void filter(const Observation& observation, const Input& input) { observation_model_->SetObservation(observation); @@ -128,7 +141,17 @@ class RaoBlackwellCoordinateParticleFilter delta_loglikes[i] = new_loglikes[i] - loglikes_[i]; } loglikes_ = new_loglikes; - UpdateWeights(delta_loglikes); + + typename Belief::Function delta_weights(delta_loglikes.size()); + for(size_t i = 0; i < delta_weights.size(); i++) + delta_weights[i] = delta_loglikes[i]; + + belief_.delta_log_prob_mass(delta_weights); + + if(belief_.kl_given_uniform() > max_kl_divergence_) + { + resample(belief_.size()); + } MEASURE("updating weights"); } @@ -143,8 +166,17 @@ class RaoBlackwellCoordinateParticleFilter return belief_; } + void set_particles(const std::vector& samples) + { + belief_.set_uniform(samples.size()); + for(int i = 0; i < belief_.size(); i++) + belief_.location(i) = samples[i]; - void Resample(const size_t& sample_count) + indices_ = std::vector(samples.size(), 0); + observation_model_->Reset(); + } + + void resample(const size_t& sample_count) { std::vector indices(sample_count); std::vector noises(sample_count); @@ -171,56 +203,6 @@ class RaoBlackwellCoordinateParticleFilter } -private: - - void UpdateWeights(std::vector log_weight_diffs) - { - typename Belief::Function delta_weights(log_weight_diffs.size()); - for(size_t i = 0; i < delta_weights.size(); i++) - delta_weights[i] = log_weight_diffs[i]; - - belief_.delta_log_prob_mass(delta_weights); - - if(belief_.kl_given_uniform() > max_kl_divergence_) - { - Resample(belief_.size()); - } - } - - void SamplingBlocks(const std::vector >& sampling_blocks) - { - sampling_blocks_ = sampling_blocks; - - // make sure sizes are consistent - size_t dimension = 0; - for(size_t i = 0; i < sampling_blocks_.size(); i++) - for(size_t j = 0; j < sampling_blocks_[i].size(); j++) - dimension++; - - if(dimension != process_model_->NoiseDimension()) - { - std::cout << "the dimension of the sampling blocks is " << dimension - << " while the dimension of the noise is " - << process_model_->NoiseDimension() << std::endl; - exit(-1); - } - } - -public: - // set - void Samples(const std::vector& samples) - { - belief_.set_uniform(samples.size()); - for(int i = 0; i < belief_.size(); i++) - belief_.location(i) = samples[i]; - - indices_ = std::vector(samples.size(), 0); - observation_model_->Reset(); - } - - - - private: Belief belief_; From 0af4b4c1de7409c9309c318aecc07d3254c285c4 Mon Sep 17 00:00:00 2001 From: manuel Date: Fri, 21 Aug 2015 20:26:40 +0200 Subject: [PATCH 021/131] cleaned cpf a bit more --- ...o_blackwell_coordinate_particle_filter.hpp | 73 ++++++++++--------- 1 file changed, 37 insertions(+), 36 deletions(-) diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index e419837..df002ec 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -51,19 +51,23 @@ namespace ff { template -class RaoBlackwellCoordinateParticleFilter +class RBCoordinateParticleFilter { public: typedef typename internal::Traits::State State; typedef typename internal::Traits::Input Input; typedef typename internal::Traits::Noise Noise; + typedef typename ObservationModel::Observation Observation; typedef fl::DiscreteDistribution Belief; + typedef typename Belief::Function List; + public: - RaoBlackwellCoordinateParticleFilter( + /// constructor and destructor ********************************************* + RBCoordinateParticleFilter( const boost::shared_ptr process_model, const boost::shared_ptr observation_model, const std::vector >& sampling_blocks, @@ -88,15 +92,15 @@ class RaoBlackwellCoordinateParticleFilter exit(-1); } } - virtual ~RaoBlackwellCoordinateParticleFilter() {} + virtual ~RBCoordinateParticleFilter() {} -public: + /// the filter functions *************************************************** void filter(const Observation& observation, const Input& input) { observation_model_->SetObservation(observation); - loglikes_ = std::vector(belief_.size(), 0); + loglikes_ = List::Zero(belief_.size()); noises_ = std::vector(belief_.size(), Noise::Zero(process_model_->NoiseDimension())); @@ -129,29 +133,25 @@ class RaoBlackwellCoordinateParticleFilter // compute likelihood ---------------------------------------------- bool update_occlusions = (i_block == sampling_blocks_.size()-1); - std::vector new_loglikes = + std::vector new_loglikes_std = observation_model_->Loglikes(next_samples_, indices_, update_occlusions); - MEASURE("evaluation"); - // update the weights and resample if necessary -------------------- - std::vector delta_loglikes(new_loglikes.size()); - for(size_t i = 0; i < delta_loglikes.size(); i++) + List new_loglikes(new_loglikes_std.size()); + for(size_t i = 0; i < new_loglikes.size(); i++) { - delta_loglikes[i] = new_loglikes[i] - loglikes_[i]; + new_loglikes[i] = new_loglikes_std[i]; } - loglikes_ = new_loglikes; - - typename Belief::Function delta_weights(delta_loglikes.size()); - for(size_t i = 0; i < delta_weights.size(); i++) - delta_weights[i] = delta_loglikes[i]; + MEASURE("evaluation"); - belief_.delta_log_prob_mass(delta_weights); + // update the weights and resample if necessary -------------------- + belief_.delta_log_prob_mass(new_loglikes - loglikes_); if(belief_.kl_given_uniform() > max_kl_divergence_) { resample(belief_.size()); } + loglikes_ = new_loglikes; MEASURE("updating weights"); } @@ -161,27 +161,12 @@ class RaoBlackwellCoordinateParticleFilter } } - Belief& belief() - { - return belief_; - } - - void set_particles(const std::vector& samples) - { - belief_.set_uniform(samples.size()); - for(int i = 0; i < belief_.size(); i++) - belief_.location(i) = samples[i]; - - indices_ = std::vector(samples.size(), 0); - observation_model_->Reset(); - } - void resample(const size_t& sample_count) { std::vector indices(sample_count); std::vector noises(sample_count); std::vector next_samples(sample_count); - std::vector loglikes(sample_count); + List loglikes(sample_count); Belief new_belief(sample_count); @@ -203,14 +188,30 @@ class RaoBlackwellCoordinateParticleFilter } + /// mutators *************************************************************** + Belief& belief() + { + return belief_; + } + + void set_particles(const std::vector& samples) + { + belief_.set_uniform(samples.size()); + for(int i = 0; i < belief_.size(); i++) + belief_.location(i) = samples[i]; + + indices_ = std::vector(samples.size(), 0); + observation_model_->Reset(); + } + + private: Belief belief_; - std::vector indices_; + std::vector noises_; std::vector next_samples_; - - std::vector loglikes_; + List loglikes_; // models boost::shared_ptr observation_model_; From 8d5be2d053f5e5d040226c2ac84ac4cbe34b2930 Mon Sep 17 00:00:00 2001 From: manuel Date: Sat, 22 Aug 2015 16:27:15 +0200 Subject: [PATCH 022/131] switched to using eigen arrays instead of std in observatio model. but something is strange, multi object tracking performance seems reduced --- .../kinect_image_observation_model_cpu.hpp | 14 +++++-- .../kinect_image_observation_model_gpu.hpp | 16 +++++--- ...inect_image_observation_model_gpu_hack.hpp | 16 +++++--- .../rao_blackwell_observation_model.hpp | 38 ++++++++++--------- ...o_blackwell_coordinate_particle_filter.hpp | 26 +++++++------ 5 files changed, 65 insertions(+), 45 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index 2048a4a..d7a3a19 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -87,6 +87,12 @@ class KinectImageObservationModelCPU: typedef typename Traits::PixelObservationModelPtr PixelObservationModelPtr; typedef typename Traits::OcclusionProcessModelPtr OcclusionProcessModelPtr; + typedef typename Base::StateArray StateArray; + typedef typename Base::RealArray RealArray; + typedef typename Base::IntArray IntArray; + + + // TODO: DO WE NEED ALL OF THIS IN THE CONSTRUCTOR?? KinectImageObservationModelCPU( const Eigen::Matrix3d& camera_matrix, @@ -116,13 +122,15 @@ class KinectImageObservationModelCPU: ~KinectImageObservationModelCPU() { } - std::vector Loglikes(const std::vector& states, - std::vector& indices, + RealArray Loglikes(const StateArray& states, + IntArray& indices, const bool& update = false) { std::vector > new_occlusions(states.size()); std::vector > new_occlusion_times(states.size()); - std::vector loglikes(states.size(),0); + + RealArray loglikes = RealArray::Zero(states.size()); +// std::vector loglikes(states.size(),0); for(size_t state_index = 0; state_index < size_t(states.size()); state_index++) { if(update) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 6c2aebc..6f03d65 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -38,7 +38,7 @@ struct Traits > typedef double Scalar; typedef Eigen::Matrix Observation; - typedef RaoBlackwellObservationModel ObservationModelBase; + typedef RaoBlackwellObservationModel Base; typedef typename Eigen::Matrix CameraMatrix; @@ -54,7 +54,7 @@ struct Traits > */ template class KinectImageObservationModelGPU: - public internal::Traits >::ObservationModelBase + public internal::Traits >::Base { public: typedef internal::Traits > Traits; @@ -63,6 +63,10 @@ class KinectImageObservationModelGPU: typedef typename Traits::Observation Observation; typedef typename Traits::CameraMatrix CameraMatrix; + typedef typename Traits::Base::StateArray StateArray; + typedef typename Traits::Base::RealArray RealArray; + typedef typename Traits::Base::IntArray IntArray; + // TODO: ALL THIS SHOULD SWITCH FROM USING VISIBILITY TO OCCLUSION KinectImageObservationModelGPU(const CameraMatrix& camera_matrix, @@ -83,7 +87,7 @@ class KinectImageObservationModelGPU: resource_registered_(false), nr_calls_set_observation_(0), observation_time_(0), - Traits::ObservationModelBase(delta_time) + Traits::Base(delta_time) { visibility_probs_.resize(n_rows_ * n_cols_); } @@ -182,8 +186,8 @@ class KinectImageObservationModelGPU: constants_set_ = true; } - std::vector Loglikes(const std::vector& states, - std::vector& occlusion_indices, + RealArray Loglikes(const StateArray& states, + IntArray& occlusion_indices, const bool& update_occlusions = false) { if (!initialized_) @@ -269,7 +273,7 @@ class KinectImageObservationModelGPU: // convert - std::vector log_likelihoods(flog_likelihoods.size()); + RealArray log_likelihoods(flog_likelihoods.size()); for(size_t i = 0; i < flog_likelihoods.size(); i++) log_likelihoods[i] = flog_likelihoods[i]; diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp index 665028f..b96fc8f 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp @@ -41,7 +41,7 @@ struct Traits > typedef double Scalar; typedef Eigen::Matrix Observation; - typedef RaoBlackwellObservationModel ObservationModelBase; + typedef RaoBlackwellObservationModel Base; typedef typename Eigen::Matrix CameraMatrix; @@ -57,7 +57,7 @@ struct Traits > */ template class KinectImageObservationModelGPUHack: - public internal::Traits >::ObservationModelBase + public internal::Traits >::Base { public: typedef internal::Traits > Traits; @@ -66,6 +66,10 @@ class KinectImageObservationModelGPUHack: typedef typename Traits::Observation Observation; typedef typename Traits::CameraMatrix CameraMatrix; + typedef typename Traits::Base::StateArray StateArray; + typedef typename Traits::Base::RealArray RealArray; + typedef typename Traits::Base::IntArray IntArray; + // TODO: ALL THIS SHOULD SWITCH FROM USING VISIBILITY TO OCCLUSION KinectImageObservationModelGPUHack(const CameraMatrix& camera_matrix, @@ -86,7 +90,7 @@ class KinectImageObservationModelGPUHack: resource_registered_(false), nr_calls_set_observation_(0), observation_time_(0), - Traits::ObservationModelBase(delta_time) + Traits::Base(delta_time) { visibility_probs_.resize(n_rows_ * n_cols_); } @@ -208,8 +212,8 @@ class KinectImageObservationModelGPUHack: constants_set_ = true; } - std::vector Loglikes(const std::vector& states, - std::vector& occlusion_indices, + RealArray Loglikes(const StateArray& states, + IntArray& occlusion_indices, const bool& update_occlusions = false) { if (!initialized_) @@ -302,7 +306,7 @@ class KinectImageObservationModelGPUHack: // convert - std::vector log_likelihoods(flog_likelihoods.size()); + RealArray log_likelihoods(flog_likelihoods.size()); for(size_t i = 0; i < flog_likelihoods.size(); i++) log_likelihoods[i] = flog_likelihoods[i]; diff --git a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp index ff1bcf8..d9a4f29 100644 --- a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp +++ b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp @@ -29,18 +29,18 @@ along with this program. If not, see . #ifndef FAST_FILTERING_MODELS_OBSERVATION_MODELS_INTERFACES_RAO_BLACKWELL_OBSERVATION_MODEL_HPP #define FAST_FILTERING_MODELS_OBSERVATION_MODELS_INTERFACES_RAO_BLACKWELL_OBSERVATION_MODEL_HPP -#include -#include +//#include +#include + +#include +//#include + + namespace ff { -/** - * Rao-Blackwellized particle filter observation model interface - * - * \ingroup observation_models - */ template class RaoBlackwellObservationModel { @@ -48,25 +48,27 @@ class RaoBlackwellObservationModel typedef State_ State; typedef Observation_ Observation; -public: - RaoBlackwellObservationModel(const double& delta_time): - delta_time_(delta_time) - {} + typedef Eigen::Array StateArray; + typedef Eigen::Array RealArray; + typedef Eigen::Array IntArray; +public: + /// constructor and destructor ********************************************* + RaoBlackwellObservationModel(const fl::Real& delta_time): + delta_time_(delta_time) { } virtual ~RaoBlackwellObservationModel() { } - virtual std::vector Loglikes(const std::vector& states, - std::vector& indices, - const bool& update = false) = 0; + /// likelihood computation ************************************************* + virtual RealArray Loglikes(const StateArray& states, + IntArray& indices, + const bool& update = false) = 0; + /// accessors ************************************************************** virtual void SetObservation(const Observation& image) = 0; - - // reset the latent variables virtual void Reset() = 0; - protected: - double delta_time_; + fl::Real delta_time_; }; } diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index df002ec..ded2989 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -58,13 +58,15 @@ class RBCoordinateParticleFilter typedef typename internal::Traits::Input Input; typedef typename internal::Traits::Noise Noise; + typedef Eigen::Array StateArray; + typedef Eigen::Array RealArray; + typedef Eigen::Array IntArray; + typedef typename ObservationModel::Observation Observation; typedef fl::DiscreteDistribution Belief; - typedef typename Belief::Function List; - public: /// constructor and destructor ********************************************* RBCoordinateParticleFilter( @@ -100,7 +102,7 @@ class RBCoordinateParticleFilter { observation_model_->SetObservation(observation); - loglikes_ = List::Zero(belief_.size()); + loglikes_ = RealArray::Zero(belief_.size()); noises_ = std::vector(belief_.size(), Noise::Zero(process_model_->NoiseDimension())); @@ -133,11 +135,11 @@ class RBCoordinateParticleFilter // compute likelihood ---------------------------------------------- bool update_occlusions = (i_block == sampling_blocks_.size()-1); - std::vector new_loglikes_std = + RealArray new_loglikes_std = observation_model_->Loglikes(next_samples_, indices_, update_occlusions); - List new_loglikes(new_loglikes_std.size()); + RealArray new_loglikes(new_loglikes_std.size()); for(size_t i = 0; i < new_loglikes.size(); i++) { new_loglikes[i] = new_loglikes_std[i]; @@ -163,10 +165,10 @@ class RBCoordinateParticleFilter void resample(const size_t& sample_count) { - std::vector indices(sample_count); + IntArray indices(sample_count); std::vector noises(sample_count); - std::vector next_samples(sample_count); - List loglikes(sample_count); + StateArray next_samples(sample_count); + RealArray loglikes(sample_count); Belief new_belief(sample_count); @@ -200,18 +202,18 @@ class RBCoordinateParticleFilter for(int i = 0; i < belief_.size(); i++) belief_.location(i) = samples[i]; - indices_ = std::vector(samples.size(), 0); + indices_ = IntArray::Zero(samples.size()); observation_model_->Reset(); } private: Belief belief_; - std::vector indices_; + IntArray indices_; std::vector noises_; - std::vector next_samples_; - List loglikes_; + StateArray next_samples_; + RealArray loglikes_; // models boost::shared_ptr observation_model_; From 9f21090a38d7b3412b0f516ebdb84e6ffb8cd920 Mon Sep 17 00:00:00 2001 From: manuel Date: Sat, 22 Aug 2015 17:50:08 +0200 Subject: [PATCH 023/131] found and corrected bug. likelihoods were not being resampled with the rest of arrays, because they were being set afterwards --- include/dbot/rao_blackwell_coordinate_particle_filter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index ded2989..92ecba5 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -148,12 +148,12 @@ class RBCoordinateParticleFilter // update the weights and resample if necessary -------------------- belief_.delta_log_prob_mass(new_loglikes - loglikes_); + loglikes_ = new_loglikes; if(belief_.kl_given_uniform() > max_kl_divergence_) { resample(belief_.size()); } - loglikes_ = new_loglikes; MEASURE("updating weights"); } From 09446b2d664100865bdf7307b8aef980f5d1b508 Mon Sep 17 00:00:00 2001 From: manuel Date: Sat, 22 Aug 2015 17:59:16 +0200 Subject: [PATCH 024/131] renamed some functions in the rao_blackwell_observation_model --- .../kinect_image_observation_model_cpu.hpp | 22 +++++++++---------- .../kinect_image_observation_model_gpu.hpp | 14 ++++++------ ...inect_image_observation_model_gpu_hack.hpp | 14 ++++++------ .../rao_blackwell_observation_model.hpp | 13 +++++------ ...o_blackwell_coordinate_particle_filter.hpp | 6 ++--- 5 files changed, 34 insertions(+), 35 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index d7a3a19..89cfef8 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -60,7 +60,7 @@ struct Traits > { typedef Eigen::Matrix Observation; - typedef RaoBlackwellObservationModel ObservationModelBase; + typedef RBObservationModel ObservationModelBase; typedef boost::shared_ptr ObjectRendererPtr; typedef boost::shared_ptr PixelObservationModelPtr; @@ -117,19 +117,19 @@ class KinectImageObservationModelCPU: { static_assert_base(State, RigidBodiesState); - Reset(); + reset(); } ~KinectImageObservationModelCPU() { } - RealArray Loglikes(const StateArray& states, + RealArray loglikes(const StateArray& states, IntArray& indices, const bool& update = false) { std::vector > new_occlusions(states.size()); std::vector > new_occlusion_times(states.size()); - RealArray loglikes = RealArray::Zero(states.size()); + RealArray log_likes = RealArray::Zero(states.size()); // std::vector loglikes(states.size(),0); for(size_t state_index = 0; state_index < size_t(states.size()); state_index++) { @@ -177,7 +177,7 @@ class KinectImageObservationModelCPU: { if(isnan(observations_[intersect_indices[i]])) { - loglikes[state_index] += log(1.); + log_likes[state_index] += log(1.); } else { @@ -202,7 +202,7 @@ class KinectImageObservationModelCPU: observation_model_->Condition(std::numeric_limits::infinity(), true); float p_obsIinf = observation_model_->Probability(observations_[intersect_indices[i]]); - loglikes[state_index] += log((p_obsIpred_vis + p_obsIpred_occl)/p_obsIinf); + log_likes[state_index] += log((p_obsIpred_vis + p_obsIpred_occl)/p_obsIinf); // we update the occlusion with the observations if(update) @@ -221,10 +221,10 @@ class KinectImageObservationModelCPU: for(size_t state_index = 0; state_index < indices.size(); state_index++) indices[state_index] = state_index; } - return loglikes; + return log_likes; } - void SetObservation(const Observation& image) + void set_observation(const Observation& image) { std::vector std_measurement(image.size()); @@ -232,10 +232,10 @@ class KinectImageObservationModelCPU: for(size_t col = 0; col < image.cols(); col++) std_measurement[row*image.cols() + col] = image(row, col); - SetObservation(std_measurement, this->delta_time_); + set_observation(std_measurement, this->delta_time_); } - virtual void Reset() + virtual void reset() { occlusions_.resize(1); occlusions_[0] = std::vector(n_rows_*n_cols_, initial_occlusion_); @@ -251,7 +251,7 @@ class KinectImageObservationModelCPU: } private: - void SetObservation(const std::vector& observations, const Scalar& delta_time) + void set_observation(const std::vector& observations, const Scalar& delta_time) { observations_ = observations; observation_time_ += delta_time; diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 6f03d65..e1f9dca 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -38,7 +38,7 @@ struct Traits > typedef double Scalar; typedef Eigen::Matrix Observation; - typedef RaoBlackwellObservationModel Base; + typedef RBObservationModel Base; typedef typename Eigen::Matrix CameraMatrix; @@ -127,7 +127,7 @@ class KinectImageObservationModelGPU: std:: cout << "set occlusions..." << std::endl; // set_occlusions(); - Reset(); + reset(); float c = p_visible_visible_ - p_visible_occluded_; float log_c = log(c); @@ -186,7 +186,7 @@ class KinectImageObservationModelGPU: constants_set_ = true; } - RealArray Loglikes(const StateArray& states, + RealArray loglikes(const StateArray& states, IntArray& occlusion_indices, const bool& update_occlusions = false) { @@ -281,17 +281,17 @@ class KinectImageObservationModelGPU: } - void SetObservation(const Observation& image){ + void set_observation(const Observation& image){ std::vector std_measurement(image.size()); for(size_t row = 0; row < image.rows(); row++) for(size_t col = 0; col < image.cols(); col++) std_measurement[row*image.cols() + col] = image(row, col); - SetObservation(std_measurement, this->delta_time_); + set_observation(std_measurement, this->delta_time_); } - virtual void Reset() + virtual void reset() { Occlusions(); observation_time_ = 0; @@ -313,7 +313,7 @@ class KinectImageObservationModelGPU: private: // TODO: this function should disappear, BOTH OF THEM - void SetObservation(const std::vector& observations, const Scalar& delta_time) + void set_observation(const std::vector& observations, const Scalar& delta_time) { observation_time_ += delta_time; if (initialized_) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp index b96fc8f..97700f8 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp @@ -41,7 +41,7 @@ struct Traits > typedef double Scalar; typedef Eigen::Matrix Observation; - typedef RaoBlackwellObservationModel Base; + typedef RBObservationModel Base; typedef typename Eigen::Matrix CameraMatrix; @@ -154,7 +154,7 @@ class KinectImageObservationModelGPUHack: std:: cout << "set occlusions..." << std::endl; // set_occlusions(); - Reset(); + reset(); float c = p_visible_visible_ - p_visible_occluded_; float log_c = log(c); @@ -212,7 +212,7 @@ class KinectImageObservationModelGPUHack: constants_set_ = true; } - RealArray Loglikes(const StateArray& states, + RealArray loglikes(const StateArray& states, IntArray& occlusion_indices, const bool& update_occlusions = false) { @@ -314,17 +314,17 @@ class KinectImageObservationModelGPUHack: } - void SetObservation(const Observation& image){ + void set_observation(const Observation& image){ std::vector std_measurement(image.size()); for(size_t row = 0; row < image.rows(); row++) for(size_t col = 0; col < image.cols(); col++) std_measurement[row*image.cols() + col] = image(row, col); - SetObservation(std_measurement, this->delta_time_); + set_observation(std_measurement, this->delta_time_); } - virtual void Reset() + virtual void reset() { Occlusions(); observation_time_ = 0; @@ -346,7 +346,7 @@ class KinectImageObservationModelGPUHack: private: // TODO: this function should disappear, BOTH OF THEM - void SetObservation(const std::vector& observations, const Scalar& delta_time) + void set_observation(const std::vector& observations, const Scalar& delta_time) { observation_time_ += delta_time; if (initialized_) diff --git a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp index d9a4f29..bb31144 100644 --- a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp +++ b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp @@ -42,7 +42,7 @@ namespace ff { template -class RaoBlackwellObservationModel +class RBObservationModel { public: typedef State_ State; @@ -54,18 +54,17 @@ class RaoBlackwellObservationModel public: /// constructor and destructor ********************************************* - RaoBlackwellObservationModel(const fl::Real& delta_time): - delta_time_(delta_time) { } - virtual ~RaoBlackwellObservationModel() { } + RBObservationModel(const fl::Real& delta_time): delta_time_(delta_time) { } + virtual ~RBObservationModel() { } /// likelihood computation ************************************************* - virtual RealArray Loglikes(const StateArray& states, + virtual RealArray loglikes(const StateArray& states, IntArray& indices, const bool& update = false) = 0; /// accessors ************************************************************** - virtual void SetObservation(const Observation& image) = 0; - virtual void Reset() = 0; + virtual void set_observation(const Observation& image) = 0; + virtual void reset() = 0; protected: fl::Real delta_time_; diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index 92ecba5..b166e63 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -100,7 +100,7 @@ class RBCoordinateParticleFilter void filter(const Observation& observation, const Input& input) { - observation_model_->SetObservation(observation); + observation_model_->set_observation(observation); loglikes_ = RealArray::Zero(belief_.size()); noises_ = std::vector(belief_.size(), @@ -136,7 +136,7 @@ class RBCoordinateParticleFilter // compute likelihood ---------------------------------------------- bool update_occlusions = (i_block == sampling_blocks_.size()-1); RealArray new_loglikes_std = - observation_model_->Loglikes(next_samples_, + observation_model_->loglikes(next_samples_, indices_, update_occlusions); RealArray new_loglikes(new_loglikes_std.size()); @@ -203,7 +203,7 @@ class RBCoordinateParticleFilter belief_.location(i) = samples[i]; indices_ = IntArray::Zero(samples.size()); - observation_model_->Reset(); + observation_model_->reset(); } From 4c80abe64a802fc4161d79b241bdeef07b900098 Mon Sep 17 00:00:00 2001 From: manuel Date: Sat, 22 Aug 2015 18:11:33 +0200 Subject: [PATCH 025/131] some more clean up of cpf --- ...o_blackwell_coordinate_particle_filter.hpp | 21 +++---------------- 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index b166e63..717de00 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -105,14 +105,10 @@ class RBCoordinateParticleFilter loglikes_ = RealArray::Zero(belief_.size()); noises_ = std::vector(belief_.size(), Noise::Zero(process_model_->NoiseDimension())); - next_samples_.resize(belief_.size()); - for(int i = 0; i < belief_.size(); i++) - next_samples_[i] = belief_.location(i); for(size_t i_block = 0; i_block < sampling_blocks_.size(); i_block++) { - INIT_PROFILING; // add noise of this block ----------------------------------------- for(size_t i_sampl = 0; i_sampl < belief_.size(); i_sampl++) { @@ -122,7 +118,6 @@ class RBCoordinateParticleFilter unit_gaussian_.sample()(0); } } - MEASURE("sampling"); // propagate using partial noise ----------------------------------- for(size_t i_sampl = 0; i_sampl < belief_.size(); i_sampl++) @@ -131,20 +126,11 @@ class RBCoordinateParticleFilter next_samples_[i_sampl] = process_model_->MapStandardGaussian(noises_[i_sampl]); } - MEASURE("propagation"); // compute likelihood ---------------------------------------------- - bool update_occlusions = (i_block == sampling_blocks_.size()-1); - RealArray new_loglikes_std = - observation_model_->loglikes(next_samples_, - indices_, update_occlusions); - - RealArray new_loglikes(new_loglikes_std.size()); - for(size_t i = 0; i < new_loglikes.size(); i++) - { - new_loglikes[i] = new_loglikes_std[i]; - } - MEASURE("evaluation"); + bool update = (i_block == sampling_blocks_.size()-1); + RealArray new_loglikes = + observation_model_->loglikes(next_samples_, indices_, update); // update the weights and resample if necessary -------------------- belief_.delta_log_prob_mass(new_loglikes - loglikes_); @@ -154,7 +140,6 @@ class RBCoordinateParticleFilter { resample(belief_.size()); } - MEASURE("updating weights"); } for(int i = 0; i < belief_.size(); i++) From 33bf09adb5e8dda4754bd11d8b1da8beb60cd4ea Mon Sep 17 00:00:00 2001 From: manuel Date: Sat, 22 Aug 2015 18:28:03 +0200 Subject: [PATCH 026/131] some more clean up to cpf --- ...o_blackwell_coordinate_particle_filter.hpp | 32 ++++++++----------- 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index 717de00..7bed3bb 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -25,8 +25,8 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . *************************************************************************/ -#ifndef FAST_FILTERING_FILTERS_STOCHASTIC_RAO_BLACKWELL_COORDINATE_PARTICLE_FILTER_HPP -#define FAST_FILTERING_FILTERS_STOCHASTIC_RAO_BLACKWELL_COORDINATE_PARTICLE_FILTER_HPP +#ifndef DBOT__RB_COORDINATE_PARTICLE_FILTER_HPP +#define DBOT__RB_COORDINATE_PARTICLE_FILTER_HPP #include #include @@ -80,12 +80,12 @@ class RBCoordinateParticleFilter { sampling_blocks_ = sampling_blocks; - // make sure sizes are consistent + // make sure sizes are consistent -------------------------------------- size_t dimension = 0; for(size_t i = 0; i < sampling_blocks_.size(); i++) - for(size_t j = 0; j < sampling_blocks_[i].size(); j++) - dimension++; - + { + dimension += sampling_blocks_[i].size(); + } if(dimension != process_model_->NoiseDimension()) { std::cout << "the dimension of the sampling blocks is " << dimension @@ -105,7 +105,7 @@ class RBCoordinateParticleFilter loglikes_ = RealArray::Zero(belief_.size()); noises_ = std::vector(belief_.size(), Noise::Zero(process_model_->NoiseDimension())); - next_samples_.resize(belief_.size()); + old_particles_ = belief_.locations(); for(size_t i_block = 0; i_block < sampling_blocks_.size(); i_block++) { @@ -122,15 +122,15 @@ class RBCoordinateParticleFilter // propagate using partial noise ----------------------------------- for(size_t i_sampl = 0; i_sampl < belief_.size(); i_sampl++) { - process_model_->Condition(belief_.location(i_sampl), input); - next_samples_[i_sampl] = + process_model_->Condition(old_particles_[i_sampl], input); + belief_.location(i_sampl) = process_model_->MapStandardGaussian(noises_[i_sampl]); } // compute likelihood ---------------------------------------------- bool update = (i_block == sampling_blocks_.size()-1); RealArray new_loglikes = - observation_model_->loglikes(next_samples_, indices_, update); + observation_model_->loglikes(belief_.locations(), indices_, update); // update the weights and resample if necessary -------------------- belief_.delta_log_prob_mass(new_loglikes - loglikes_); @@ -141,11 +141,6 @@ class RBCoordinateParticleFilter resample(belief_.size()); } } - - for(int i = 0; i < belief_.size(); i++) - { - belief_.location(i) = next_samples_[i]; - } } void resample(const size_t& sample_count) @@ -164,13 +159,13 @@ class RBCoordinateParticleFilter indices[i] = indices_[index]; noises[i] = noises_[index]; - next_samples[i] = next_samples_[index]; + next_samples[i] = old_particles_[index]; loglikes[i] = loglikes_[index]; } belief_ = new_belief; indices_ = indices; noises_ = noises; - next_samples_ = next_samples; + old_particles_ = next_samples; loglikes_ = loglikes; } @@ -193,11 +188,12 @@ class RBCoordinateParticleFilter private: + /// member variables ******************************************************* Belief belief_; IntArray indices_; std::vector noises_; - StateArray next_samples_; + StateArray old_particles_; RealArray loglikes_; // models From eb10617074d7794cce7927a506ad028d35fe8f75 Mon Sep 17 00:00:00 2001 From: manuel Date: Sat, 22 Aug 2015 19:03:54 +0200 Subject: [PATCH 027/131] added the default state set_fct to observation model --- .../kinect_image_observation_model_cpu.hpp | 12 ++++++++++++ .../kinect_image_observation_model_gpu.hpp | 12 +++++++++++- .../kinect_image_observation_model_gpu_hack.hpp | 12 +++++++++++- .../rao_blackwell_observation_model.hpp | 3 ++- 4 files changed, 36 insertions(+), 3 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index 89cfef8..00d4d77 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -32,6 +32,7 @@ along with this program. If not, see . #include #include +#include #include #include #include @@ -117,11 +118,18 @@ class KinectImageObservationModelCPU: { static_assert_base(State, RigidBodiesState); + default_exists_ = false; reset(); } ~KinectImageObservationModelCPU() { } + void default_state(const State& state) + { + default_state_ = state; + default_exists_ = true; + } + RealArray loglikes(const StateArray& states, IntArray& indices, const bool& update = false) @@ -266,6 +274,7 @@ class KinectImageObservationModelCPU: const size_t max_sample_count_; const boost::shared_ptr > rigid_bodies_state_; + // models ObjectRendererPtr object_model_; PixelObservationModelPtr observation_model_; @@ -278,6 +287,9 @@ class KinectImageObservationModelCPU: // observed data std::vector observations_; double observation_time_; + + State default_state_; + bool default_exists_; }; } diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index e1f9dca..9a840f5 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -5,7 +5,7 @@ #include "boost/shared_ptr.hpp" #include "Eigen/Core" - +#include #include #include @@ -89,11 +89,19 @@ class KinectImageObservationModelGPU: observation_time_(0), Traits::Base(delta_time) { + default_exists_ = false; + visibility_probs_.resize(n_rows_ * n_cols_); } ~KinectImageObservationModelGPU() { } + void default_state(const State& state) + { + default_state_ = state; + default_exists_ = true; + } + // TODO: DO WE NEED TWO DIFFERENT FUNCTIONS FOR THIS?? void Initialize() { @@ -431,6 +439,8 @@ class KinectImageObservationModelGPU: enum time_measurement {SEND_OBSERVATIONS, RENDER, MAP_RESOURCE, GET_MAPPED_ARRAY, SET_TEXTURE_ARRAY, MAP_TEXTURE, COMPUTE_LIKELIHOODS, UNMAP_RESOURCE}; + State default_state_; + bool default_exists_; }; } diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp index 97700f8..89903c0 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp @@ -5,7 +5,7 @@ #include "boost/shared_ptr.hpp" #include "Eigen/Core" - +#include #include #include @@ -92,11 +92,19 @@ class KinectImageObservationModelGPUHack: observation_time_(0), Traits::Base(delta_time) { + default_exists_ = false; + visibility_probs_.resize(n_rows_ * n_cols_); } ~KinectImageObservationModelGPUHack() { } + void default_state(const State& state) + { + default_state_ = state; + default_exists_ = true; + } + // TODO: DO WE NEED TWO DIFFERENT FUNCTIONS FOR THIS?? void Initialize(const KinematicsFromURDF& kinematics) { @@ -466,6 +474,8 @@ class KinectImageObservationModelGPUHack: enum time_measurement {SEND_OBSERVATIONS, RENDER, MAP_RESOURCE, GET_MAPPED_ARRAY, SET_TEXTURE_ARRAY, MAP_TEXTURE, COMPUTE_LIKELIHOODS, UNMAP_RESOURCE}; + State default_state_; + bool default_exists_; }; } diff --git a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp index bb31144..b5ddc68 100644 --- a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp +++ b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp @@ -58,12 +58,13 @@ class RBObservationModel virtual ~RBObservationModel() { } /// likelihood computation ************************************************* - virtual RealArray loglikes(const StateArray& states, + virtual RealArray loglikes(const StateArray& deviations, IntArray& indices, const bool& update = false) = 0; /// accessors ************************************************************** virtual void set_observation(const Observation& image) = 0; + virtual void default_state(const State& state) = 0; virtual void reset() = 0; protected: From 7bf001aac4b416524630829e88e8618e19578eb5 Mon Sep 17 00:00:00 2001 From: manuel Date: Sat, 22 Aug 2015 19:20:42 +0200 Subject: [PATCH 028/131] cleaned cpu image observation model a little --- .../kinect_image_observation_model_cpu.hpp | 75 +++++++------------ 1 file changed, 28 insertions(+), 47 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index 00d4d77..54c8dac 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -92,6 +92,7 @@ class KinectImageObservationModelCPU: typedef typename Base::RealArray RealArray; typedef typename Base::IntArray IntArray; + typedef typename Eigen::Transform Affine; // TODO: DO WE NEED ALL OF THIS IN THE CONSTRUCTOR?? @@ -130,71 +131,51 @@ class KinectImageObservationModelCPU: default_exists_ = true; } - RealArray loglikes(const StateArray& states, + RealArray loglikes(const StateArray& deviations, IntArray& indices, const bool& update = false) { - std::vector > new_occlusions(states.size()); - std::vector > new_occlusion_times(states.size()); + std::vector > new_occlusions(deviations.size()); + std::vector > new_occlusion_times(deviations.size()); - RealArray log_likes = RealArray::Zero(states.size()); -// std::vector loglikes(states.size(),0); - for(size_t state_index = 0; state_index < size_t(states.size()); state_index++) + RealArray log_likes = RealArray::Zero(deviations.size()); + for(size_t i_state = 0; i_state < size_t(deviations.size()); i_state++) { if(update) { - new_occlusions[state_index] = occlusions_[indices[state_index]]; - new_occlusion_times[state_index] = occlusion_times_[indices[state_index]]; + new_occlusions[i_state] = occlusions_[indices[i_state]]; + new_occlusion_times[i_state] = occlusion_times_[indices[i_state]]; } - // we predict observations_ - std::vector intersect_indices; - std::vector predictions; - //TODO: DOES THIS MAKE SENSE? THE OBJECT MODEL SHOULD KNOW ABOUT THE STATE... - int body_count = states[state_index].count(); - - std::vector rotations(body_count); - std::vector translations(body_count); - for(size_t part_index = 0; part_index < body_count; part_index++) + // render the object model ----------------------------------------- + int body_count = deviations[i_state].count(); + std::vector poses(body_count); + for(size_t i_object = 0; i_object < body_count; i_object++) { - rotations[part_index] = - states[state_index].component(part_index).orientation().rotation_matrix(); - translations[part_index] = - states[state_index].component(part_index).position(); + poses[i_object] = + deviations[i_state].component(i_object).affine(); } + object_model_->set_poses(poses); + std::vector intersect_indices; + std::vector predictions; + object_model_->Render(camera_matrix_, n_rows_, n_cols_, + intersect_indices, predictions); - -// std::vector poses(body_count); -// for(size_t part_index = 0; part_index < body_count; part_index++) -// { -// poses[part_index] = states[state_index].affine(part_index); -// } - - - - object_model_->set_poses(rotations, translations); -// object_model_->state(states[state_index]); - - - - - object_model_->Render(camera_matrix_, n_rows_, n_cols_, intersect_indices, predictions); - - // we loop through all the pixels which intersect the object model + // compute likelihoods --------------------------------------------- for(size_t i = 0; i < size_t(predictions.size()); i++) { if(isnan(observations_[intersect_indices[i]])) { - log_likes[state_index] += log(1.); + log_likes[i_state] += log(1.); } else { double delta_time = observation_time_ - - occlusion_times_[indices[state_index]][intersect_indices[i]]; + occlusion_times_[indices[i_state]][intersect_indices[i]]; occlusion_process_model_->Condition(delta_time, - occlusions_[indices[state_index]][intersect_indices[i]]); + occlusions_[indices[i_state]][intersect_indices[i]]); float occlusion = occlusion_process_model_->MapStandardGaussian(); @@ -210,14 +191,14 @@ class KinectImageObservationModelCPU: observation_model_->Condition(std::numeric_limits::infinity(), true); float p_obsIinf = observation_model_->Probability(observations_[intersect_indices[i]]); - log_likes[state_index] += log((p_obsIpred_vis + p_obsIpred_occl)/p_obsIinf); + log_likes[i_state] += log((p_obsIpred_vis + p_obsIpred_occl)/p_obsIinf); // we update the occlusion with the observations if(update) { - new_occlusions[state_index][intersect_indices[i]] = + new_occlusions[i_state][intersect_indices[i]] = p_obsIpred_occl/(p_obsIpred_vis + p_obsIpred_occl); - new_occlusion_times[state_index][intersect_indices[i]] = observation_time_; + new_occlusion_times[i_state][intersect_indices[i]] = observation_time_; } } } @@ -226,8 +207,8 @@ class KinectImageObservationModelCPU: { occlusions_ = new_occlusions; occlusion_times_ = new_occlusion_times; - for(size_t state_index = 0; state_index < indices.size(); state_index++) - indices[state_index] = state_index; + for(size_t i_state = 0; i_state < indices.size(); i_state++) + indices[i_state] = i_state; } return log_likes; } From fa81de6caf440c2a2f27539f0a650e0489483433 Mon Sep 17 00:00:00 2001 From: manuel Date: Sat, 22 Aug 2015 19:41:49 +0200 Subject: [PATCH 029/131] working on observation models for default state stuff --- .../kinect_image_observation_model_cpu.hpp | 12 +++++++----- .../kinect_image_observation_model_gpu.hpp | 17 ++++++++++------- .../kinect_image_observation_model_gpu_hack.hpp | 16 ++++++++++------ 3 files changed, 27 insertions(+), 18 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index 54c8dac..c08acf8 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -119,7 +119,8 @@ class KinectImageObservationModelCPU: { static_assert_base(State, RigidBodiesState); - default_exists_ = false; + default_poses_ = std::vector(object_model_->vertices().size(), + Affine::Identity()); reset(); } @@ -127,8 +128,10 @@ class KinectImageObservationModelCPU: void default_state(const State& state) { - default_state_ = state; - default_exists_ = true; + for(size_t i = 0; i < state.count(); i++) + { + default_poses_[i] = state.component(i).affine(); + } } RealArray loglikes(const StateArray& deviations, @@ -269,8 +272,7 @@ class KinectImageObservationModelCPU: std::vector observations_; double observation_time_; - State default_state_; - bool default_exists_; + std::vector default_poses_; }; } diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 9a840f5..9525e53 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -67,6 +67,9 @@ class KinectImageObservationModelGPU: typedef typename Traits::Base::RealArray RealArray; typedef typename Traits::Base::IntArray IntArray; + typedef typename Eigen::Transform Affine; + + // TODO: ALL THIS SHOULD SWITCH FROM USING VISIBILITY TO OCCLUSION KinectImageObservationModelGPU(const CameraMatrix& camera_matrix, @@ -89,8 +92,6 @@ class KinectImageObservationModelGPU: observation_time_(0), Traits::Base(delta_time) { - default_exists_ = false; - visibility_probs_.resize(n_rows_ * n_cols_); } @@ -98,8 +99,10 @@ class KinectImageObservationModelGPU: void default_state(const State& state) { - default_state_ = state; - default_exists_ = true; + for(size_t i = 0; i < state.count(); i++) + { + default_poses_[i] = state.component(i).affine(); + } } // TODO: DO WE NEED TWO DIFFERENT FUNCTIONS FOR THIS?? @@ -165,7 +168,8 @@ class KinectImageObservationModelGPU: const std::string vertex_shader_path, const std::string fragment_shader_path) { - + default_poses_ = std::vector(vertices_double.size(), + Affine::Identity()); // since you love doubles i changed the argument type of the vertices to double and convert it here :) vertices_.resize(vertices_double.size()); @@ -439,8 +443,7 @@ class KinectImageObservationModelGPU: enum time_measurement {SEND_OBSERVATIONS, RENDER, MAP_RESOURCE, GET_MAPPED_ARRAY, SET_TEXTURE_ARRAY, MAP_TEXTURE, COMPUTE_LIKELIHOODS, UNMAP_RESOURCE}; - State default_state_; - bool default_exists_; + std::vector default_poses_; }; } diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp index 89903c0..9037aae 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp @@ -70,6 +70,9 @@ class KinectImageObservationModelGPUHack: typedef typename Traits::Base::RealArray RealArray; typedef typename Traits::Base::IntArray IntArray; + typedef typename Eigen::Transform Affine; + + // TODO: ALL THIS SHOULD SWITCH FROM USING VISIBILITY TO OCCLUSION KinectImageObservationModelGPUHack(const CameraMatrix& camera_matrix, @@ -92,8 +95,6 @@ class KinectImageObservationModelGPUHack: observation_time_(0), Traits::Base(delta_time) { - default_exists_ = false; - visibility_probs_.resize(n_rows_ * n_cols_); } @@ -101,8 +102,10 @@ class KinectImageObservationModelGPUHack: void default_state(const State& state) { - default_state_ = state; - default_exists_ = true; + for(size_t i = 0; i < state.count(); i++) + { + default_poses_[i] = state.component(i).affine(); + } } // TODO: DO WE NEED TWO DIFFERENT FUNCTIONS FOR THIS?? @@ -192,6 +195,8 @@ class KinectImageObservationModelGPUHack: const std::string vertex_shader, const std::string fragment_shader) { + default_poses_ = std::vector(vertices_double.size(), + Affine::Identity()); vertex_shader_ = vertex_shader; fragment_shader_ = fragment_shader; @@ -474,8 +479,7 @@ class KinectImageObservationModelGPUHack: enum time_measurement {SEND_OBSERVATIONS, RENDER, MAP_RESOURCE, GET_MAPPED_ARRAY, SET_TEXTURE_ARRAY, MAP_TEXTURE, COMPUTE_LIKELIHOODS, UNMAP_RESOURCE}; - State default_state_; - bool default_exists_; + std::vector default_poses_; }; } From 050daeddac1a2a50431a79fdc4c10cde5ec8fb62 Mon Sep 17 00:00:00 2001 From: manuel Date: Sat, 22 Aug 2015 20:09:40 +0200 Subject: [PATCH 030/131] observation models in principle ready for using with a default_state, not tested yet --- .../kinect_image_observation_model_cpu.hpp | 3 +- .../kinect_image_observation_model_gpu.hpp | 43 ++++++++++--------- ...inect_image_observation_model_gpu_hack.hpp | 38 ++++++++-------- 3 files changed, 44 insertions(+), 40 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index c08acf8..0a47344 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -156,7 +156,8 @@ class KinectImageObservationModelCPU: for(size_t i_object = 0; i_object < body_count; i_object++) { poses[i_object] = - deviations[i_state].component(i_object).affine(); + deviations[i_state].component(i_object).affine() + * default_poses_[i_object]; } object_model_->set_poses(poses); std::vector intersect_indices; diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 9525e53..1375692 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -198,7 +198,7 @@ class KinectImageObservationModelGPU: constants_set_ = true; } - RealArray loglikes(const StateArray& states, + RealArray loglikes(const StateArray& deviations, IntArray& occlusion_indices, const bool& update_occlusions = false) { @@ -214,7 +214,7 @@ class KinectImageObservationModelGPU: exit(-1); } - n_poses_ = states.size(); + n_poses_ = deviations.size(); std::vector flog_likelihoods (n_poses_, 0); set_number_of_poses(n_poses_); @@ -230,35 +230,38 @@ class KinectImageObservationModelGPU: cuda_->set_prev_sample_indices(occlusion_indices_transformed.data()); MEASURE("gpu: setting occlusion indices"); // convert to internal state format - std::vector > > states_internal_format( + std::vector > > poses( n_poses_, - std::vector >(states[0].count(), + std::vector >(default_poses_.size(), std::vector(7, 0))); MEASURE("gpu: creating state vectors"); - for(size_t state_index = 0; state_index < size_t(n_poses_); state_index++) - for(size_t body_index = 0; body_index < states[state_index].count(); body_index++) + for(size_t i_state = 0; i_state < size_t(n_poses_); i_state++) + { + for(size_t i_obj = 0; i_obj < default_poses_.size(); i_obj++) { - const Eigen::Quaternion& quaternion - = states[state_index].component(body_index).orientation().quaternion(); - states_internal_format[state_index][body_index][0] = quaternion.w(); - states_internal_format[state_index][body_index][1] = quaternion.x(); - states_internal_format[state_index][body_index][2] = quaternion.y(); - states_internal_format[state_index][body_index][3] = quaternion.z(); - const Eigen::Matrix& position = - states[state_index].component(body_index).position(); - states_internal_format[state_index][body_index][4] = position[0]; - states_internal_format[state_index][body_index][5] = position[1]; - states_internal_format[state_index][body_index][6] = position[2]; + Affine pose = deviations[i_state].component(i_obj).affine() + * default_poses_[i_obj]; + Eigen::Quaternion quaternion(pose.rotation()); + Eigen::Matrix position = pose.translation(); + + poses[i_state][i_obj][0] = quaternion.w(); + poses[i_state][i_obj][1] = quaternion.x(); + poses[i_state][i_obj][2] = quaternion.y(); + poses[i_state][i_obj][3] = quaternion.z(); + poses[i_state][i_obj][4] = position[0]; + poses[i_state][i_obj][5] = position[1]; + poses[i_state][i_obj][6] = position[2]; } + } MEASURE("gpu: converting state format"); - opengl_->Render(states_internal_format); + opengl_->Render(poses); MEASURE("gpu: rendering"); @@ -277,8 +280,8 @@ class KinectImageObservationModelGPU: if(update_occlusions) { - for(size_t state_index = 0; state_index < occlusion_indices.size(); state_index++) - occlusion_indices[state_index] = state_index; + for(size_t i_state = 0; i_state < occlusion_indices.size(); i_state++) + occlusion_indices[i_state] = i_state; MEASURE("gpu: updating occlusions"); } diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp index 9037aae..58d215b 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp @@ -225,7 +225,7 @@ class KinectImageObservationModelGPUHack: constants_set_ = true; } - RealArray loglikes(const StateArray& states, + RealArray loglikes(const StateArray& deviations, IntArray& occlusion_indices, const bool& update_occlusions = false) { @@ -241,7 +241,7 @@ class KinectImageObservationModelGPUHack: exit(-1); } - n_poses_ = states.size(); + n_poses_ = deviations.size(); std::vector flog_likelihoods (n_poses_, 0); set_number_of_poses(n_poses_); @@ -257,9 +257,9 @@ class KinectImageObservationModelGPUHack: cuda_->set_prev_sample_indices(occlusion_indices_transformed.data()); MEASURE("gpu: setting occlusion indices"); // convert to internal state format - std::vector > > states_internal_format( + std::vector > > poses( n_poses_, - std::vector >(states[0].count(), + std::vector >(deviations[0].count(), std::vector(7, 0))); MEASURE("gpu: creating state vectors"); @@ -267,24 +267,24 @@ class KinectImageObservationModelGPUHack: // TODO: this is a hack!!! #pragma omp parallel for - for(size_t state_index = 0; state_index < size_t(n_poses_); state_index++) + for(size_t i_state = 0; i_state < size_t(n_poses_); i_state++) { int tid = omp_get_thread_num(); - kinematics_[tid].InitKDLData(states[state_index]); + kinematics_[tid].InitKDLData(deviations[i_state]); - for(size_t body_index = 0; body_index < states[state_index].count(); body_index++) + for(size_t i_object = 0; i_object < deviations[i_state].count(); i_object++) { - const Eigen::Quaternion& quaternion = kinematics_[tid].GetLinkOrientation(body_index); - states_internal_format[state_index][body_index][0] = quaternion.w(); - states_internal_format[state_index][body_index][1] = quaternion.x(); - states_internal_format[state_index][body_index][2] = quaternion.y(); - states_internal_format[state_index][body_index][3] = quaternion.z(); - const Eigen::Matrix& position = kinematics_[tid].GetLinkPosition(body_index); - states_internal_format[state_index][body_index][4] = position[0]; - states_internal_format[state_index][body_index][5] = position[1]; - states_internal_format[state_index][body_index][6] = position[2]; + const Eigen::Quaternion& quaternion = kinematics_[tid].GetLinkOrientation(i_object); + poses[i_state][i_object][0] = quaternion.w(); + poses[i_state][i_object][1] = quaternion.x(); + poses[i_state][i_object][2] = quaternion.y(); + poses[i_state][i_object][3] = quaternion.z(); + const Eigen::Matrix& position = kinematics_[tid].GetLinkPosition(i_object); + poses[i_state][i_object][4] = position[0]; + poses[i_state][i_object][5] = position[1]; + poses[i_state][i_object][6] = position[2]; } } @@ -292,7 +292,7 @@ class KinectImageObservationModelGPUHack: - opengl_->Render(states_internal_format); + opengl_->Render(poses); MEASURE("gpu: rendering"); @@ -311,8 +311,8 @@ class KinectImageObservationModelGPUHack: if(update_occlusions) { - for(size_t state_index = 0; state_index < occlusion_indices.size(); state_index++) - occlusion_indices[state_index] = state_index; + for(size_t i_state = 0; i_state < occlusion_indices.size(); i_state++) + occlusion_indices[i_state] = i_state; MEASURE("gpu: updating occlusions"); } From c48558a50e311cde5393d6ce4f021d8a763effd7 Mon Sep 17 00:00:00 2001 From: manuel Date: Sat, 22 Aug 2015 20:19:19 +0200 Subject: [PATCH 031/131] removed hacky gpu observation model --- ...inect_image_observation_model_gpu_hack.hpp | 486 ------------------ 1 file changed, 486 deletions(-) delete mode 100644 include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp deleted file mode 100644 index 58d215b..0000000 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu_hack.hpp +++ /dev/null @@ -1,486 +0,0 @@ -#ifndef POSE_TRACKING_MODELS_OBSERVATION_MODELS_KINECT_IMAGE_OBSERVATION_MODEL_GPU_HACK_HPP -#define POSE_TRACKING_MODELS_OBSERVATION_MODELS_KINECT_IMAGE_OBSERVATION_MODEL_GPU_HACK_HPP - -#include -#include "boost/shared_ptr.hpp" -#include "Eigen/Core" - -#include -#include -#include - -#include -#include - -#include - - -#include -#include -#include -#include -#include - -#include - -namespace ff -{ - -// Forward declarations -template class KinectImageObservationModelGPUHack; - -namespace internal -{ -/** - * ImageObservationModelCPU distribution traits specialization - * \internal - */ -template -struct Traits > -{ - typedef double Scalar; - typedef Eigen::Matrix Observation; - - typedef RBObservationModel Base; - - typedef typename Eigen::Matrix CameraMatrix; - -// typedef sf::RigidBodySystem<-1> State; -}; -} - -/** - * \class ImageObservationModelGPU - * - * \ingroup distributions - * \ingroup observation_models - */ -template -class KinectImageObservationModelGPUHack: - public internal::Traits >::Base -{ -public: - typedef internal::Traits > Traits; - - typedef typename Traits::Scalar Scalar; - typedef typename Traits::Observation Observation; - typedef typename Traits::CameraMatrix CameraMatrix; - - typedef typename Traits::Base::StateArray StateArray; - typedef typename Traits::Base::RealArray RealArray; - typedef typename Traits::Base::IntArray IntArray; - - typedef typename Eigen::Transform Affine; - - - - // TODO: ALL THIS SHOULD SWITCH FROM USING VISIBILITY TO OCCLUSION - KinectImageObservationModelGPUHack(const CameraMatrix& camera_matrix, - const size_t& n_rows, - const size_t& n_cols, - const size_t& max_sample_count, - const Scalar& initial_occlusion_prob, - const double& delta_time): - camera_matrix_(camera_matrix), - n_rows_(n_rows), - n_cols_(n_cols), - initial_visibility_prob_(1 - initial_occlusion_prob), - max_sample_count_(max_sample_count), - n_poses_(max_sample_count), - constants_set_(false), - initialized_(false), - observations_set_(false), - resource_registered_(false), - nr_calls_set_observation_(0), - observation_time_(0), - Traits::Base(delta_time) - { - visibility_probs_.resize(n_rows_ * n_cols_); - } - - ~KinectImageObservationModelGPUHack() { } - - void default_state(const State& state) - { - for(size_t i = 0; i < state.count(); i++) - { - default_poses_[i] = state.component(i).affine(); - } - } - - // TODO: DO WE NEED TWO DIFFERENT FUNCTIONS FOR THIS?? - void Initialize(const KinematicsFromURDF& kinematics) - { - if (constants_set_) - { - - - // TODO: this is a disgusting hack!!! - std::cout << "warning: copying kinematics in gpu observation model, this is a hack!!!" << std::endl; - std::cout << "resizing " << std::endl; - kinematics_.resize(30); - - for(size_t i = 0; i < 30; i++) - { - std::cout << "iteration " << i << std::endl; - kinematics_[i] = kinematics; - } - std::cout << "done copying kinematics" << std::endl; - - - - - - - - - - - - opengl_ = boost::shared_ptr - (new ObjectRasterizer(vertices_, - indices_, - vertex_shader_, - fragment_shader_)); - cuda_ = boost::shared_ptr (new fil::CudaFilter()); - - initialized_ = true; - - - opengl_->PrepareRender(camera_matrix_.cast()); - - - opengl_->set_number_of_max_poses(max_sample_count_); - n_poses_x_ = opengl_->get_n_poses_x(); - cuda_->set_number_of_max_poses(max_sample_count_, n_poses_x_); - - - std:: cout << "set resolution in cuda..." << std::endl; - - opengl_->set_resolution(n_rows_, n_cols_); - cuda_->set_resolution(n_rows_, n_cols_); - - RegisterResource(); - - std:: cout << "set occlusions..." << std::endl; - - // set_occlusions(); - reset(); - - float c = p_visible_visible_ - p_visible_occluded_; - float log_c = log(c); - - std::vector > dummy_com_models; - cuda_->Init(dummy_com_models, 0.0f, 0.0f, - initial_visibility_prob_, c, log_c, p_visible_occluded_, - tail_weight_, model_sigma_, sigma_factor_, max_depth_, exponential_rate_); - - - count_ = 0; - - } else { - std:: cout << "WARNING: GPUImageObservationModel::Initialize() was not executed, because GPUImageObservationModel::set_constants() has not been called previously." << std::endl; - } - } - - void Constants(const std::vector > vertices_double, - const std::vector > > indices, - const float p_occluded_visible, - const float p_occluded_occluded, - const float tail_weight, - const float model_sigma, - const float sigma_factor, - const float max_depth, - const float exponential_rate, - const std::string vertex_shader, - const std::string fragment_shader) - { - default_poses_ = std::vector(vertices_double.size(), - Affine::Identity()); - - vertex_shader_ = vertex_shader; - fragment_shader_ = fragment_shader; - - - // since you love doubles i changed the argument type of the vertices to double and convert it here :) - vertices_.resize(vertices_double.size()); - for(size_t object_index = 0; object_index < vertices_.size(); object_index++) - { - vertices_[object_index].resize(vertices_double[object_index].size()); - for(size_t vertex_index = 0; vertex_index < vertices_[object_index].size(); vertex_index++) - vertices_[object_index][vertex_index] = vertices_double[object_index][vertex_index].cast(); - } - - - indices_ = indices; - p_visible_visible_ = 1.0 - p_occluded_visible; - p_visible_occluded_ = 1.0 - p_occluded_occluded; - tail_weight_ = tail_weight; - model_sigma_ = model_sigma; - sigma_factor_ = sigma_factor; - max_depth_ = max_depth; - exponential_rate_ = exponential_rate; - - - constants_set_ = true; - } - - RealArray loglikes(const StateArray& deviations, - IntArray& occlusion_indices, - const bool& update_occlusions = false) - { - if (!initialized_) - - { - std:: cout << "GPU: not initialized" << std::endl; - exit(-1); - } - else if(!observations_set_) - { - std:: cout << "GPU: observations not set" << std::endl; - exit(-1); - } - - n_poses_ = deviations.size(); - std::vector flog_likelihoods (n_poses_, 0); - set_number_of_poses(n_poses_); - - // transform occlusion indices from size_t to int - std::vector occlusion_indices_transformed (occlusion_indices.size(), 0); - for (size_t i = 0; i < occlusion_indices.size(); i++) - { - occlusion_indices_transformed[i] = (int) occlusion_indices[i]; - } - - INIT_PROFILING; - // copy occlusion indices to GPU - cuda_->set_prev_sample_indices(occlusion_indices_transformed.data()); - MEASURE("gpu: setting occlusion indices"); - // convert to internal state format - std::vector > > poses( - n_poses_, - std::vector >(deviations[0].count(), - std::vector(7, 0))); - - MEASURE("gpu: creating state vectors"); - - -// TODO: this is a hack!!! -#pragma omp parallel for - for(size_t i_state = 0; i_state < size_t(n_poses_); i_state++) - { - - int tid = omp_get_thread_num(); - - kinematics_[tid].InitKDLData(deviations[i_state]); - - for(size_t i_object = 0; i_object < deviations[i_state].count(); i_object++) - { - const Eigen::Quaternion& quaternion = kinematics_[tid].GetLinkOrientation(i_object); - poses[i_state][i_object][0] = quaternion.w(); - poses[i_state][i_object][1] = quaternion.x(); - poses[i_state][i_object][2] = quaternion.y(); - poses[i_state][i_object][3] = quaternion.z(); - const Eigen::Matrix& position = kinematics_[tid].GetLinkPosition(i_object); - poses[i_state][i_object][4] = position[0]; - poses[i_state][i_object][5] = position[1]; - poses[i_state][i_object][6] = position[2]; - } - } - - MEASURE("gpu: converting state format"); - - - - opengl_->Render(poses); - - MEASURE("gpu: rendering"); - - - cudaGraphicsMapResources(1, &combined_texture_resource_, 0); - cudaGraphicsSubResourceGetMappedArray(&texture_array_, combined_texture_resource_, 0, 0); - cuda_->set_texture_array(texture_array_); - cuda_->MapTexture(); - MEASURE("gpu: mapping texture"); - - cuda_->CompareMultiple(update_occlusions, flog_likelihoods); - cudaGraphicsUnmapResources(1, &combined_texture_resource_, 0); - - MEASURE("gpu: computing likelihoods"); - - - if(update_occlusions) - { - for(size_t i_state = 0; i_state < occlusion_indices.size(); i_state++) - occlusion_indices[i_state] = i_state; - - MEASURE("gpu: updating occlusions"); - } - - - // convert - RealArray log_likelihoods(flog_likelihoods.size()); - for(size_t i = 0; i < flog_likelihoods.size(); i++) - log_likelihoods[i] = flog_likelihoods[i]; - - return log_likelihoods; - } - - - void set_observation(const Observation& image){ - std::vector std_measurement(image.size()); - - for(size_t row = 0; row < image.rows(); row++) - for(size_t col = 0; col < image.cols(); col++) - std_measurement[row*image.cols() + col] = image(row, col); - - set_observation(std_measurement, this->delta_time_); - } - - virtual void reset() - { - Occlusions(); - observation_time_ = 0; - } - - - // TODO: this image should be in a different format BOTH OF THEM!! - const std::vector Occlusions(size_t index) const - { - std::vector visibility_probs = cuda_->get_visibility_probabilities((int) index); - return visibility_probs; - } - - void RangeImage(std::vector > &intersect_indices, - std::vector > &depth) - { - opengl_->get_depth_values(intersect_indices, depth); - } - -private: - // TODO: this function should disappear, BOTH OF THEM - void set_observation(const std::vector& observations, const Scalar& delta_time) - { - observation_time_ += delta_time; - if (initialized_) - { - cuda_->set_observations(observations.data(), observation_time_); - observations_set_ = true; - } - } - - void Occlusions(const float& visibility_prob = -1) - { - float default_visibility_probability = visibility_prob; - if (visibility_prob == -1) default_visibility_probability = initial_visibility_prob_; - - std::vector visibility_probabilities (n_rows_ * n_cols_ * n_poses_, default_visibility_probability); - cuda_->set_visibility_probabilities(visibility_probabilities.data()); - // TODO set update times if you want to use them - - } - - const Eigen::Matrix3d camera_matrix_; - const size_t n_rows_; - const size_t n_cols_; - const float initial_visibility_prob_; - const size_t max_sample_count_; - - void set_number_of_poses(int n_poses){ - if (initialized_) { - n_poses_ = n_poses; - opengl_->set_number_of_poses(n_poses_); - n_poses_x_ = opengl_->get_n_poses_x(); - cuda_->set_number_of_poses(n_poses_, n_poses_x_); - } else { - std:: cout << "WARNING: GPUImageObservationModel::set_number_of_poses() was not executed, because GPUImageObservationModel::Initialize() has not been called previously." << std::endl; - } - } - - void checkCUDAError(const char *msg) - { - cudaError_t err = cudaGetLastError(); - if( cudaSuccess != err) - { - fprintf(stderr, "Cuda error: %s: %s.\n", msg, cudaGetErrorString( err) ); - exit(EXIT_FAILURE); - } - } - - - void UnregisterResource() - { - if (resource_registered_) { - cudaGraphicsUnregisterResource(combined_texture_resource_); - checkCUDAError("cudaGraphicsUnregisterResource"); - resource_registered_ = false; - } - } - - void RegisterResource() - { - if (!resource_registered_) { - combined_texture_opengl_ = opengl_->get_combined_texture(); - cudaGraphicsGLRegisterImage(&combined_texture_resource_, combined_texture_opengl_, GL_TEXTURE_2D, cudaGraphicsRegisterFlagsReadOnly); - checkCUDAError("cudaGraphicsGLRegisterImage)"); - resource_registered_ = true; - } - } - - - std::vector kinematics_; - - boost::shared_ptr opengl_; - boost::shared_ptr cuda_; - - // arrays for timings - std::vector > cpu_times_; - std::vector > cuda_times_; - - // data - std::vector > vertices_; - std::vector > > indices_; - std::vector visibility_probs_; - - // constants for likelihood evaluation - float p_visible_visible_; - float p_visible_occluded_; - float tail_weight_; - float model_sigma_; - float sigma_factor_; - float max_depth_; - float exponential_rate_; - - - std::string vertex_shader_; - std::string fragment_shader_; - - double start_time_; - - int n_poses_; - int n_poses_x_; - - int count_; - - // booleans to ensure correct usage of function calls - bool constants_set_, initialized_, observations_set_, resource_registered_; - int nr_calls_set_observation_; - - // Shared resource between OpenGL and CUDA - GLuint combined_texture_opengl_; - cudaGraphicsResource* combined_texture_resource_; - cudaArray_t texture_array_; - - double observation_time_; - - // used for time observations - static const int TIME_MEASUREMENTS_COUNT = 8; - static const int COUNT = 500; - enum time_measurement {SEND_OBSERVATIONS, RENDER, MAP_RESOURCE, GET_MAPPED_ARRAY, SET_TEXTURE_ARRAY, - MAP_TEXTURE, COMPUTE_LIKELIHOODS, UNMAP_RESOURCE}; - - std::vector default_poses_; -}; - -} -#endif From 8f6dc396fff7a3a944249835099d4530a5d82c0e Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 25 Aug 2015 18:14:04 +0200 Subject: [PATCH 032/131] minor --- .../rao_blackwell_observation_model.hpp | 3 +++ .../dbot/rao_blackwell_coordinate_particle_filter.hpp | 10 ++++++++++ 2 files changed, 13 insertions(+) diff --git a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp index b5ddc68..f2daa11 100644 --- a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp +++ b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp @@ -41,6 +41,9 @@ along with this program. If not, see . namespace ff { + +/// \todo this observation model is now specific to rigid body rendering, +/// terminology should be adapted accordingly. template class RBObservationModel { diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index 7bed3bb..d3e4782 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -186,6 +186,16 @@ class RBCoordinateParticleFilter observation_model_->reset(); } + boost::shared_ptr observation_model() + { + return observation_model_; + } + + boost::shared_ptr process_model() + { + return process_model_; + } + private: /// member variables ******************************************************* From 8104177a404b0c22b59699a390da30ebff15be02 Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 25 Aug 2015 19:50:44 +0200 Subject: [PATCH 033/131] in the process of changing observation models to deal with default and deviation properly. currently not functional --- .../kinect_image_observation_model_cpu.hpp | 26 ++++++++----------- .../kinect_image_observation_model_gpu.hpp | 23 ++++++---------- .../rao_blackwell_observation_model.hpp | 13 +++++++++- 3 files changed, 31 insertions(+), 31 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index 0a47344..9ca03da 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -119,21 +119,14 @@ class KinectImageObservationModelCPU: { static_assert_base(State, RigidBodiesState); - default_poses_ = std::vector(object_model_->vertices().size(), - Affine::Identity()); + this->default_poses_.recount(object_model_->vertices().size()); + this->default_poses_.setZero(); + reset(); } ~KinectImageObservationModelCPU() { } - void default_state(const State& state) - { - for(size_t i = 0; i < state.count(); i++) - { - default_poses_[i] = state.component(i).affine(); - } - } - RealArray loglikes(const StateArray& deviations, IntArray& indices, const bool& update = false) @@ -153,11 +146,15 @@ class KinectImageObservationModelCPU: // render the object model ----------------------------------------- int body_count = deviations[i_state].count(); std::vector poses(body_count); - for(size_t i_object = 0; i_object < body_count; i_object++) + for(size_t i_obj = 0; i_obj < body_count; i_obj++) { - poses[i_object] = - deviations[i_state].component(i_object).affine() - * default_poses_[i_object]; + auto pose = this->default_poses_.component(i_obj); + auto deviation = deviations[i_state].component(i_obj); + + pose.orientation() = deviation.orientation()*pose.orientation(); + pose.position() = deviation.position() + pose.position(); + + poses[i_obj] = pose.affine(); } object_model_->set_poses(poses); std::vector intersect_indices; @@ -273,7 +270,6 @@ class KinectImageObservationModelCPU: std::vector observations_; double observation_time_; - std::vector default_poses_; }; } diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 1375692..937f933 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -97,13 +97,6 @@ class KinectImageObservationModelGPU: ~KinectImageObservationModelGPU() { } - void default_state(const State& state) - { - for(size_t i = 0; i < state.count(); i++) - { - default_poses_[i] = state.component(i).affine(); - } - } // TODO: DO WE NEED TWO DIFFERENT FUNCTIONS FOR THIS?? void Initialize() @@ -167,9 +160,9 @@ class KinectImageObservationModelGPU: const float exponential_rate, const std::string vertex_shader_path, const std::string fragment_shader_path) - { - default_poses_ = std::vector(vertices_double.size(), - Affine::Identity()); + { + this->default_poses_.recount(vertices_double.size()); + this->default_poses_.setZero(); // since you love doubles i changed the argument type of the vertices to double and convert it here :) vertices_.resize(vertices_double.size()); @@ -232,7 +225,7 @@ class KinectImageObservationModelGPU: // convert to internal state format std::vector > > poses( n_poses_, - std::vector >(default_poses_.size(), + std::vector >(vertices_.size(), std::vector(7, 0))); MEASURE("gpu: creating state vectors"); @@ -240,10 +233,11 @@ class KinectImageObservationModelGPU: for(size_t i_state = 0; i_state < size_t(n_poses_); i_state++) { - for(size_t i_obj = 0; i_obj < default_poses_.size(); i_obj++) + for(size_t i_obj = 0; i_obj < vertices_.size(); i_obj++) { - Affine pose = deviations[i_state].component(i_obj).affine() - * default_poses_[i_obj]; + Affine pose; +// = deviations[i_state].component(i_obj).affine() +// * this->default_poses_[i_obj]; Eigen::Quaternion quaternion(pose.rotation()); Eigen::Matrix position = pose.translation(); @@ -446,7 +440,6 @@ class KinectImageObservationModelGPU: enum time_measurement {SEND_OBSERVATIONS, RENDER, MAP_RESOURCE, GET_MAPPED_ARRAY, SET_TEXTURE_ARRAY, MAP_TEXTURE, COMPUTE_LIKELIHOODS, UNMAP_RESOURCE}; - std::vector default_poses_; }; } diff --git a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp index f2daa11..727a6db 100644 --- a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp +++ b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp @@ -33,6 +33,8 @@ along with this program. If not, see . #include #include +#include +#include //#include @@ -54,6 +56,11 @@ class RBObservationModel typedef Eigen::Array StateArray; typedef Eigen::Array RealArray; typedef Eigen::Array IntArray; + typedef Eigen::Matrix RealVector; + + typedef fl::ComposedVector, RealVector> PoseArray; + + public: /// constructor and destructor ********************************************* @@ -67,11 +74,15 @@ class RBObservationModel /// accessors ************************************************************** virtual void set_observation(const Observation& image) = 0; - virtual void default_state(const State& state) = 0; + virtual PoseArray& default_poses() + { + return default_poses_; + } virtual void reset() = 0; protected: fl::Real delta_time_; + PoseArray default_poses_; }; } From 82c56f4964de918353b6e070121220b42df9b385 Mon Sep 17 00:00:00 2001 From: manuel Date: Wed, 26 Aug 2015 17:49:21 +0200 Subject: [PATCH 034/131] cpu model is now dealing with poses and delta poses properly --- .../kinect_image_observation_model_cpu.hpp | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index 9ca03da..756d60f 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -127,15 +127,15 @@ class KinectImageObservationModelCPU: ~KinectImageObservationModelCPU() { } - RealArray loglikes(const StateArray& deviations, + RealArray loglikes(const StateArray& deltas, IntArray& indices, const bool& update = false) { - std::vector > new_occlusions(deviations.size()); - std::vector > new_occlusion_times(deviations.size()); + std::vector > new_occlusions(deltas.size()); + std::vector > new_occlusion_times(deltas.size()); - RealArray log_likes = RealArray::Zero(deviations.size()); - for(size_t i_state = 0; i_state < size_t(deviations.size()); i_state++) + RealArray log_likes = RealArray::Zero(deltas.size()); + for(size_t i_state = 0; i_state < size_t(deltas.size()); i_state++) { if(update) { @@ -144,17 +144,18 @@ class KinectImageObservationModelCPU: } // render the object model ----------------------------------------- - int body_count = deviations[i_state].count(); + int body_count = deltas[i_state].count(); std::vector poses(body_count); for(size_t i_obj = 0; i_obj < body_count; i_obj++) { - auto pose = this->default_poses_.component(i_obj); - auto deviation = deviations[i_state].component(i_obj); + auto pose_0 = this->default_poses_.component(i_obj); + auto delta = deltas[i_state].component(i_obj); - pose.orientation() = deviation.orientation()*pose.orientation(); - pose.position() = deviation.position() + pose.position(); + fl::PoseVector pose_1; + pose_1.orientation()=delta.orientation() * pose_0.orientation(); + pose_1.position() = delta.position() + pose_0.position(); - poses[i_obj] = pose.affine(); + poses[i_obj] = pose_1.affine(); } object_model_->set_poses(poses); std::vector intersect_indices; From e0cd8c1e941e86f5cfcabc2a91047a6aeab96961 Mon Sep 17 00:00:00 2001 From: manuel Date: Wed, 26 Aug 2015 19:19:49 +0200 Subject: [PATCH 035/131] default pose and delta pose are now being properly used in the observation models --- .../kinect_image_observation_model_cpu.hpp | 8 ++--- .../kinect_image_observation_model_gpu.hpp | 31 ++++++++++--------- 2 files changed, 20 insertions(+), 19 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index 756d60f..baf1c0f 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -151,11 +151,11 @@ class KinectImageObservationModelCPU: auto pose_0 = this->default_poses_.component(i_obj); auto delta = deltas[i_state].component(i_obj); - fl::PoseVector pose_1; - pose_1.orientation()=delta.orientation() * pose_0.orientation(); - pose_1.position() = delta.position() + pose_0.position(); + fl::PoseVector pose; + pose.orientation() = delta.orientation() * pose_0.orientation(); + pose.position() = delta.position() + pose_0.position(); - poses[i_obj] = pose_1.affine(); + poses[i_obj] = pose.affine(); } object_model_->set_poses(poses); std::vector intersect_indices; diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 937f933..b336eb2 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -191,7 +191,7 @@ class KinectImageObservationModelGPU: constants_set_ = true; } - RealArray loglikes(const StateArray& deviations, + RealArray loglikes(const StateArray& deltas, IntArray& occlusion_indices, const bool& update_occlusions = false) { @@ -207,7 +207,7 @@ class KinectImageObservationModelGPU: exit(-1); } - n_poses_ = deviations.size(); + n_poses_ = deltas.size(); std::vector flog_likelihoods (n_poses_, 0); set_number_of_poses(n_poses_); @@ -235,19 +235,20 @@ class KinectImageObservationModelGPU: { for(size_t i_obj = 0; i_obj < vertices_.size(); i_obj++) { - Affine pose; -// = deviations[i_state].component(i_obj).affine() -// * this->default_poses_[i_obj]; - Eigen::Quaternion quaternion(pose.rotation()); - Eigen::Matrix position = pose.translation(); - - poses[i_state][i_obj][0] = quaternion.w(); - poses[i_state][i_obj][1] = quaternion.x(); - poses[i_state][i_obj][2] = quaternion.y(); - poses[i_state][i_obj][3] = quaternion.z(); - poses[i_state][i_obj][4] = position[0]; - poses[i_state][i_obj][5] = position[1]; - poses[i_state][i_obj][6] = position[2]; + auto pose_0 = this->default_poses_.component(i_obj); + auto delta = deltas[i_state].component(i_obj); + + fl::PoseVector pose; + pose.orientation() = delta.orientation() * pose_0.orientation(); + pose.position() = delta.position() + pose_0.position(); + + poses[i_state][i_obj][0] = pose.orientation().quaternion().w(); + poses[i_state][i_obj][1] = pose.orientation().quaternion().x(); + poses[i_state][i_obj][2] = pose.orientation().quaternion().y(); + poses[i_state][i_obj][3] = pose.orientation().quaternion().z(); + poses[i_state][i_obj][4] = pose.position()[0]; + poses[i_state][i_obj][5] = pose.position()[1]; + poses[i_state][i_obj][6] = pose.position()[2]; } } From 2ebf336929be03214f4a1f2581ad81a81acf6131 Mon Sep 17 00:00:00 2001 From: Claudia Pfreundt Date: Thu, 27 Aug 2015 16:35:56 +0200 Subject: [PATCH 036/131] integrated initialization and setting of constants into KinectImageObservationModelGPU constructor. Provides default constant values now. --- .../kinect_image_observation_model_gpu.hpp | 174 ++++++++---------- 1 file changed, 76 insertions(+), 98 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index b336eb2..752f4b2 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -73,98 +73,39 @@ class KinectImageObservationModelGPU: // TODO: ALL THIS SHOULD SWITCH FROM USING VISIBILITY TO OCCLUSION KinectImageObservationModelGPU(const CameraMatrix& camera_matrix, - const size_t& n_rows, - const size_t& n_cols, - const size_t& max_sample_count, - const Scalar& initial_occlusion_prob, - const double& delta_time): + const size_t& n_rows, + const size_t& n_cols, + const size_t& max_sample_count, + const std::vector > vertices_double, + const std::vector > > indices, + const std::string vertex_shader_path, + const std::string fragment_shader_path, + const Scalar& initial_occlusion_prob = 0.1d, + const double& delta_time = 0.033d, + const float p_occluded_visible = 0.1f, + const float p_occluded_occluded = 0.7f, + const float tail_weight = 0.01f, + const float model_sigma = 0.003f, + const float sigma_factor = 0.0014247f, + const float max_depth = 6.0f, + const float exponential_rate = -log(0.5f)): camera_matrix_(camera_matrix), n_rows_(n_rows), n_cols_(n_cols), initial_visibility_prob_(1 - initial_occlusion_prob), max_sample_count_(max_sample_count), n_poses_(max_sample_count), - constants_set_(false), - initialized_(false), observations_set_(false), resource_registered_(false), nr_calls_set_observation_(0), observation_time_(0), Traits::Base(delta_time) { - visibility_probs_.resize(n_rows_ * n_cols_); - } - - ~KinectImageObservationModelGPU() { } - - - // TODO: DO WE NEED TWO DIFFERENT FUNCTIONS FOR THIS?? - void Initialize() - { - if (constants_set_) - { - opengl_ = boost::shared_ptr - (new ObjectRasterizer(vertices_, - indices_, - vertex_shader_path_, - fragment_shader_path_)); - cuda_ = boost::shared_ptr (new fil::CudaFilter()); - - initialized_ = true; - - - opengl_->PrepareRender(camera_matrix_.cast()); - - - opengl_->set_number_of_max_poses(max_sample_count_); - n_poses_x_ = opengl_->get_n_poses_x(); - cuda_->set_number_of_max_poses(max_sample_count_, n_poses_x_); - - - std:: cout << "set resolution in cuda..." << std::endl; - - opengl_->set_resolution(n_rows_, n_cols_); - cuda_->set_resolution(n_rows_, n_cols_); - - RegisterResource(); - - std:: cout << "set occlusions..." << std::endl; - - // set_occlusions(); - reset(); - - float c = p_visible_visible_ - p_visible_occluded_; - float log_c = log(c); - - std::vector > dummy_com_models; - cuda_->Init(dummy_com_models, 0.0f, 0.0f, - initial_visibility_prob_, c, log_c, p_visible_occluded_, - tail_weight_, model_sigma_, sigma_factor_, max_depth_, exponential_rate_); - - - count_ = 0; - - } else { - std:: cout << "WARNING: GPUImageObservationModel::Initialize() was not executed, because GPUImageObservationModel::set_constants() has not been called previously." << std::endl; - } - } - - void Constants(const std::vector > vertices_double, - const std::vector > > indices, - const float p_occluded_visible, - const float p_occluded_occluded, - const float tail_weight, - const float model_sigma, - const float sigma_factor, - const float max_depth, - const float exponential_rate, - const std::string vertex_shader_path, - const std::string fragment_shader_path) - { + // set constants this->default_poses_.recount(vertices_double.size()); this->default_poses_.setZero(); - // since you love doubles i changed the argument type of the vertices to double and convert it here :) + // convert doubles to floats vertices_.resize(vertices_double.size()); for(size_t object_index = 0; object_index < vertices_.size(); object_index++) { @@ -188,20 +129,60 @@ class KinectImageObservationModelGPU: fragment_shader_path_ = fragment_shader_path; - constants_set_ = true; + // initialize opengl and cuda + opengl_ = boost::shared_ptr + (new ObjectRasterizer(vertices_, + indices_, + vertex_shader_path_, + fragment_shader_path_)); + cuda_ = boost::shared_ptr (new fil::CudaFilter()); + + + + opengl_->PrepareRender(camera_matrix_.cast()); + + + opengl_->set_number_of_max_poses(max_sample_count_); + n_poses_x_ = opengl_->get_n_poses_x(); + cuda_->set_number_of_max_poses(max_sample_count_, n_poses_x_); + + + std:: cout << "set resolution in cuda..." << std::endl; + + opengl_->set_resolution(n_rows_, n_cols_); + cuda_->set_resolution(n_rows_, n_cols_); + + RegisterResource(); + + std:: cout << "set occlusions..." << std::endl; + +// set_occlusions(); + reset(); + + float c = p_visible_visible_ - p_visible_occluded_; + float log_c = log(c); + + std::vector > dummy_com_models; + cuda_->Init(dummy_com_models, 0.0f, 0.0f, + initial_visibility_prob_, c, log_c, p_visible_occluded_, + tail_weight_, model_sigma_, sigma_factor_, max_depth_, exponential_rate_); + + + count_ = 0; + + + visibility_probs_.resize(n_rows_ * n_cols_); } + ~KinectImageObservationModelGPU() { } + + RealArray loglikes(const StateArray& deltas, IntArray& occlusion_indices, const bool& update_occlusions = false) { - if (!initialized_) - { - std:: cout << "GPU: not initialized" << std::endl; - exit(-1); - } - else if(!observations_set_) + if(!observations_set_) { std:: cout << "GPU: observations not set" << std::endl; exit(-1); @@ -326,11 +307,10 @@ class KinectImageObservationModelGPU: void set_observation(const std::vector& observations, const Scalar& delta_time) { observation_time_ += delta_time; - if (initialized_) - { - cuda_->set_observations(observations.data(), observation_time_); - observations_set_ = true; - } + + cuda_->set_observations(observations.data(), observation_time_); + observations_set_ = true; + } void Occlusions(const float& visibility_prob = -1) @@ -351,14 +331,12 @@ class KinectImageObservationModelGPU: const size_t max_sample_count_; void set_number_of_poses(int n_poses){ - if (initialized_) { - n_poses_ = n_poses; - opengl_->set_number_of_poses(n_poses_); - n_poses_x_ = opengl_->get_n_poses_x(); - cuda_->set_number_of_poses(n_poses_, n_poses_x_); - } else { - std:: cout << "WARNING: GPUImageObservationModel::set_number_of_poses() was not executed, because GPUImageObservationModel::Initialize() has not been called previously." << std::endl; - } + + n_poses_ = n_poses; + opengl_->set_number_of_poses(n_poses_); + n_poses_x_ = opengl_->get_n_poses_x(); + cuda_->set_number_of_poses(n_poses_, n_poses_x_); + } void checkCUDAError(const char *msg) @@ -425,7 +403,7 @@ class KinectImageObservationModelGPU: int count_; // booleans to ensure correct usage of function calls - bool constants_set_, initialized_, observations_set_, resource_registered_; + bool observations_set_, resource_registered_; int nr_calls_set_observation_; // Shared resource between OpenGL and CUDA From dde83ae22ccb549e0cd267e8221024af4c7dec14 Mon Sep 17 00:00:00 2001 From: Claudia Pfreundt Date: Thu, 27 Aug 2015 16:55:05 +0200 Subject: [PATCH 037/131] adapted to new code convention and integrated existance check for vertex and fragment shader paths --- .../kinect_image_observation_model_gpu.hpp | 36 +++++++++++++------ 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 752f4b2..30883ab 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -3,6 +3,7 @@ #include #include "boost/shared_ptr.hpp" +#include "boost/filesystem.hpp" #include "Eigen/Core" #include @@ -125,6 +126,20 @@ class KinectImageObservationModelGPU: exponential_rate_ = exponential_rate; + + if(!boost::filesystem::exists(vertex_shader_path)) + { + std::cout << "vertex shader does not exist at: " + << vertex_shader_path << std::endl; + exit(-1); + } + if(!boost::filesystem::exists(fragment_shader_path)) + { + std::cout << "fragment_shader does not exist at: " + << fragment_shader_path << std::endl; + exit(-1); + } + vertex_shader_path_ = vertex_shader_path; fragment_shader_path_ = fragment_shader_path; @@ -152,11 +167,10 @@ class KinectImageObservationModelGPU: opengl_->set_resolution(n_rows_, n_cols_); cuda_->set_resolution(n_rows_, n_cols_); - RegisterResource(); + register_resource(); std:: cout << "set occlusions..." << std::endl; -// set_occlusions(); reset(); float c = p_visible_visible_ - p_visible_occluded_; @@ -284,19 +298,19 @@ class KinectImageObservationModelGPU: virtual void reset() { - Occlusions(); + set_occlusions(); observation_time_ = 0; } // TODO: this image should be in a different format BOTH OF THEM!! - const std::vector Occlusions(size_t index) const + const std::vector get_occlusions(size_t index) const { std::vector visibility_probs = cuda_->get_visibility_probabilities((int) index); return visibility_probs; } - void RangeImage(std::vector > &intersect_indices, + void get_range_image(std::vector > &intersect_indices, std::vector > &depth) { opengl_->get_depth_values(intersect_indices, depth); @@ -313,7 +327,7 @@ class KinectImageObservationModelGPU: } - void Occlusions(const float& visibility_prob = -1) + void set_occlusions(const float& visibility_prob = -1) { float default_visibility_probability = visibility_prob; if (visibility_prob == -1) default_visibility_probability = initial_visibility_prob_; @@ -339,7 +353,7 @@ class KinectImageObservationModelGPU: } - void checkCUDAError(const char *msg) + void check_cuda_error(const char *msg) { cudaError_t err = cudaGetLastError(); if( cudaSuccess != err) @@ -350,21 +364,21 @@ class KinectImageObservationModelGPU: } - void UnregisterResource() + void unregister_resource() { if (resource_registered_) { cudaGraphicsUnregisterResource(combined_texture_resource_); - checkCUDAError("cudaGraphicsUnregisterResource"); + check_cuda_error("cudaGraphicsUnregisterResource"); resource_registered_ = false; } } - void RegisterResource() + void register_resource() { if (!resource_registered_) { combined_texture_opengl_ = opengl_->get_combined_texture(); cudaGraphicsGLRegisterImage(&combined_texture_resource_, combined_texture_opengl_, GL_TEXTURE_2D, cudaGraphicsRegisterFlagsReadOnly); - checkCUDAError("cudaGraphicsGLRegisterImage)"); + check_cuda_error("cudaGraphicsGLRegisterImage)"); resource_registered_ = true; } } From 9df04a9f28c4a3ee7c6b463bc97b7a9f299b09a2 Mon Sep 17 00:00:00 2001 From: Claudia Pfreundt Date: Thu, 27 Aug 2015 18:02:18 +0200 Subject: [PATCH 038/131] deleted needless classes. changed constructor of rasterizer to include projection matrix setup. --- CMakeLists.txt | 4 +- .../cuda_opengl_filter.hpp | 222 -------- .../cuda_opengl_multiple_filter.hpp | 42 -- .../kinect_image_observation_model_gpu.hpp | 27 +- .../object_rasterizer.hpp | 20 +- .../cuda_opengl_filter.cpp | 478 ------------------ .../cuda_opengl_multiple_filter.cpp | 210 -------- .../object_rasterizer.cpp | 16 +- 8 files changed, 25 insertions(+), 994 deletions(-) delete mode 100644 include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.hpp delete mode 100644 include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.hpp delete mode 100644 src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.cpp delete mode 100644 src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a54f7f8..2fcc751 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -171,8 +171,8 @@ if(USE_CUDA AND CUDA_FOUND AND OPENGL_FOUND AND GLEW_FOUND) message("building cuda implementation") set(dbot_gpu_sources - src/${PROJECT_NAME_DBOT}/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.cpp - src/${PROJECT_NAME_DBOT}/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.cpp + #src/${PROJECT_NAME_DBOT}/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.cpp + #src/${PROJECT_NAME_DBOT}/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.cpp src/${PROJECT_NAME_DBOT}/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp src/${PROJECT_NAME_DBOT}/models/observation_models/kinect_image_observation_model_gpu/shader.cpp) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.hpp deleted file mode 100644 index e5e662d..0000000 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.hpp +++ /dev/null @@ -1,222 +0,0 @@ -#ifndef POSE_TRACKING_MODELS_OBSERVATION_MODELS_CUDA_OPENGL_FILTER_HPP -#define POSE_TRACKING_MODELS_OBSERVATION_MODELS_CUDA_OPENGL_FILTER_HPP - -#include -#include -#include "boost/shared_ptr.hpp" - -#include -#include - -namespace fil { - -/// A particle filter that uses the GPU through OpenGL and CUDA. -/** It provides a propagate, evaluate and resample function and a couple of functions for initializing - * values and copying them to the GPU. - */ -class CudaOpenglFilter -{ -public: - /// constructor which does not do much, except for initializing some boolean values - CudaOpenglFilter(); - - /// destructor which outputs the average time used for rasterization and comparison - ~CudaOpenglFilter(); - - /// creates cuda and opengl object and initializes some values - /** necessary previous call: set_constants(..) - * should be called once, after set_constants(..) and before anything else - */ - void Initialize(); - - - - /// propagates the states that already reside on the GPU and returns them in the states vector - /** necessary previous calls: Initialize(), set_states(..) - * @param[in] current_time a number that will be subtracted from the previously passed - * number to build the delta_time needed for propagation - * @param[out] states the states vector that is returned from this function, containing - * the new (propagated) set of states - */ - void Propagate(const float ¤t_time, std::vector > &states); - - - - /// evaluates the different states that are passed by rendering them and comparing them to the observations - /** necessary previous call: Initialize() - * This Evaluate(..)(1) function is for evaluating the initial states, hence it does not need a current - * observation time (it is 0). It does not use occlusion. All visibility probabilites have the same - * (default) value. - * @param states [state_nr][feature_nr (0-6, 7-13, .. for respective object)] = feature value - * @param observations [pixel] = depth observed at that pixel - * @param[out] log_likelihoods [state_nr] = log_likelihood calculated and returned by this function - */ - void Evaluate( - const std::vector > > &states, - const std::vector &observations, - std::vector &log_likelihoods); - - - - /// evaluates the different states that are passed by rendering them and comparing them to the observations - /** necessary previous call: Initialize(), Evaluate(..)(the other one) - * This Evaluate(..)(2) function is for evaluating the regular states every filter round. - * @param states [state_nr][feature_nr (0-6, 7-13, .. for respective object)] = feature value - * @param observations [pixel] = depth observed at that pixel - * @param observation_time current time that will be subtracted from the previous time, to find out - * how much time passed and calculate the occlusion probability accordingly - * @param[out] log_likelihoods [state_nr] = log_likelihood calculated and returned by this function - */ - void Evaluate( - const std::vector > > &states, - const std::vector &observations, - const float &observation_time, - std::vector &log_likelihoods); - - - - /// resamples the states that already reside on the GPU. - /** necessary previous calls: Initialize(), set_states(..) - * Depending on their likelihood, the states are being resampled and their - * data (features, visibility probabilities) are shifted accordingly on the GPU. - * @return [old_state_nr] = new_state_nr - */ - std::vector Resample(); - - /// sends the specified observations to the GPU - /** necessary previous call: Initialize() - * This function has to be called previously to calling Evaluate(), since the observations - * are needed for the likelihood computation that takes place in the Evaluate() function. - * @param observations [pixel] = depth observed at that pixel - */ - void set_observations(const std::vector &observations); - - - - - /// sets all the constants needed for the filtering - /** should be called once, as soon as the caller knows the values for all constants - * @param vertices [object_nr][vertex_nr] = vertex of that object - * @param indices [object_nr][triangle_nr][vertex_index (0|1|2)] = index of this vertex - * in its vertices list - * @param n_init_poses number of initial poses, used to locate the object - * @param n_poses number of poses, used in each filter round - * @param angle_sigma used for propagation of the states - * @param trans_sigma used for propagation of the states - * @param com_models [object_nr][dimension (0|1|2)] = center of mass model of that object - * @param camera_matrix matrix containing the intrinsic parameters of the camera - * @param p_visible_init initial probability for a pixel to be visible - * @param p_visible_visible probability that a pixel is visible if it was visible before - * @param p_visible_occluded probability that a pixel is visible if it was occluded before - * @param tail_weight used for probability calculation in the comparison step - * @param model_sigma used for probability calculation in the comparison step - * @param sigma_factor used for probability calculation in the comparison step - * @param max_depth used for probability calculation in the comparison step - * @param exponential_rate used for probability calculation in the comparison step - */ - void set_constants(const std::vector > vertices, - const std::vector > > indices, - const int n_init_poses, - const int n_poses, - const float angle_sigma, - const float trans_sigma, - const std::vector > com_models, - const Eigen::Matrix3f camera_matrix, - const float p_visible_init, - const float p_visible_visible, - const float p_visible_occluded, - const float tail_weight, - const float model_sigma, - const float sigma_factor, - const float max_depth, - const float exponential_rate, const std::string vertex_shader_path, const std::string fragment_shader_path); - - /// sets the resolution - /** necessary previous call: Initialize() - * should be called once after initialization to change the resolution if it is not - * the default of 60x80. - * @param n_cols number of columns (width) - * @param n_rows number of rows (height) - */ - void set_resolution(int n_cols, int n_rows); - - /// initializes the states for the regular filter rounds - /** necessary previous call: Initialize() - * needs to be called before any call to Propagate() or Resample(), because they - * change the status of the states on the GPU. - * @param states [state_nr][feature_nr (0-6, 7-13, .. for respective object)] = feature value - * @param seed a number used as seed for a random number generator - */ - void set_states(std::vector > states, int seed); - - void set_number_of_poses(int n_poses); - - /// only needed for CUDA debugging. ignore. - void destroy_context(); - - -protected: - - void checkCUDAError(const char *msg); - void UnregisterResource(); - void RegisterResource(); - - boost::shared_ptr opengl_; - boost::shared_ptr cuda_; - - // resolution - int n_cols_, n_rows_; - - // arrays for timings - std::vector > cpu_times_; - std::vector > cuda_times_; - - // constants - std::vector > vertices_; - std::vector > > indices_; - std::vector log_likelihoods_; - std::vector > com_models_; - Eigen::Matrix3f camera_matrix_; - float angle_sigma_, trans_sigma_; - float p_visible_init_; - float p_visible_visible_; - float p_visible_occluded_; - float tail_weight_; - float model_sigma_; - float sigma_factor_; - float max_depth_; - float exponential_rate_; - - std::string vertex_shader_path_; - std::string fragment_shader_path_; - - - int n_init_poses_, n_regular_poses_; - int n_poses_; - int n_renders_; - int n_poses_x_; - - int count_; - - // booleans to ensure correct usage of function calls - bool constants_set_, resolution_set_, states_set_, initialized_, observations_set_, resource_registered_; - - // Shared resource between OpenGL and CUDA - GLuint combined_texture_opengl_; - cudaGraphicsResource* combined_texture_resource_; - cudaArray_t texture_array_; - -private: - - static const int TIME_MEASUREMENTS_COUNT = 8; - static const int COUNT = 500; - - enum time_measurement {SEND_OBSERVATIONS, RENDER, MAP_RESOURCE, GET_MAPPED_ARRAY, SET_TEXTURE_ARRAY, - MAP_TEXTURE, COMPUTE_LIKELIHOODS, UNMAP_RESOURCE}; - - -}; - -} - -#endif // CUDA_OPENGL_FILTER_HPP diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.hpp deleted file mode 100644 index 3f779d8..0000000 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.hpp +++ /dev/null @@ -1,42 +0,0 @@ -#ifndef POSE_TRACKING_MODELS_OBSERVATION_MODELS_CUDA_OPENGL_MULTIPLE_FILTER_HPP -#define POSE_TRACKING_MODELS_OBSERVATION_MODELS_CUDA_OPENGL_MULTIPLE_FILTER_HPP - -#include -namespace fil { - -class CudaOpenglMultipleFilter : public fil::CudaOpenglFilter -{ -public: - - void PropagateMultiple(const float ¤t_time, std::vector > > &states); - - void EvaluateMultiple(const std::vector > > &states, - const std::vector prev_sample_indices, - const float &observation_time, - bool update, - std::vector &log_likelihoods); - - void InitialEvaluateMultiple(int n_objects, - const std::vector > > &states, - const std::vector &observations, - std::vector > &log_likelihoods); - - /// resamples the visibility probabilities that reside on the GPU according to the indices given. - /** necessary previous call: Initialize() - * @param resampling_indices [new_state_nr] = old_state_nr - */ - void ResampleMultiple(std::vector resampling_indices); - - void set_states_multiple(int n_objects, int n_features, int seed); - -private: - - static const int TIME_MEASUREMENTS_COUNT = 4; - static const int COUNT = 500; - - enum time_measurement {SEND_INDICES, RENDER, MAP_RESOURCE, COMPUTE_LIKELIHOODS}; - -}; -} - -#endif // CUDA_OPENGL_MULTIPLE_FILTER_HPP diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 30883ab..132570d 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -93,8 +93,16 @@ class KinectImageObservationModelGPU: camera_matrix_(camera_matrix), n_rows_(n_rows), n_cols_(n_cols), - initial_visibility_prob_(1 - initial_occlusion_prob), max_sample_count_(max_sample_count), + indices_(indices), + initial_visibility_prob_(1 - initial_occlusion_prob), + p_visible_visible_(1.0 - p_occluded_visible), + p_visible_occluded_(1.0 - p_occluded_occluded), + tail_weight_(tail_weight), + model_sigma_(model_sigma), + sigma_factor_(sigma_factor), + max_depth_(max_depth), + exponential_rate_(exponential_rate), n_poses_(max_sample_count), observations_set_(false), resource_registered_(false), @@ -116,17 +124,7 @@ class KinectImageObservationModelGPU: } - indices_ = indices; - p_visible_visible_ = 1.0 - p_occluded_visible; - p_visible_occluded_ = 1.0 - p_occluded_occluded; - tail_weight_ = tail_weight; - model_sigma_ = model_sigma; - sigma_factor_ = sigma_factor; - max_depth_ = max_depth; - exponential_rate_ = exponential_rate; - - - + // check for incorrect path names if(!boost::filesystem::exists(vertex_shader_path)) { std::cout << "vertex shader does not exist at: " @@ -149,12 +147,13 @@ class KinectImageObservationModelGPU: (new ObjectRasterizer(vertices_, indices_, vertex_shader_path_, - fragment_shader_path_)); + fragment_shader_path_, + camera_matrix_.cast())); cuda_ = boost::shared_ptr (new fil::CudaFilter()); - opengl_->PrepareRender(camera_matrix_.cast()); + //opengl_->PrepareRender(camera_matrix_.cast()); opengl_->set_number_of_max_poses(max_sample_count_); diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp index 22b05fd..b27bd57 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp @@ -23,11 +23,15 @@ class ObjectRasterizer * @param[in] indices [object_nr][index_nr][0 - 2] = {index}. This list should contain the indices * that index the vertices list and tell us which vertices to connect to a triangle (every group of 3). * For each object, the indices should be in the range of [0, nr_vertices - 1]. + * @param[in] vertex_shader_path path to the vertex shader + * @param[in] fragment_shader_path path to the fragment shader + * @param[in] camera_matrix matrix of the intrinsic parameters of the camera */ ObjectRasterizer(const std::vector > vertices, const std::vector > > indices, const std::string vertex_shader_path, - const std::string fragment_shader_path); + const std::string fragment_shader_path, + const Eigen::Matrix3f camera_matrix); /// constructor with no arguments, should not be used ObjectRasterizer(); @@ -36,16 +40,6 @@ class ObjectRasterizer ~ObjectRasterizer(); - /// prepare openGL for a function call to Render() - /** Has to be called once before calling Render() (which can then be - * called frequently). It sets the shaders to the correct program and computes - * the perspective projection matrix from the camera parameters. The near/far planes - * are set by the constants NEAR_PLANE and FAR_PLANE in the header. - * @param[in] camera_matrix matrix of the intrinsic parameters of the camera - */ - void PrepareRender(const Eigen::Matrix3f camera_matrix); - - /// render the objects in all given states and return the depth for all relevant pixels of each rendered object /** This function renders all poses (of all objects) into one large texture. Reading back the depth values @@ -54,7 +48,7 @@ class ObjectRasterizer * @param[in] states [pose_nr][object_nr][0 - 6] = {qw, qx, qy, qz, tx, ty, tz}. This should contain the quaternion * and the translation for each object per pose. * @param[in,out] intersect_indices [pose_nr][0 - nr_relevant_pixels] = {pixel_nr}. This list should be empty when passed - * to the function. Afterwards, it will contain the pixel number of all pixels that were rendered to, per pose. + * to the function. Afterwards, it will contain the pixel numbers of all pixels that were rendered to, per pose. * @param[in,out] depth [pose_nr][0 - nr_relevant_pixels] = {depth_value}. This list should be empty when passed to the function. * Afterwards, it will contain the depth value of all pixels that were rendered to, per pose. */ @@ -79,7 +73,7 @@ class ObjectRasterizer void set_objects(std::vector object_numbers); /// set a new resolution - /** This function reallocates the framebuffer textures. + /** This function reallocates the framebuffer textures. This is expensive, so only do it if necessary. * @param[in] n_rows the height of the image * @param[in] n_cols the width of the image */ diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.cpp b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.cpp deleted file mode 100644 index 15e7b0b..0000000 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.cpp +++ /dev/null @@ -1,478 +0,0 @@ -/** @author Claudia Pfreundt */ - -//#define PROFILING_ACTIVE - -#include -#include -#include - -#include - -#include -#include -#include - - -#include -#include - -using namespace std; -using namespace Eigen; -using namespace fil; - - -CudaOpenglFilter::CudaOpenglFilter() : - constants_set_(false), - resolution_set_(false), - states_set_(false), - initialized_(false), - observations_set_(false), - resource_registered_(false) -{ -} - -void CudaOpenglFilter::Initialize() -{ - if (constants_set_) - { - opengl_ = boost::shared_ptr - (new ObjectRasterizer(vertices_, - indices_, - vertex_shader_path_, - fragment_shader_path_)); - cuda_ = boost::shared_ptr (new CudaFilter()); - - initialized_ = true; - set_number_of_poses(n_init_poses_); - -// combined_texture_opengl_ = opengl_->get_combined_texture(); - - - - - float c = p_visible_visible_ - p_visible_occluded_; - float log_c = log(c); - - cuda_->Init(com_models_, angle_sigma_, trans_sigma_, - p_visible_init_, c, log_c, p_visible_occluded_, - tail_weight_, model_sigma_, sigma_factor_, max_depth_, exponential_rate_); - - - log_likelihoods_.resize(n_init_poses_); - count_ = 0; - - } else { - cout << "WARNING: CudaOpenglFilter::Initialize() was not executed, because CudaOpenglFilter::set_constants() has not been called previously." << endl; - } -} - - - - - - - -void CudaOpenglFilter::Propagate(const float ¤t_time, vector > &states) { - if (initialized_ && states_set_) { - cuda_->Propagate(current_time, states); - } else { - cout << "WARNING: CudaOpenglFilter::Propagate() was not executed, because CudaOpenglFilter::Initialize() has not been called previously." << endl; - } -} - - - -void CudaOpenglFilter::Evaluate( - const vector > > &states, - const vector &observations, - vector &log_likelihoods) -{ - if (initialized_) { - cuda_->set_observations(observations.data()); - opengl_->Render(states); - - // map the resources in CUDA - cudaGraphicsMapResources(1, &combined_texture_resource_, 0); - cudaGraphicsSubResourceGetMappedArray(&texture_array_, combined_texture_resource_, 0, 0); - - cuda_->set_texture_array(texture_array_); - cuda_->MapTexture(); - float observation_time = 0; - - cuda_->Compare(observation_time, true, log_likelihoods); - log_likelihoods_ = log_likelihoods; - - cudaGraphicsUnmapResources(1, &combined_texture_resource_, 0); - - // change to regular number of poses - set_number_of_poses(n_regular_poses_); - - // TODO don't remember what this is good for? Should it not stay the same size until after resampling? - log_likelihoods_.resize(n_poses_); - } else { - cout << "WARNING: CudaOpenglFilter::Evaluate() was not executed, because CudaOpenglFilter::Initialize() has not been called previously." << endl; - } -} - - - -void CudaOpenglFilter::Evaluate( - const vector > > &states, - const vector &observations, - const float &observation_time, - vector &log_likelihoods) -{ - if (initialized_) { - -#ifdef PROFILING_ACTIVE - cudaEvent_t start_event, stop_event; - cudaEventCreate(&start_event); - cudaEventCreate(&stop_event); - float milliseconds; - double start; - double stop; - vector cuda_times; - vector cpu_times; - cudaEventRecord(start_event); - start = sf::hf::get_wall_time(); -#endif - - cuda_->set_observations(observations.data()); - -#ifdef PROFILING_ACTIVE - stop = sf::hf::get_wall_time(); - cpu_times.push_back(stop - start); - cudaEventRecord(stop_event); - cudaEventSynchronize(stop_event); - cudaEventElapsedTime(&milliseconds, start_event, stop_event); - cuda_times.push_back(milliseconds); - cudaEventRecord(start_event); - start = sf::hf::get_wall_time(); -#endif - - - opengl_->Render(states); - -#ifdef PROFILING_ACTIVE - stop = sf::hf::get_wall_time(); - cpu_times.push_back(stop - start); - cudaEventRecord(stop_event); - cudaEventSynchronize(stop_event); - cudaEventElapsedTime(&milliseconds, start_event, stop_event); - cuda_times.push_back(milliseconds); - cudaEventRecord(start_event); - start = sf::hf::get_wall_time(); -#endif - -// map the resources in CUDA - cudaGraphicsMapResources(1, &combined_texture_resource_, 0); - -#ifdef PROFILING_ACTIVE - stop = sf::hf::get_wall_time(); - cpu_times.push_back(stop - start); - cudaEventRecord(stop_event); - cudaEventSynchronize(stop_event); - cudaEventElapsedTime(&milliseconds, start_event, stop_event); - cuda_times.push_back(milliseconds); - cudaEventRecord(start_event); - start = sf::hf::get_wall_time(); -#endif - - cudaGraphicsSubResourceGetMappedArray(&texture_array_, combined_texture_resource_, 0, 0); - -#ifdef PROFILING_ACTIVE - stop = sf::hf::get_wall_time(); - cpu_times.push_back(stop - start); - cudaEventRecord(stop_event); - cudaEventSynchronize(stop_event); - cudaEventElapsedTime(&milliseconds, start_event, stop_event); - cuda_times.push_back(milliseconds); - cudaEventRecord(start_event); - start = sf::hf::get_wall_time(); -#endif - - cuda_->set_texture_array(texture_array_); - -#ifdef PROFILING_ACTIVE - stop = sf::hf::get_wall_time(); - cpu_times.push_back(stop - start); - cudaEventRecord(stop_event); - cudaEventSynchronize(stop_event); - cudaEventElapsedTime(&milliseconds, start_event, stop_event); - cuda_times.push_back(milliseconds); - cudaEventRecord(start_event); - start = sf::hf::get_wall_time(); -#endif - - cuda_->MapTexture(); - -#ifdef PROFILING_ACTIVE - stop = sf::hf::get_wall_time(); - cpu_times.push_back(stop - start); - cudaEventRecord(stop_event); - cudaEventSynchronize(stop_event); - cudaEventElapsedTime(&milliseconds, start_event, stop_event); - cuda_times.push_back(milliseconds); - cudaEventRecord(start_event); - start = sf::hf::get_wall_time(); -#endif - - cuda_->Compare(observation_time, false, log_likelihoods); - log_likelihoods_ = log_likelihoods; - -#ifdef PROFILING_ACTIVE - stop = sf::hf::get_wall_time(); - cpu_times.push_back(stop - start); - cudaEventRecord(stop_event); - cudaEventSynchronize(stop_event); - cudaEventElapsedTime(&milliseconds, start_event, stop_event); - cuda_times.push_back(milliseconds); - cudaEventRecord(start_event); - start = sf::hf::get_wall_time(); -#endif - - cudaGraphicsUnmapResources(1, &combined_texture_resource_, 0); - -#ifdef PROFILING_ACTIVE - stop = sf::hf::get_wall_time(); - cpu_times.push_back(stop - start); - cudaEventRecord(stop_event); - cudaEventSynchronize(stop_event); - cudaEventElapsedTime(&milliseconds, start_event, stop_event); - cuda_times.push_back(milliseconds); - - cpu_times_.push_back(cpu_times); - cuda_times_.push_back(cuda_times); - count_++; - - if (count_ == COUNT) { - string names[TIME_MEASUREMENTS_COUNT]; - names[SEND_OBSERVATIONS] = "send observations"; - names[RENDER] = "render poses"; - names[MAP_RESOURCE] = "map resource"; - names[GET_MAPPED_ARRAY] = "get mapped array"; - names[SET_TEXTURE_ARRAY] = "set texture array"; - names[MAP_TEXTURE] = "map texture"; - names[COMPUTE_LIKELIHOODS] = "compute likelihoods"; - names[UNMAP_RESOURCE] = "unmap resource"; - - float final_cpu_times[TIME_MEASUREMENTS_COUNT] = {0}; - float final_cuda_times[TIME_MEASUREMENTS_COUNT] = {0}; - - for (int i = 0; i < count_; i++) { - for (int j = 0; j < TIME_MEASUREMENTS_COUNT; j++) { - final_cpu_times[j] += cpu_times_[i][j]; - final_cuda_times[j] += cuda_times_[i][j]; - } - } - - float total_time_cuda = 0; - float total_time_cpu = 0; - for (int i = 0; i < TIME_MEASUREMENTS_COUNT; i++) { - cout << names[i] << ": \t(GPU) " << final_cuda_times[i] * 1e3 / count_<< "\t(CPU) " << final_cpu_times[i] * 1e6 / count_<< endl; - total_time_cuda += final_cuda_times[i] * 1e3 / count_; - total_time_cpu += final_cpu_times[i] * 1e6 / count_; - } - cout << "TOTAL: " << "\t(GPU) " << total_time_cuda << "\t(CPU) " << total_time_cpu << endl; - } -#endif - - } else { - cout << "WARNING: CudaOpenglFilter::Evaluate() was not executed, because CudaOpenglFilter::Initialize() has not been called previously." << endl; - } -} - - - -// only use if log_likelihoods were assigned after Evalute() step -vector CudaOpenglFilter::Resample() -{ - vector resampling_indices; - ff::hf::DiscreteSampler sampler(log_likelihoods_); - - for (int i = 0; i < n_poses_; i++) { - resampling_indices.push_back(sampler.Sample()); - } - - if (initialized_ && states_set_) { - cuda_->Resample(resampling_indices); - } else { - cout << "WARNING: CudaOpenglFilter::Resample() was not executed, because CudaOpenglFilter::Initialize() or ::set_states() has not been called previously." << endl; - } - - return resampling_indices; -} - - - - -// ===================================================================================== // -// ==================================== SETTERS ======================================= // -// ===================================================================================== // - - -void CudaOpenglFilter::set_observations(const vector &observations) { - if (initialized_) { - cuda_->set_observations(observations.data()); - observations_set_ = true; - // TODO observations_set_ = false in some function.. evaluate or resample - } -} - -void CudaOpenglFilter::set_constants(const std::vector > vertices, - const std::vector > > indices, - const int n_init_poses, - const int n_poses, - const float angle_sigma, - const float trans_sigma, - const std::vector > com_models, - const Eigen::Matrix3f camera_matrix, - const float p_visible_init, - const float p_visible_visible, - const float p_visible_occluded, - const float tail_weight, - const float model_sigma, - const float sigma_factor, - const float max_depth, - const float exponential_rate, - const std::string vertex_shader_path, - const std::string fragment_shader_path) { - vertices_ = vertices; - indices_ = indices; - n_init_poses_ = n_init_poses; - n_regular_poses_ = n_poses; - angle_sigma_ = angle_sigma; - trans_sigma_ = trans_sigma; - com_models_ = com_models; - camera_matrix_ = camera_matrix; - p_visible_init_ = p_visible_init; - p_visible_visible_ = p_visible_visible; - p_visible_occluded_ = p_visible_occluded; - tail_weight_ = tail_weight; - model_sigma_ = model_sigma; - sigma_factor_ = sigma_factor; - max_depth_ = max_depth; - exponential_rate_ = exponential_rate; - vertex_shader_path_ = vertex_shader_path; - fragment_shader_path_ = fragment_shader_path; - - - constants_set_ = true; -} - -void CudaOpenglFilter::set_number_of_poses(int n_poses) { - if (initialized_) { - // resource needs to be unregistered so that OpenGL can reallocate the texture with a different size - UnregisterResource(); - - n_poses_ = n_poses; - opengl_->set_number_of_max_poses(n_poses_); - opengl_->PrepareRender(camera_matrix_); - n_poses_x_ = opengl_->get_n_poses_x(); - - cuda_->set_number_of_max_poses(n_poses_, n_poses_x_); - - - RegisterResource(); - } else { - cout << "WARNING: CudaOpenglFilter::set_number_of_poses() was not executed, because CudaOpenglFilter::Initialize() has not been called previously." << endl; - } -} - -void CudaOpenglFilter::set_resolution(int n_cols, int n_rows) { - if (initialized_) { - n_cols_ = n_cols; - n_rows_ = n_rows; - // resource needs to be unregistered so that OpenGL can reallocate the texture with a different size - UnregisterResource(); - - opengl_->set_resolution(n_rows_, n_cols_); - cuda_->set_resolution(n_rows_, n_cols_); - - resolution_set_ = true; - - RegisterResource(); - } else { - cout << "WARNING: CudaOpenglFilter::set_resolution() was not executed, because CudaOpenglFilter::Initialize() has not been called previously." << endl; - } -} - - - -void CudaOpenglFilter::set_states(vector > states, int seed) { - if (initialized_) { - cuda_->set_states(states, seed); - states_set_ = true; - } else { - cout << "WARNING: CudaOpenglFilter::set_states() was not executed, because CudaOpenglFilter::Initialize() has not been called previously." << endl; - } -} - - - - - -void CudaOpenglFilter::UnregisterResource() { - if (resource_registered_) { - cudaGraphicsUnregisterResource(combined_texture_resource_); - checkCUDAError("cudaGraphicsUnregisterResource"); - resource_registered_ = false; - } -} - - -void CudaOpenglFilter::RegisterResource() { - if (!resource_registered_) { - combined_texture_opengl_ = opengl_->get_combined_texture(); - cudaGraphicsGLRegisterImage(&combined_texture_resource_, combined_texture_opengl_, GL_TEXTURE_2D, cudaGraphicsRegisterFlagsReadOnly); - checkCUDAError("cudaGraphicsGLRegisterImage)"); - resource_registered_ = true; - } -} - - - - - - -void CudaOpenglFilter::checkCUDAError(const char *msg) -{ - cudaError_t err = cudaGetLastError(); - if( cudaSuccess != err) - { - fprintf(stderr, "Cuda error: %s: %s.\n", msg, cudaGetErrorString( err) ); - exit(EXIT_FAILURE); - } -} - - - -CudaOpenglFilter::~CudaOpenglFilter() { -// delete combined_texture_resource_; -} - -void CudaOpenglFilter::destroy_context() { - cuda_->destroy_context(); -} - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.cpp b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.cpp deleted file mode 100644 index 816da1e..0000000 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.cpp +++ /dev/null @@ -1,210 +0,0 @@ -#define PROFILING_ACTIVE - -#include -#include - -#include - -using namespace fil; -using namespace std; - - -void CudaOpenglMultipleFilter::PropagateMultiple(const float ¤t_time, vector > > &states) { - if (initialized_) { - cuda_->PropagateMultiple(current_time, states); - } else { - cout << "WARNING: CudaOpenglFilter::Propagate() was not executed, because CudaOpenglFilter::Initialize() has not been called previously." << endl; - } -} - - -// REMEMBER TO SET n_poses_ RIGHT WHEN YOU EVALUATE MORE THAN N_PARTICLES -void CudaOpenglMultipleFilter::EvaluateMultiple(const vector > > &states, - const vector prev_sample_indices, - const float &observation_time, - bool update, - vector &log_likelihoods) { - if (initialized_ && observations_set_) { - -#ifdef PROFILING_ACTIVE - cudaEvent_t start_event, stop_event; - cudaEventCreate(&start_event); - cudaEventCreate(&stop_event); - float milliseconds; - double start; - double stop; - vector cuda_times; - vector cpu_times; - cudaEventRecord(start_event); - start = ff::hf::get_wall_time(); -#endif - cuda_->set_prev_sample_indices(prev_sample_indices.data()); - - -#ifdef PROFILING_ACTIVE - stop = ff::hf::get_wall_time(); - cpu_times.push_back(stop - start); - cudaEventRecord(stop_event); - cudaEventSynchronize(stop_event); - cudaEventElapsedTime(&milliseconds, start_event, stop_event); - cuda_times.push_back(milliseconds); - cudaEventRecord(start_event); - start = ff::hf::get_wall_time(); -#endif - - opengl_->Render(states); - - -#ifdef PROFILING_ACTIVE - stop = ff::hf::get_wall_time(); - cpu_times.push_back(stop - start); - cudaEventRecord(stop_event); - cudaEventSynchronize(stop_event); - cudaEventElapsedTime(&milliseconds, start_event, stop_event); - cuda_times.push_back(milliseconds); - cudaEventRecord(start_event); - start = ff::hf::get_wall_time(); -#endif - -// std::vector > intersect_indices; -// std::vector > depth; -// opengl_->RenderCombinedFast(states, -// intersect_indices, -// depth); - -// for (int i = 0; i < intersect_indices.size(); i++) { -// cout << "particle: " << i << endl; -// for (int j = 0; j < intersect_indices[i].size(); j++) { -// cout << "index: " << intersect_indices[i][j] << ", depth: " << depth[i][j] << endl; -// } -// } - - cudaGraphicsMapResources(1, &combined_texture_resource_, 0); - cudaGraphicsSubResourceGetMappedArray(&texture_array_, combined_texture_resource_, 0, 0); - cuda_->set_texture_array(texture_array_); - cuda_->MapTexture(); - - -#ifdef PROFILING_ACTIVE - stop = ff::hf::get_wall_time(); - cpu_times.push_back(stop - start); - cudaEventRecord(stop_event); - cudaEventSynchronize(stop_event); - cudaEventElapsedTime(&milliseconds, start_event, stop_event); - cuda_times.push_back(milliseconds); - cudaEventRecord(start_event); - start = ff::hf::get_wall_time(); -#endif - cuda_->Compare(observation_time, false, log_likelihoods); -// cuda_->CompareMultiple(observation_time, false, update, log_likelihoods); - log_likelihoods_ = log_likelihoods; - cudaGraphicsUnmapResources(1, &combined_texture_resource_, 0); - - -#ifdef PROFILING_ACTIVE - stop = ff::hf::get_wall_time(); - cpu_times.push_back(stop - start); - cudaEventRecord(stop_event); - cudaEventSynchronize(stop_event); - cudaEventElapsedTime(&milliseconds, start_event, stop_event); - cuda_times.push_back(milliseconds); - - cpu_times_.push_back(cpu_times); - cuda_times_.push_back(cuda_times); - count_++; - - if (count_ == COUNT) { - string names[TIME_MEASUREMENTS_COUNT]; - names[SEND_INDICES] = "send indices"; - names[RENDER] = "render poses"; - names[MAP_RESOURCE] = "map resource"; - names[COMPUTE_LIKELIHOODS] = "compute likelihoods"; - - float final_cpu_times[TIME_MEASUREMENTS_COUNT] = {0}; - float final_cuda_times[TIME_MEASUREMENTS_COUNT] = {0}; - - for (int i = 0; i < count_; i++) { - for (int j = 0; j < TIME_MEASUREMENTS_COUNT; j++) { - final_cpu_times[j] += cpu_times_[i][j]; - final_cuda_times[j] += cuda_times_[i][j]; - } - } - - float total_time_cuda = 0; - float total_time_cpu = 0; - cout << "EvaluateMultiple() runtimes: " << endl; - for (int i = 0; i < TIME_MEASUREMENTS_COUNT; i++) { - cout << names[i] << ": \t(GPU) " << final_cuda_times[i] * 1e3 / count_<< "\t(CPU) " << final_cpu_times[i] * 1e6 / count_<< endl; - total_time_cuda += final_cuda_times[i] * 1e3 / count_; - total_time_cpu += final_cpu_times[i] * 1e6 / count_; - } - cout << "TOTAL: " << "\t(GPU) " << total_time_cuda << "\t(CPU) " << total_time_cpu << endl; - } -#endif - - } else { - cout << "WARNING: CudaOpenglFilter::EvaluateMultiple() was not executed, because CudaOpenglFilter::Initialize() or CudaOpenglFilter::set_observations() has not been called previously." << endl; - } -} - - - -void CudaOpenglMultipleFilter::InitialEvaluateMultiple( - int n_objects, - const vector > > &states, - const vector &observations, - vector > &log_likelihoods) -{ - if (initialized_) { - cuda_->set_observations(observations.data()); - for (int i = 0; i < n_objects; i++) { - vector number (1, i); - opengl_->set_objects(number); - opengl_->Render(states); - - // map the resources in CUDA - cudaGraphicsMapResources(1, &combined_texture_resource_, 0); - cudaGraphicsSubResourceGetMappedArray(&texture_array_, combined_texture_resource_, 0, 0); - - cuda_->set_texture_array(texture_array_); - cuda_->MapTexture(); - float observation_time = 0; - - vector tmp_log_likelihoods (n_poses_, 0); - cuda_->Compare(observation_time, true, tmp_log_likelihoods); - log_likelihoods.push_back(tmp_log_likelihoods); -// log_likelihoods_ = log_likelihoods; - - cudaGraphicsUnmapResources(1, &combined_texture_resource_, 0); - } - vector numbers; - for (int i = 0; i < n_objects; i++) numbers.push_back(i); - opengl_->set_objects(numbers); - - // change to regular number of poses - set_number_of_poses(n_regular_poses_); - - // TODO don't remember what this is good for? - log_likelihoods_.resize(n_poses_); - } else { - cout << "WARNING: CudaOpenglFilter::Evaluate() was not executed, because CudaOpenglFilter::Initialize() has not been called previously." << endl; - } -} - - -void CudaOpenglMultipleFilter::ResampleMultiple(vector resampling_indices) { - if (initialized_) { - cuda_->ResampleMultiple(resampling_indices); - } else { - cout << "WARNING: CudaOpenglFilter::Resample() was not executed, because CudaOpenglFilter::Initialize() has not been called previously." << endl; - } -} - -void CudaOpenglMultipleFilter::set_states_multiple(int n_objects, int n_features, int seed) { - if (initialized_) { - cuda_->set_states_multiple(n_objects, n_features, seed); - states_set_ = true; - } else { - cout << "WARNING: CudaOpenglFilter::set_states() was not executed, because CudaOpenglFilter::Initialize() has not been called previously." << endl; - } -} diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp index 0716b7c..95fc250 100644 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp +++ b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp @@ -28,7 +28,8 @@ ObjectRasterizer::ObjectRasterizer() ObjectRasterizer::ObjectRasterizer(const std::vector > vertices, const std::vector > > indices, const std::string vertex_shader_path, - const std::string fragment_shader_path) : + const std::string fragment_shader_path, + const Eigen::Matrix3f camera_matrix) : n_rows_(WINDOW_HEIGHT), n_cols_(WINDOW_WIDTH), vertex_shader_path_(vertex_shader_path), @@ -311,21 +312,10 @@ ObjectRasterizer::ObjectRasterizer(const std::vector Date: Thu, 27 Aug 2015 18:30:28 +0200 Subject: [PATCH 039/131] adapted to new convention also in cuda_filter and object_rasterizer --- .../cuda_filter.hpp | 28 ++- .../kinect_image_observation_model_gpu.hpp | 8 +- .../object_rasterizer.hpp | 22 +- .../cuda_filter.cu | 208 +++++++++--------- .../object_rasterizer.cpp | 46 ++-- 5 files changed, 158 insertions(+), 154 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp index 4d689d0..c09e702 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp @@ -7,6 +7,10 @@ #include +#include +#include +#include + namespace fil { class CudaFilter @@ -15,17 +19,17 @@ class CudaFilter CudaFilter(); ~CudaFilter(); - void Init(std::vector > com_models, float angle_sigma, float trans_sigma, + void init(std::vector > com_models, float angle_sigma, float trans_sigma, float p_visible_init, float c, float log_c, float p_visible_occluded, float tail_weight, float model_sigma, float sigma_factor, float max_depth, float exponential_rate); // filter functions - void Propagate(const float ¤t_time, std::vector > &states); - void PropagateMultiple(const float ¤t_time, std::vector > > &states); - void Compare(float observation_time, bool constant_occlusion, std::vector &log_likelihoods); - void CompareMultiple(bool update, std::vector &log_likelihoods); - void Resample(std::vector resampling_indices); - void ResampleMultiple(std::vector resampling_indices); + void propagate(const float ¤t_time, std::vector > &states); + void propagate_multiple(const float ¤t_time, std::vector > > &states); + void compare(float observation_time, bool constant_occlusion, std::vector &log_likelihoods); + void compare_multiple(bool update, std::vector &log_likelihoods); + void resample(std::vector resampling_indices); + void resample_multiple(std::vector resampling_indices); // setters @@ -44,7 +48,7 @@ class CudaFilter std::vector get_visibility_probabilities(int state_id); std::vector > get_visibility_probabilities(); - void MapTexture(); + void map_texture(); void destroy_context(); private: @@ -114,10 +118,10 @@ class CudaFilter void set_default_kernel_config(); // helper functions - template void allocate(T * &pointer, size_t size, char* name); - void coutArray(float* array, int size, char* name); - void coutArray(std::vector array, char* name); - void checkCUDAError(const char *msg); + template void allocate(T * &pointer, size_t size, std::string name); + void cout_array(float* array, int size, char* name); + void cout_array(std::vector array, char* name); + void check_cuda_error(const char *msg); }; diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 132570d..55c85bc 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -176,7 +176,7 @@ class KinectImageObservationModelGPU: float log_c = log(c); std::vector > dummy_com_models; - cuda_->Init(dummy_com_models, 0.0f, 0.0f, + cuda_->init(dummy_com_models, 0.0f, 0.0f, initial_visibility_prob_, c, log_c, p_visible_occluded_, tail_weight_, model_sigma_, sigma_factor_, max_depth_, exponential_rate_); @@ -250,7 +250,7 @@ class KinectImageObservationModelGPU: - opengl_->Render(poses); + opengl_->render(poses); MEASURE("gpu: rendering"); @@ -258,10 +258,10 @@ class KinectImageObservationModelGPU: cudaGraphicsMapResources(1, &combined_texture_resource_, 0); cudaGraphicsSubResourceGetMappedArray(&texture_array_, combined_texture_resource_, 0, 0); cuda_->set_texture_array(texture_array_); - cuda_->MapTexture(); + cuda_->map_texture(); MEASURE("gpu: mapping texture"); - cuda_->CompareMultiple(update_occlusions, flog_likelihoods); + cuda_->compare_multiple(update_occlusions, flog_likelihoods); cudaGraphicsUnmapResources(1, &combined_texture_resource_, 0); MEASURE("gpu: computing likelihoods"); diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp index b27bd57..afe5e7d 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp @@ -52,7 +52,7 @@ class ObjectRasterizer * @param[in,out] depth [pose_nr][0 - nr_relevant_pixels] = {depth_value}. This list should be empty when passed to the function. * Afterwards, it will contain the depth value of all pixels that were rendered to, per pose. */ - void Render(const std::vector > > states, + void render(const std::vector > > states, std::vector > &intersect_indices, std::vector > &depth); @@ -63,7 +63,7 @@ class ObjectRasterizer * @param[in] states [pose_nr][object_nr][0 - 6] = {qw, qx, qy, qz, tx, ty, tz}. This should contain the quaternion * and the translation for each object per pose. */ - void Render(const std::vector > > states); + void render(const std::vector > > states); /// sets the objects that should be rendered. /** This function only needs to be called if any objects initially passed in the constructor should be left out when rendering. @@ -182,22 +182,22 @@ class ObjectRasterizer // ====================== PRIVATE FUNCTIONS ====================== // - std::string getTextForEnum( int enumVal ); - void checkGLErrors(const char *label); - bool checkFramebufferStatus(); - void ReadDepth(std::vector > &intersect_indices, + std::string get_text_for_enum( int enumVal ); + void check_GL_errors(const char *label); + bool check_framebuffer_status(); + void read_depth(std::vector > &intersect_indices, std::vector > &depth, GLuint pixel_buffer_object, GLuint framebuffer_texture, render_type calling_function); - Eigen::Matrix4f GetModelMatrix(const std::vector state); + Eigen::Matrix4f get_model_matrix(const std::vector state); Eigen::Matrix4f GetProjectionMatrix(float n, float f, float l, float r, float t, float b); - void DisplayTimeObservations(std::map factors, render_type calling_function); - void ReallocateBuffers(); - void SetupViewMatrix(); - void SetupProjectionMatrix(const Eigen::Matrix3f camera_matrix); + void display_time_observations(std::map factors, render_type calling_function); + void reallocate_buffers(); + void setup_view_matrix(); + void setup_projection_matrix(const Eigen::Matrix3f camera_matrix); }; diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu index c698c03..9693556 100644 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu +++ b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu @@ -70,7 +70,7 @@ texture texture_reference; // ====================== MATRIX MANIPULATION FUNCTIONS ======================= // -__device__ void multiplyMatrices(float *A, float *B, float *C) { +__device__ void multiply_matrices(float *A, float *B, float *C) { float sum = 0; for (int i = 0; i < VECTOR_DIM; i++) { // iterate through rows for (int j = 0; j < VECTOR_DIM; j++) { // iterate through cols @@ -83,7 +83,7 @@ __device__ void multiplyMatrices(float *A, float *B, float *C) { } } -__device__ float3 multiplyMatrixWithVector(float* M, float3 v) { +__device__ float3 multiply_matrix_with_vector(float* M, float3 v) { float result[3]; float v_copy[3]; v_copy[0] = v.x; v_copy[1] = v.y; v_copy[2] = v.z; @@ -101,7 +101,7 @@ __device__ float3 multiplyMatrixWithVector(float* M, float3 v) { } /* axis is defined as follows: 0 = x, 1 = y, 2 = z */ -__device__ void createRotationMatrix(const float angle, const int axis, float *R) { +__device__ void create_rotation_matrix(const float angle, const int axis, float *R) { float cos_angle = cos(angle); float sin_angle = sin(angle); @@ -120,7 +120,7 @@ __device__ void createRotationMatrix(const float angle, const int axis, float *R } } -__device__ void transposeMatrix(float *A, float *T) { +__device__ void transpose_matrix(float *A, float *T) { T[0] = A[0]; T[1] = A[3]; T[2] = A[6]; @@ -157,14 +157,14 @@ __device__ float3 negate(const float3 &a) { // ======================= QUATERNION CONVERSIONS AND MANIPULATION FUNCTIONS ======================= // -__device__ void quaternionToMatrix(const float4 q_in, float *Q) { +__device__ void quaternion_to_matrix(const float4 q_in, float *Q) { float4 q = normalize(q_in); Q[0] = 1.0f - 2.0f*q.y*q.y - 2.0f*q.z*q.z; Q[1] = 2.0f*q.x*q.y - 2.0f*q.z*q.w; Q[2] = 2.0f*q.x*q.z + 2.0f*q.y*q.w; Q[3] = 2.0f*q.x*q.y + 2.0f*q.z*q.w; Q[4] = 1.0f - 2.0f*q.x*q.x - 2.0f*q.z*q.z; Q[5] = 2.0f*q.y*q.z - 2.0f*q.x*q.w; Q[6] = 2.0f*q.x*q.z - 2.0f*q.y*q.w; Q[7] = 2.0f*q.y*q.z + 2.0f*q.x*q.w; Q[8] = 1.0f - 2.0f*q.x*q.x - 2.0f*q.y*q.y; } -__device__ float4 matrixToQuaternion(float *Q) { +__device__ float4 matrix_to_quaternion(float *Q) { float4 q; q.w = sqrtf( fmaxf( 0, 1 + Q[0] + Q[4] + Q[8] ) ) / 2; @@ -184,7 +184,7 @@ __device__ float4 matrixToQuaternion(float *Q) { return q; } -__device__ float4 multiplyQuaternions(float4 q1, float4 q2) { +__device__ float4 multiply_quaternions(float4 q1, float4 q2) { float w = (q1.w * q2.w) - (q1.x * q2.x) - (q1.y * q2.y) - (q1.z * q2.z); float x = (q1.w * q2.x) + (q1.x * q2.w) + (q1.y * q2.z) - (q1.z * q2.y); float y = (q1.w * q2.y) - (q1.x * q2.z) + (q1.y * q2.w) + (q1.z * q2.x); @@ -197,7 +197,7 @@ __device__ float4 multiplyQuaternions(float4 q1, float4 q2) { // ======================= helper functions for compare (observation model) ======================= // -__device__ float propagateOcclusion(float initial_p_source, float time) { +__device__ float propagate_occlusion(float initial_p_source, float time) { if (isnan(time)) { return initial_p_source; } @@ -249,7 +249,7 @@ __device__ float prob(float observation, float prediction, bool visible) -__global__ void setupNumberGenerators(int current_time, curandStateMRG32k3a *mrg_state, int n_poses) +__global__ void setup_number_generators_kernel(int current_time, curandStateMRG32k3a *mrg_state, int n_poses) { int id = blockIdx.x * blockDim.x + threadIdx.x; if (id < n_poses) { @@ -259,7 +259,7 @@ __global__ void setupNumberGenerators(int current_time, curandStateMRG32k3a *mrg } -__global__ void propagate(float *states, int n_states, int states_size, float delta_time, curandStateMRG32k3a *mrg_state) +__global__ void propagate_kernel(float *states, int n_states, int states_size, float delta_time, curandStateMRG32k3a *mrg_state) { int id = blockIdx.x * blockDim.x + threadIdx.x; if (id < n_states) { @@ -306,23 +306,23 @@ __global__ void propagate(float *states, int n_states, int states_size, float de float3 t_rand = make_float3(trans_x, trans_y, trans_z); - createRotationMatrix(angle_x, 0, rot_matrix_x); - createRotationMatrix(angle_y, 1, rot_matrix_y); - createRotationMatrix(angle_z, 2, rot_matrix_z); + create_rotation_matrix(angle_x, 0, rot_matrix_x); + create_rotation_matrix(angle_y, 1, rot_matrix_y); + create_rotation_matrix(angle_z, 2, rot_matrix_z); - multiplyMatrices(rot_matrix_y, rot_matrix_z, tmp_matrix); - multiplyMatrices(rot_matrix_x, tmp_matrix, q_rand_matrix); + multiply_matrices(rot_matrix_y, rot_matrix_z, tmp_matrix); + multiply_matrices(rot_matrix_x, tmp_matrix, q_rand_matrix); - float4 q_rand_vector = matrixToQuaternion(q_rand_matrix); + float4 q_rand_vector = matrix_to_quaternion(q_rand_matrix); - quaternionToMatrix(q_init_vector, q_init_matrix); + quaternion_to_matrix(q_init_vector, q_init_matrix); - float3 t = negate(multiplyMatrixWithVector(q_init_matrix, multiplyMatrixWithVector(q_rand_matrix, local_rot_center))) - + multiplyMatrixWithVector(q_init_matrix, local_rot_center) + float3 t = negate(multiply_matrix_with_vector(q_init_matrix, multiply_matrix_with_vector(q_rand_matrix, local_rot_center))) + + multiply_matrix_with_vector(q_init_matrix, local_rot_center) + t_init + t_rand; - float4 q = multiplyQuaternions(q_init_vector, q_rand_vector); + float4 q = multiply_quaternions(q_init_vector, q_rand_vector); q = normalize(q); /* write state back into global memory */ @@ -346,7 +346,7 @@ __global__ void propagate(float *states, int n_states, int states_size, float de -__global__ void propagate_multiple(float *states, int n_states, int n_objects, int states_size, float delta_time, curandStateMRG32k3a *mrg_state) +__global__ void propagate_multiple_kernel(float *states, int n_states, int n_objects, int states_size, float delta_time, curandStateMRG32k3a *mrg_state) { int id = blockIdx.x * blockDim.x + threadIdx.x; if (id < n_states) { @@ -394,23 +394,23 @@ __global__ void propagate_multiple(float *states, int n_states, int n_objects, i float3 t_rand = make_float3(trans_x, trans_y, trans_z); - createRotationMatrix(angle_x, 0, rot_matrix_x); - createRotationMatrix(angle_y, 1, rot_matrix_y); - createRotationMatrix(angle_z, 2, rot_matrix_z); + create_rotation_matrix(angle_x, 0, rot_matrix_x); + create_rotation_matrix(angle_y, 1, rot_matrix_y); + create_rotation_matrix(angle_z, 2, rot_matrix_z); - multiplyMatrices(rot_matrix_y, rot_matrix_z, tmp_matrix); - multiplyMatrices(rot_matrix_x, tmp_matrix, q_rand_matrix); + multiply_matrices(rot_matrix_y, rot_matrix_z, tmp_matrix); + multiply_matrices(rot_matrix_x, tmp_matrix, q_rand_matrix); - float4 q_rand_vector = matrixToQuaternion(q_rand_matrix); + float4 q_rand_vector = matrix_to_quaternion(q_rand_matrix); - quaternionToMatrix(q_init_vector, q_init_matrix); + quaternion_to_matrix(q_init_vector, q_init_matrix); - float3 t = negate(multiplyMatrixWithVector(q_init_matrix, multiplyMatrixWithVector(q_rand_matrix, local_rot_center))) - + multiplyMatrixWithVector(q_init_matrix, local_rot_center) + float3 t = negate(multiply_matrix_with_vector(q_init_matrix, multiply_matrix_with_vector(q_rand_matrix, local_rot_center))) + + multiply_matrix_with_vector(q_init_matrix, local_rot_center) + t_init + t_rand; - float4 q = multiplyQuaternions(q_init_vector, q_rand_vector); + float4 q = multiply_quaternions(q_init_vector, q_rand_vector); q = normalize(q); /* write state back into global memory */ @@ -440,7 +440,7 @@ __global__ void propagate_multiple(float *states, int n_states, int n_objects, i -__global__ void compare(float *observations, float* visibility_probs, int n_pixels_per_pose, +__global__ void compare_kernel(float *observations, float* visibility_probs, int n_pixels_per_pose, bool constant_occlusion, float *d_log_likelihoods, float delta_time, int n_poses, int n_rows, int n_cols) { int block_id = blockIdx.x + blockIdx.y * gridDim.x; if (block_id < n_poses) { @@ -478,7 +478,7 @@ __global__ void compare(float *observations, float* visibility_probs, int n_pixe // Could save some data transfer time, but will cost more execution time, since all // the threads in one warp will have to wait for the else-branch to finish if (!constant_occlusion) { - visibility_prob = propagateOcclusion(visibility_probs[global_index], delta_time); + visibility_prob = propagate_occlusion(visibility_probs[global_index], delta_time); visibility_probs[global_index] = visibility_prob; } // if (!constant_occlusion) { @@ -536,7 +536,7 @@ __global__ void compare(float *observations, float* visibility_probs, int n_pixe -__global__ void compare_multiple(float *observations, float* old_visibility_probs, float* new_visibility_probs, int* occlusion_image_indices, int nr_pixels, +__global__ void compare_multiple_kernel(float *observations, float* old_visibility_probs, float* new_visibility_probs, int* occlusion_image_indices, int nr_pixels, float *d_log_likelihoods, float delta_time, int n_poses, int n_rows, int n_cols, bool update, float* test_array) { int block_id = blockIdx.x + blockIdx.y * gridDim.x; if (block_id < n_poses) { @@ -596,7 +596,7 @@ __global__ void compare_multiple(float *observations, float* old_visibility_prob // Could save some data transfer time, but will cost more execution time, since all // the threads in one warp will have to wait for the else-branch to finish - visibility_prob = propagateOcclusion(visibility_probs[occlusion_pixel_index], delta_time); + visibility_prob = propagate_occlusion(visibility_probs[occlusion_pixel_index], delta_time); if (update) visibility_probs[occlusion_pixel_index] = visibility_prob; @@ -647,7 +647,7 @@ __global__ void compare_multiple(float *observations, float* old_visibility_prob -__global__ void resample(float *visibility_probs, +__global__ void resample_kernel(float *visibility_probs, float *visibility_probs_copy, float *states, float *states_copy, @@ -674,7 +674,7 @@ __global__ void resample(float *visibility_probs, } -__global__ void resample_multiple(float *visibility_probs, +__global__ void resample_multiple_kernel(float *visibility_probs, float *visibility_probs_copy, int *resampling_indices, int nr_pixels) { @@ -711,14 +711,14 @@ CudaFilter::CudaFilter() : props.major = 2; props.minor = 0; cudaChooseDevice( &device_number, &props ); - checkCUDAError("choosing device"); + check_cuda_error("choosing device"); /* tell CUDA which device we will be using for graphic interop * requires that the CUDA device be specified by * cudaGLSetGLDevice() before any other runtime calls. */ cudaGLSetGLDevice( device_number ); - checkCUDAError("cudaGLsetGLDevice"); + check_cuda_error("cudaGLsetGLDevice"); cudaGetDeviceProperties(&props, device_number); // we will run the program only on one graphics card, the first one we can find = 0 warp_size_ = props.warpSize; // equals 32 for all current graphics cards, but might change in the future @@ -747,7 +747,7 @@ CudaFilter::CudaFilter() : } -void CudaFilter::Init(vector > com_models, float angle_sigma, float trans_sigma, +void CudaFilter::init(vector > com_models, float angle_sigma, float trans_sigma, float p_visible_init, float c, float log_c, float p_visible_occluded, float tail_weight, float model_sigma, float sigma_factor, float max_depth, float exponential_rate) { @@ -768,12 +768,12 @@ void CudaFilter::Init(vector > com_models, float angle_sigma, floa cudaMemset(d_log_likelihoods_, 0, sizeof(float) * n_poses_); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemset d_log_likelihoods"); + check_cuda_error("cudaMemset d_log_likelihoods"); #endif cudaMemcpyToSymbol(g_sigmas, &local_sigmas, sizeof(float2), 0, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpyToSymbol local_sigmas -> sigmas"); + check_cuda_error("cudaMemcpyToSymbol local_sigmas -> sigmas"); #endif vector com_models_raw; @@ -783,57 +783,57 @@ void CudaFilter::Init(vector > com_models, float angle_sigma, floa cudaMemcpyToSymbol(g_rot_center, com_models_raw.data(), com_models_raw.size() * sizeof(float3), 0, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpyToSymbol com_model -> rot_center"); + check_cuda_error("cudaMemcpyToSymbol com_model -> rot_center"); #endif cudaMemcpyToSymbol(g_p_visible_init, &p_visible_init, sizeof(float), 0, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpyToSymbol p_visible_init -> g_p_visible_init"); + check_cuda_error("cudaMemcpyToSymbol p_visible_init -> g_p_visible_init"); #endif cudaMemcpyToSymbol(g_c, &c, sizeof(float), 0, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpyToSymbol c -> g_c"); + check_cuda_error("cudaMemcpyToSymbol c -> g_c"); #endif cudaMemcpyToSymbol(g_log_c, &log_c, sizeof(float), 0, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpyToSymbol log_c -> g_log_c"); + check_cuda_error("cudaMemcpyToSymbol log_c -> g_log_c"); #endif cudaMemcpyToSymbol(g_p_visible_occluded, &p_visible_occluded, sizeof(float), 0, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpyToSymbol p_visible_occluded -> g_p_visible_occluded"); + check_cuda_error("cudaMemcpyToSymbol p_visible_occluded -> g_p_visible_occluded"); #endif cudaMemcpyToSymbol(g_tail_weight, &tail_weight, sizeof(float), 0, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpyToSymbol tail_weight -> g_tail_weight"); + check_cuda_error("cudaMemcpyToSymbol tail_weight -> g_tail_weight"); #endif cudaMemcpyToSymbol(g_model_sigma, &model_sigma, sizeof(float), 0, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpyToSymbol model_sigma -> g_model_sigma"); + check_cuda_error("cudaMemcpyToSymbol model_sigma -> g_model_sigma"); #endif cudaMemcpyToSymbol(g_sigma_factor, &sigma_factor, sizeof(float), 0, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpyToSymbol sigma_factor -> g_sigma_factor"); + check_cuda_error("cudaMemcpyToSymbol sigma_factor -> g_sigma_factor"); #endif cudaMemcpyToSymbol(g_max_depth, &max_depth, sizeof(float), 0, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpyToSymbol max_depth -> g_max_depth"); + check_cuda_error("cudaMemcpyToSymbol max_depth -> g_max_depth"); #endif cudaMemcpyToSymbol(g_exponential_rate, &exponential_rate, sizeof(float), 0, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpyToSymbol exponential_rate -> g_exponential_rate"); + check_cuda_error("cudaMemcpyToSymbol exponential_rate -> g_exponential_rate"); #endif } -void CudaFilter::Propagate(const float ¤t_time, vector > &states) +void CudaFilter::propagate(const float ¤t_time, vector > &states) { @@ -841,9 +841,9 @@ void CudaFilter::Propagate(const float ¤t_time, vector > &st last_propagation_time_ = current_time; - propagate <<< n_blocks_, n_threads_ >>> (d_states_, n_poses_, n_features_, delta_time, d_mrg_states_); + propagate_kernel <<< n_blocks_, n_threads_ >>> (d_states_, n_poses_, n_features_, delta_time, d_mrg_states_); #ifdef CHECK_ERRORS - checkCUDAError("propagate kernel call"); + check_cuda_error("propagate kernel call"); #endif @@ -851,7 +851,7 @@ void CudaFilter::Propagate(const float ¤t_time, vector > &st // TODO necessary? Remove for performance? cudaDeviceSynchronize(); #ifdef CHECK_ERRORS - checkCUDAError("cudaDeviceSynchronize propagate"); + check_cuda_error("cudaDeviceSynchronize propagate"); #endif @@ -859,7 +859,7 @@ void CudaFilter::Propagate(const float ¤t_time, vector > &st float *states_raw = (float*) malloc(n_poses_ * n_features_ * sizeof(float)); cudaMemcpy(states_raw, d_states_, n_poses_ * n_features_ * sizeof(float), cudaMemcpyDeviceToHost); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy d_states -> states"); + check_cuda_error("cudaMemcpy d_states -> states"); #endif @@ -873,7 +873,7 @@ void CudaFilter::Propagate(const float ¤t_time, vector > &st -void CudaFilter::PropagateMultiple(const float ¤t_time, vector > > &states) +void CudaFilter::propagate_multiple(const float ¤t_time, vector > > &states) { float delta_time = current_time - last_propagation_time_; @@ -893,13 +893,13 @@ void CudaFilter::PropagateMultiple(const float ¤t_time, vector d_states"); + check_cuda_error("cudaMemcpy states -> d_states"); #endif - propagate_multiple <<< n_blocks_, n_threads_ >>> (d_states_, n_poses_, n_objects, n_features_, delta_time, d_mrg_states_); + propagate_multiple_kernel <<< n_blocks_, n_threads_ >>> (d_states_, n_poses_, n_objects, n_features_, delta_time, d_mrg_states_); #ifdef CHECK_ERRORS - checkCUDAError("propagate kernel call"); + check_cuda_error("propagate kernel call"); #endif @@ -907,14 +907,14 @@ void CudaFilter::PropagateMultiple(const float ¤t_time, vector states"); + check_cuda_error("cudaMemcpy d_states -> states"); #endif @@ -932,7 +932,7 @@ void CudaFilter::PropagateMultiple(const float ¤t_time, vector &log_likelihoods) { +void CudaFilter::compare(float observation_time, bool constant_occlusion, vector &log_likelihoods) { #ifdef PROFILING_ACTIVE cudaEvent_t start, stop; @@ -953,15 +953,15 @@ void CudaFilter::Compare(float observation_time, bool constant_occlusion, vector cudaEventRecord(start); #endif - compare <<< gridDim, 128 >>> (d_observations_, d_visibility_probs_, n_cols_ * n_rows_, + compare_kernel <<< gridDim, 128 >>> (d_observations_, d_visibility_probs_, n_cols_ * n_rows_, constant_occlusion, d_log_likelihoods_, delta_time, n_poses_, n_rows_, n_cols_); #ifdef CHECK_ERRORS - checkCUDAError("compare kernel call"); + check_cuda_error("compare kernel call"); #endif cudaDeviceSynchronize(); #ifdef CHECK_ERRORS - checkCUDAError("cudaDeviceSynchronize compare"); + check_cuda_error("cudaDeviceSynchronize compare"); #endif #ifdef PROFILING_ACTIVE @@ -978,12 +978,12 @@ void CudaFilter::Compare(float observation_time, bool constant_occlusion, vector cudaMemcpy(&log_likelihoods[0], d_log_likelihoods_, n_poses_ * sizeof(float), cudaMemcpyDeviceToHost); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy d_log_likelihoods -> log_likelihoods"); + check_cuda_error("cudaMemcpy d_log_likelihoods -> log_likelihoods"); #endif cudaDeviceSynchronize(); #ifdef CHECK_ERRORS - checkCUDAError("cudaDeviceSynchronize compare"); + check_cuda_error("cudaDeviceSynchronize compare"); #endif #ifdef PROFILING_ACTIVE @@ -999,7 +999,7 @@ void CudaFilter::Compare(float observation_time, bool constant_occlusion, vector -void CudaFilter::CompareMultiple(bool update, vector &log_likelihoods) { +void CudaFilter::compare_multiple(bool update, vector &log_likelihoods) { #ifdef PROFILING_ACTIVE cudaEvent_t start, stop; @@ -1027,20 +1027,20 @@ void CudaFilter::CompareMultiple(bool update, vector &log_likelihoods) { allocate(d_test_array_, TEST_SIZE * sizeof(float), "test_array"); cudaMemset(d_test_array_, 0, TEST_SIZE * sizeof(float)); - compare_multiple <<< gridDim, 128 >>> (d_observations_, d_visibility_probs_, d_visibility_probs_copy_, d_prev_sample_indices_, n_cols_ * n_rows_, + compare_multiple_kernel <<< gridDim, 128 >>> (d_observations_, d_visibility_probs_, d_visibility_probs_copy_, d_prev_sample_indices_, n_cols_ * n_rows_, d_log_likelihoods_, delta_time, n_poses_, n_rows_, n_cols_, update, d_test_array_); #ifdef CHECK_ERRORS - checkCUDAError("compare kernel call"); + check_cuda_error("compare kernel call"); #endif cudaDeviceSynchronize(); #ifdef CHECK_ERRORS - checkCUDAError("cudaDeviceSynchronize compare_multiple"); + check_cuda_error("cudaDeviceSynchronize compare_multiple"); #endif cudaMemcpy(test_array, d_test_array_, TEST_SIZE * sizeof(float), cudaMemcpyDeviceToHost); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy d_log_likelihoods -> log_likelihoods"); + check_cuda_error("cudaMemcpy d_log_likelihoods -> log_likelihoods"); #endif // for (int i = 0; i < TEST_SIZE; i++) { @@ -1072,12 +1072,12 @@ void CudaFilter::CompareMultiple(bool update, vector &log_likelihoods) { cudaMemcpy(&log_likelihoods[0], d_log_likelihoods_, n_poses_ * sizeof(float), cudaMemcpyDeviceToHost); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy d_log_likelihoods -> log_likelihoods"); + check_cuda_error("cudaMemcpy d_log_likelihoods -> log_likelihoods"); #endif cudaDeviceSynchronize(); #ifdef CHECK_ERRORS - checkCUDAError("cudaDeviceSynchronize compare"); + check_cuda_error("cudaDeviceSynchronize compare"); #endif #ifdef PROFILING_ACTIVE @@ -1097,13 +1097,13 @@ void CudaFilter::CompareMultiple(bool update, vector &log_likelihoods) { -void CudaFilter::Resample(vector resampling_indices) { +void CudaFilter::resample(vector resampling_indices) { // cout << "resample <<< " << n_poses_ << ", " << 128 << " >>>" << endl; cudaMemcpy(d_resampling_indices_, &resampling_indices[0], sizeof(int) * n_poses_, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy resampling_indices -> d_resampling_indices_"); + check_cuda_error("cudaMemcpy resampling_indices -> d_resampling_indices_"); #endif // int min = 100; @@ -1118,16 +1118,16 @@ void CudaFilter::Resample(vector resampling_indices) { int nr_pixels = n_rows_ * n_cols_; - resample <<< n_poses_, 128 >>> (d_visibility_probs_, d_visibility_probs_copy_, + resample_kernel <<< n_poses_, 128 >>> (d_visibility_probs_, d_visibility_probs_copy_, d_states_, d_states_copy_, d_resampling_indices_, nr_pixels, n_features_); #ifdef CHECK_ERRORS - checkCUDAError("resample kernel call"); + check_cuda_error("resample kernel call"); #endif cudaDeviceSynchronize(); #ifdef CHECK_ERRORS - checkCUDAError("cudaDeviceSynchronize resample"); + check_cuda_error("cudaDeviceSynchronize resample"); #endif @@ -1146,24 +1146,24 @@ void CudaFilter::Resample(vector resampling_indices) { -void CudaFilter::ResampleMultiple(vector resampling_indices) { +void CudaFilter::resample_multiple(vector resampling_indices) { cudaMemcpy(d_resampling_indices_, &resampling_indices[0], sizeof(int) * n_poses_, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy resampling_indices -> d_resampling_indices_"); + check_cuda_error("cudaMemcpy resampling_indices -> d_resampling_indices_"); #endif int nr_pixels = n_rows_ * n_cols_; - resample_multiple <<< n_poses_, 128 >>> (d_visibility_probs_, d_visibility_probs_copy_, + resample_multiple_kernel <<< n_poses_, 128 >>> (d_visibility_probs_, d_visibility_probs_copy_, d_resampling_indices_, nr_pixels); #ifdef CHECK_ERRORS - checkCUDAError("resample kernel call"); + check_cuda_error("resample kernel call"); #endif cudaDeviceSynchronize(); #ifdef CHECK_ERRORS - checkCUDAError("cudaDeviceSynchronize resample"); + check_cuda_error("cudaDeviceSynchronize resample"); #endif @@ -1205,7 +1205,7 @@ void CudaFilter::set_states(std::vector > &states, int seed) cudaMemcpy(d_states_, states_raw, states_size, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy states_raw -> d_states_"); + check_cuda_error("cudaMemcpy states_raw -> d_states_"); #endif free(states_raw); @@ -1213,7 +1213,7 @@ void CudaFilter::set_states(std::vector > &states, int seed) // setup random number generators for each thread to be used in the propagate kernel allocate(d_mrg_states_, n_poses_ * sizeof(curandStateMRG32k3a), "d_mrg_states"); - setupNumberGenerators <<< n_blocks_, n_threads_ >>> (seed, d_mrg_states_, n_poses_); + setup_number_generators_kernel <<< n_blocks_, n_threads_ >>> (seed, d_mrg_states_, n_poses_); cudaDeviceSynchronize(); } else { @@ -1239,7 +1239,7 @@ void CudaFilter::set_states_multiple(int n_objects, int n_features, int seed) // setup random number generators for each thread to be used in the propagate kernel allocate(d_mrg_states_, n_poses_ * sizeof(curandStateMRG32k3a), "d_mrg_states"); - setupNumberGenerators <<< n_blocks_, n_threads_ >>> (seed, d_mrg_states_, n_poses_); + setup_number_generators_kernel <<< n_blocks_, n_threads_ >>> (seed, d_mrg_states_, n_poses_); cudaDeviceSynchronize(); } else { @@ -1265,7 +1265,7 @@ void CudaFilter::set_observations(const float* observations, const float observa void CudaFilter::set_observations(const float* observations) { cudaMemcpy(d_observations_, observations, n_cols_ * n_rows_ * sizeof(float), cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy observations -> d_observations_"); + check_cuda_error("cudaMemcpy observations -> d_observations_"); #endif cudaDeviceSynchronize(); } @@ -1275,7 +1275,7 @@ void CudaFilter::set_prev_sample_indices(const int* prev_sample_indices) { cudaMemcpy(d_prev_sample_indices_, prev_sample_indices, n_poses_ * sizeof(int), cudaMemcpyHostToDevice); // cout << "when setting prev_sample_indices: n_poses: " << n_poses_ << ", max poses: " << n_max_poses_ << endl; #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy prev_sample_indices -> d_prev_sample_indices_"); + check_cuda_error("cudaMemcpy prev_sample_indices -> d_prev_sample_indices_"); #endif cudaDeviceSynchronize(); } @@ -1295,7 +1295,7 @@ void CudaFilter::set_resolution(int n_rows, int n_cols) { cudaMemcpy(d_visibility_probs_, &initial_visibility_probs[0], n_rows_ * n_cols_ * n_max_poses_ * sizeof(float), cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy visibility_prob_default_ -> d_visibility_probs_"); + check_cuda_error("cudaMemcpy visibility_prob_default_ -> d_visibility_probs_"); #endif cudaDeviceSynchronize(); @@ -1305,7 +1305,7 @@ void CudaFilter::set_resolution(int n_rows, int n_cols) { void CudaFilter::set_visibility_probabilities(const float* visibility_probabilities) { cudaMemcpy(d_visibility_probs_, visibility_probabilities, n_rows_ * n_cols_ * n_poses_ * sizeof(float), cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy visibility_probabilities -> d_visibility_probs_"); + check_cuda_error("cudaMemcpy visibility_probabilities -> d_visibility_probs_"); #endif } @@ -1339,13 +1339,13 @@ void CudaFilter::set_number_of_max_poses(int n_poses, int n_poses_x) { vector initial_visibility_probs (n_rows_ * n_cols_ * n_max_poses_, visibility_prob_default_); cudaMemcpy(d_visibility_probs_, &initial_visibility_probs[0], n_rows_ * n_cols_ * n_max_poses_ * sizeof(float), cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy visibility_prob_default_ -> d_visibility_probs_"); + check_cuda_error("cudaMemcpy visibility_prob_default_ -> d_visibility_probs_"); #endif cudaDeviceSynchronize(); #ifdef CHECK_ERRORS - checkCUDAError("cudaDeviceSynchronize set_number_of_states"); + check_cuda_error("cudaDeviceSynchronize set_number_of_states"); #endif @@ -1408,7 +1408,7 @@ vector CudaFilter::get_visibility_probabilities(int state_id) { int offset = state_id * n_rows_ * n_cols_; cudaMemcpy(visibility_probabilities, d_visibility_probs_ + offset, n_rows_ * n_cols_ * sizeof(float), cudaMemcpyDeviceToHost); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy d_visibility_probabilities -> visibility_probabilities"); + check_cuda_error("cudaMemcpy d_visibility_probabilities -> visibility_probabilities"); #endif vector visibility_probabilities_vector; for (int i = 0; i < n_rows_ * n_cols_; i++) { @@ -1424,7 +1424,7 @@ vector > CudaFilter::get_visibility_probabilities() { float* visibility_probabilities = (float*) malloc(n_poses_ * n_rows_ * n_cols_ * sizeof(float)); cudaMemcpy(visibility_probabilities, d_visibility_probs_, n_poses_ * n_rows_ * n_cols_ * sizeof(float), cudaMemcpyDeviceToHost); #ifdef CHECK_ERRORS - checkCUDAError("cudaMemcpy d_visibility_probabilities -> visibility_probabilities"); + check_cuda_error("cudaMemcpy d_visibility_probabilities -> visibility_probabilities"); #endif vector > visibility_probabilities_vector; vector tmp_vector (n_rows_ * n_cols_); @@ -1446,7 +1446,7 @@ vector > CudaFilter::get_visibility_probabilities() { -template void CudaFilter::allocate(T * &pointer, size_t size, char* name) { +template void CudaFilter::allocate(T * &pointer, size_t size, string name) { #ifdef CHECK_ERRORS size_t free_space_before, free_space_after, total_space; cuMemGetInfo(&free_space_before, &total_space); @@ -1464,25 +1464,25 @@ template void CudaFilter::allocate(T * &pointer, size_t size, char* } else { // cout << "memory reallocated for " << name << ": " << size / 1e6 << "MB, free space left: " << free_space_after / 1e6 << " MB" << endl; } - checkCUDAError("cudaMalloc failed"); + check_cuda_error("cudaMalloc failed"); #endif } -void CudaFilter::MapTexture() { +void CudaFilter::map_texture() { cudaBindTextureToArray(texture_reference, d_texture_array_); - checkCUDAError("cudaBindTextureToArray"); + check_cuda_error("cudaBindTextureToArray"); } -void CudaFilter::coutArray(float* array, int size, char* name) { +void CudaFilter::cout_array(float* array, int size, char* name) { for (size_t i = 0; i < size; i++) { cout << name << "[" << i << "]: " << array[i] << endl; } } -void CudaFilter::coutArray(vector array, char* name) { +void CudaFilter::cout_array(vector array, char* name) { for (size_t i = 0; i < array.size(); i++) { cout << name << "[" << i << "]: " << array[i] << endl; } @@ -1490,7 +1490,7 @@ void CudaFilter::coutArray(vector array, char* name) { -void CudaFilter::checkCUDAError(const char *msg) +void CudaFilter::check_cuda_error(const char *msg) { cudaError_t err = cudaGetLastError(); if( cudaSuccess != err) diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp index 95fc250..5e8834a 100644 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp +++ b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp @@ -115,7 +115,7 @@ ObjectRasterizer::ObjectRasterizer(const std::vector > > states, +void ObjectRasterizer::render(const std::vector > > states, std::vector > &intersect_indices, std::vector > &depth) { - Render(states); + render(states); get_depth_values(intersect_indices, depth); } -void ObjectRasterizer::Render(const std::vector > > states) { +void ObjectRasterizer::render(const std::vector > > states) { #ifdef PROFILING_ACTIVE combined_fast_set_viewport_queries_.resize(n_poses_); @@ -420,7 +420,7 @@ void ObjectRasterizer::Render(const std::vector > calls_aggregate_[COMBINED_FAST][SEND_MODEL_MATRIX]++; #endif - model_view_matrix = view_matrix_ * GetModelMatrix(states[n_poses_x_ * i + j][index]); + model_view_matrix = view_matrix_ * get_model_matrix(states[n_poses_x_ * i + j][index]); glUniformMatrix4fv(model_view_matrix_ID_, 1, GL_FALSE, model_view_matrix.data()); #ifdef PROFILING_ACTIVE @@ -471,7 +471,7 @@ void ObjectRasterizer::Render(const std::vector > start_time_ = sf::hf::get_wall_time(); calls_aggregate_[COMBINED_FAST][SEND_MODEL_MATRIX]++; #endif - model_view_matrix = view_matrix_ * GetModelMatrix(states[n_poses_x_ * (n_poses_y_ - 1) + j][index]); + model_view_matrix = view_matrix_ * get_model_matrix(states[n_poses_x_ * (n_poses_y_ - 1) + j][index]); glUniformMatrix4fv(model_view_matrix_ID_, 1, GL_FALSE, model_view_matrix.data()); #ifdef PROFILING_ACTIVE @@ -541,7 +541,7 @@ void ObjectRasterizer::Render(const std::vector > -void ObjectRasterizer::ReadDepth(vector > &intersect_indices, +void ObjectRasterizer::read_depth(vector > &intersect_indices, vector > &depth, GLuint pixel_buffer_object, GLuint framebuffer_texture, @@ -664,7 +664,7 @@ void ObjectRasterizer::set_number_of_max_poses(int n_poses) { n_poses_x_ = n_max_poses_x_; n_poses_y_ = n_max_poses_y_; - ReallocateBuffers(); + reallocate_buffers(); // } } @@ -706,7 +706,7 @@ void ObjectRasterizer::set_resolution(const int n_rows, // } } -void ObjectRasterizer::ReallocateBuffers() { +void ObjectRasterizer::reallocate_buffers() { // ======================= REALLOCATE PBO ======================= // @@ -755,7 +755,7 @@ void ObjectRasterizer::ReallocateBuffers() { 0); - checkFramebufferStatus(); + check_framebuffer_status(); GLenum color_buffers[] = {GL_COLOR_ATTACHMENT0}; glDrawBuffers(1, color_buffers); } @@ -765,7 +765,7 @@ void ObjectRasterizer::ReallocateBuffers() { -Matrix4f ObjectRasterizer::GetModelMatrix(const vector state) { +Matrix4f ObjectRasterizer::get_model_matrix(const vector state) { // Model matrix equals the state of the object. Position and Quaternion just have to be expressed as a matrix. // note: state = (q.w, q.x, q.y, q.z, v.x, v.y, v.z) Matrix4f model_matrix = Matrix4f::Identity(); @@ -793,7 +793,7 @@ Matrix4f ObjectRasterizer::GetProjectionMatrix(float n, float f, float l, float return projection_matrix; } -void ObjectRasterizer::SetupProjectionMatrix(const Eigen::Matrix3f camera_matrix) { +void ObjectRasterizer::setup_projection_matrix(const Eigen::Matrix3f camera_matrix) { // ==================== CONFIGURE IMAGE PARAMETERS ==================== // @@ -816,7 +816,7 @@ void ObjectRasterizer::SetupProjectionMatrix(const Eigen::Matrix3f camera_matrix } -void ObjectRasterizer::SetupViewMatrix() { +void ObjectRasterizer::setup_view_matrix() { // =========================== VIEW MATRIX =========================== // // OpenGL camera is rotated 180 degrees around the X-Axis compared to the Kinect camera @@ -826,7 +826,7 @@ void ObjectRasterizer::SetupViewMatrix() { view_matrix_ = view_matrix_transform.matrix(); } -void ObjectRasterizer::DisplayTimeObservations(map factors, render_type calling_function) { +void ObjectRasterizer::display_time_observations(map factors, render_type calling_function) { #ifdef PROFILING_ACTIVE @@ -973,7 +973,7 @@ void ObjectRasterizer::get_depth_values(std::vector > &intersec // ===================== TRANSFERS DEPTH VALUES TO CPU == SLOW!!! ================ // - ReadDepth(intersect_indices, depth, combined_result_buffer_, combined_texture_, COMBINED_FAST); + read_depth(intersect_indices, depth, combined_result_buffer_, combined_texture_, COMBINED_FAST); // vector intersect_indices_all; // vector depth_all; @@ -1027,12 +1027,12 @@ void ObjectRasterizer::get_depth_values(std::vector > &intersec } -string ObjectRasterizer::getTextForEnum( int enumVal ) { +string ObjectRasterizer::get_text_for_enum( int enumVal ) { return enum_strings_[enumVal]; } -void ObjectRasterizer::checkGLErrors(const char *label) { +void ObjectRasterizer::check_GL_errors(const char *label) { GLenum errCode; const GLubyte *errStr; if ((errCode = glGetError()) != GL_NO_ERROR) { @@ -1045,7 +1045,7 @@ void ObjectRasterizer::checkGLErrors(const char *label) { } } -bool ObjectRasterizer::checkFramebufferStatus() { +bool ObjectRasterizer::check_framebuffer_status() { GLenum status; status=(GLenum)glCheckFramebufferStatusEXT(GL_FRAMEBUFFER_EXT); switch(status) { From da06c1568ce37a4b985c47982089b4d380196d83 Mon Sep 17 00:00:00 2001 From: Claudia Pfreundt Date: Thu, 27 Aug 2015 19:04:13 +0200 Subject: [PATCH 040/131] marked stuff I have to change next time --- .../cuda_filter.hpp | 32 +++++++++---------- .../kinect_image_observation_model_gpu.hpp | 6 ---- .../cuda_filter.cu | 12 ------- 3 files changed, 15 insertions(+), 35 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp index c09e702..e0f9d4a 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp @@ -24,19 +24,19 @@ class CudaFilter float tail_weight, float model_sigma, float sigma_factor, float max_depth, float exponential_rate); // filter functions - void propagate(const float ¤t_time, std::vector > &states); - void propagate_multiple(const float ¤t_time, std::vector > > &states); - void compare(float observation_time, bool constant_occlusion, std::vector &log_likelihoods); + void propagate(const float ¤t_time, std::vector > &states); // not used + void propagate_multiple(const float ¤t_time, std::vector > > &states); // not used + void compare(float observation_time, bool constant_occlusion, std::vector &log_likelihoods); // not used void compare_multiple(bool update, std::vector &log_likelihoods); - void resample(std::vector resampling_indices); - void resample_multiple(std::vector resampling_indices); + void resample(std::vector resampling_indices); // not used + void resample_multiple(std::vector resampling_indices); // not used // setters - void set_states(std::vector > &states, int seed); - void set_states_multiple(int n_objects, int n_features, int seed); + void set_states(std::vector > &states, int seed); // not needed if propagation not on GPU + void set_states_multiple(int n_objects, int n_features, int seed); // not needed if propagation not on GPU void set_observations(const float* observations, const float observation_time); - void set_observations(const float* observations); + void set_observations(const float* observations); // not used outside, can be integrated into above void set_visibility_probabilities(const float* visibility_probabilities); void set_prev_sample_indices(const int* prev_sample_indices); void set_resolution(int n_rows, int n_cols); @@ -46,7 +46,7 @@ class CudaFilter // getters std::vector get_visibility_probabilities(int state_id); - std::vector > get_visibility_probabilities(); + std::vector > get_visibility_probabilities(); // returns all of them. Ask Manuel if they could need that. void map_texture(); void destroy_context(); @@ -63,15 +63,15 @@ class CudaFilter double copy_likelihoods_time_; // device pointers to arrays stored in global memory on the GPU - float *d_states_; - float *d_states_copy_; + float *d_states_; // not needed if propagation not on GPU + float *d_states_copy_; // not needed if propagation not on GPU float *d_visibility_probs_; float *d_visibility_probs_copy_; float *d_observations_; float *d_log_likelihoods_; int *d_prev_sample_indices_; - int *d_resampling_indices_; - float *d_test_array_; + int *d_resampling_indices_; // not needed if resampling not on GPU + float *d_test_array_; // debugging. can go. curandStateMRG32k3a *d_mrg_states_; // for OpenGL interop @@ -108,8 +108,8 @@ class CudaFilter float occlusion_time_; float observation_time_; -// float delta_time_; - float last_propagation_time_; + // float delta_time_; + float last_propagation_time_; // not needed if propagation not on GPU // booleans to describe the state of the cuda filter, to avoid wrong usage of the class bool n_poses_set_; @@ -119,8 +119,6 @@ class CudaFilter // helper functions template void allocate(T * &pointer, size_t size, std::string name); - void cout_array(float* array, int size, char* name); - void cout_array(std::vector array, char* name); void check_cuda_error(const char *msg); }; diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 55c85bc..2a9e507 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -42,8 +42,6 @@ struct Traits > typedef RBObservationModel Base; typedef typename Eigen::Matrix CameraMatrix; - -// typedef sf::RigidBodySystem<-1> State; }; } @@ -152,10 +150,6 @@ class KinectImageObservationModelGPU: cuda_ = boost::shared_ptr (new fil::CudaFilter()); - - //opengl_->PrepareRender(camera_matrix_.cast()); - - opengl_->set_number_of_max_poses(max_sample_count_); n_poses_x_ = opengl_->get_n_poses_x(); cuda_->set_number_of_max_poses(max_sample_count_, n_poses_x_); diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu index 9693556..7194f15 100644 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu +++ b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu @@ -1476,18 +1476,6 @@ void CudaFilter::map_texture() { } -void CudaFilter::cout_array(float* array, int size, char* name) { - for (size_t i = 0; i < size; i++) { - cout << name << "[" << i << "]: " << array[i] << endl; - } -} - -void CudaFilter::cout_array(vector array, char* name) { - for (size_t i = 0; i < array.size(); i++) { - cout << name << "[" << i << "]: " << array[i] << endl; - } -} - void CudaFilter::check_cuda_error(const char *msg) From 4a4cbfca410cfb2e9f0d2861e9a9e295b80caed8 Mon Sep 17 00:00:00 2001 From: manuel Date: Mon, 31 Aug 2015 19:27:09 +0200 Subject: [PATCH 041/131] now using process model like state transition function inside of filter --- .../brownian_object_motion_model.hpp | 41 +++---------------- .../damped_wiener_process_model.hpp | 13 +++++- ...o_blackwell_coordinate_particle_filter.hpp | 10 ++--- 3 files changed, 21 insertions(+), 43 deletions(-) diff --git a/include/dbot/models/process_models/brownian_object_motion_model.hpp b/include/dbot/models/process_models/brownian_object_motion_model.hpp index 4e0367d..3204d0e 100644 --- a/include/dbot/models/process_models/brownian_object_motion_model.hpp +++ b/include/dbot/models/process_models/brownian_object_motion_model.hpp @@ -168,8 +168,10 @@ class BrownianObjectMotionModel virtual State state(const State& prev_state, const Noise& noise, - const Input& input) const + const Input& input) { + Condition(prev_state, input); + return MapStandardGaussian(noise); } @@ -178,39 +180,6 @@ class BrownianObjectMotionModel -// virtual void Condition(const Scalar& delta_time, -// const State& state, -// const Input& control) -// { -// state_ = state; -// for(size_t i = 0; i < state_.count(); i++) -// { -// quaternion_map_[i] = ff::hf::QuaternionMatrix(state_.component(i).orientation().quaternion().coeffs()); - -// // transform the state, which is the pose and velocity with respect to to the origin, -// // into internal representation, which is the position and velocity of the center -// // and the orientation and angular velocity around the center -// state_.component(i).position() += state_.component(i).orientation().rotation_matrix()*rotation_center_[i]; -// state_.component(i).linear_velocity() += state_.component(i).angular_velocity().cross(state_.component(i).position()); - -// Eigen::Matrix linear_state; -// linear_state.topRows(3) = Eigen::Vector3d::Zero(); -// linear_state.bottomRows(3) = state_.component(i).linear_velocity(); -// linear_process_[i].Condition(delta_time, -// linear_state, -// control.template middleRows<3>(i*DIMENSION_PER_OBJECT)); - -// Eigen::Matrix angular_state; -// angular_state.topRows(3) = Eigen::Vector3d::Zero(); -// angular_state.bottomRows(3) = state_.component(i).angular_velocity(); -// angular_process_[i].Condition(delta_time, -// angular_state, -// control.template middleRows<3>(i*DIMENSION_PER_OBJECT + 3)); -// } -// } - - - virtual void Condition(const State& state, const Input& control) @@ -254,11 +223,11 @@ class BrownianObjectMotionModel virtual unsigned InputDimension() const { - return this->NoiseDimension(); + return this->noise_dimension(); } - virtual unsigned NoiseDimension() const + virtual unsigned noise_dimension() const { return state_.count() * DIMENSION_PER_OBJECT; } diff --git a/include/dbot/models/process_models/damped_wiener_process_model.hpp b/include/dbot/models/process_models/damped_wiener_process_model.hpp index 781b42d..aea8191 100644 --- a/include/dbot/models/process_models/damped_wiener_process_model.hpp +++ b/include/dbot/models/process_models/damped_wiener_process_model.hpp @@ -131,6 +131,15 @@ class DampedWienerProcessModel gaussian_.diagonal_covariance(Covariance(delta_time_)); } + virtual State state(const State& prev_state, + const Noise& noise, + const Input& input) + { + Condition(prev_state, input); + return MapStandardGaussian(noise); + + } + // virtual void Condition(const Scalar& delta_time, // const State& state, @@ -150,11 +159,11 @@ class DampedWienerProcessModel virtual unsigned Dimension() const { - return NoiseDimension(); // all dimensions are the same + return noise_dimension(); // all dimensions are the same } - virtual int NoiseDimension() const + virtual int noise_dimension() const { return gaussian_.dimension(); } diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index d3e4782..6d61676 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -86,11 +86,11 @@ class RBCoordinateParticleFilter { dimension += sampling_blocks_[i].size(); } - if(dimension != process_model_->NoiseDimension()) + if(dimension != process_model_->noise_dimension()) { std::cout << "the dimension of the sampling blocks is " << dimension << " while the dimension of the noise is " - << process_model_->NoiseDimension() << std::endl; + << process_model_->noise_dimension() << std::endl; exit(-1); } } @@ -104,7 +104,7 @@ class RBCoordinateParticleFilter loglikes_ = RealArray::Zero(belief_.size()); noises_ = std::vector(belief_.size(), - Noise::Zero(process_model_->NoiseDimension())); + Noise::Zero(process_model_->noise_dimension())); old_particles_ = belief_.locations(); for(size_t i_block = 0; i_block < sampling_blocks_.size(); i_block++) @@ -122,9 +122,9 @@ class RBCoordinateParticleFilter // propagate using partial noise ----------------------------------- for(size_t i_sampl = 0; i_sampl < belief_.size(); i_sampl++) { - process_model_->Condition(old_particles_[i_sampl], input); belief_.location(i_sampl) = - process_model_->MapStandardGaussian(noises_[i_sampl]); + process_model_->state(old_particles_[i_sampl], + noises_[i_sampl], input); } // compute likelihood ---------------------------------------------- From c36d73196290c81862389879b380031336c0b876 Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 1 Sep 2015 20:39:53 +0200 Subject: [PATCH 042/131] a few adaptations for compatibility with new process model --- .../brownian_object_motion_model.hpp | 35 ++++++++++++------- .../damped_wiener_process_model.hpp | 6 ++-- ...o_blackwell_coordinate_particle_filter.hpp | 6 ++-- 3 files changed, 29 insertions(+), 18 deletions(-) diff --git a/include/dbot/models/process_models/brownian_object_motion_model.hpp b/include/dbot/models/process_models/brownian_object_motion_model.hpp index 3204d0e..14b55cf 100644 --- a/include/dbot/models/process_models/brownian_object_motion_model.hpp +++ b/include/dbot/models/process_models/brownian_object_motion_model.hpp @@ -55,6 +55,8 @@ #include #include +#include + namespace ff { @@ -80,7 +82,9 @@ struct Traits > typedef State_ State; typedef typename State::Scalar Scalar; typedef Eigen::Matrix Input; - typedef Input Noise; + + // todo: this is a hack!! + typedef State_ Noise; typedef Eigen::Quaternion Quaternion; typedef Eigen::Matrix ObjectState; @@ -95,7 +99,12 @@ struct Traits > * \ingroup process_models */ template -class BrownianObjectMotionModel +class BrownianObjectMotionModel: + public fl::StateTransitionFunction< + State_, + typename internal::Traits >::Noise, + typename internal::Traits >::Input> + { public: typedef internal::Traits > Traits; @@ -168,7 +177,7 @@ class BrownianObjectMotionModel virtual State state(const State& prev_state, const Noise& noise, - const Input& input) + const Input& input) const { Condition(prev_state, input); return MapStandardGaussian(noise); @@ -182,7 +191,7 @@ class BrownianObjectMotionModel virtual void Condition(const State& state, - const Input& control) + const Input& control) const { state_ = state; for(size_t i = 0; i < state_.count(); i++) @@ -221,18 +230,18 @@ class BrownianObjectMotionModel angular_process_[object_index].Parameters(damping, angular_acceleration_covariance); } - virtual unsigned InputDimension() const + virtual int input_dimension() const { return this->noise_dimension(); } - virtual unsigned noise_dimension() const + virtual int noise_dimension() const { return state_.count() * DIMENSION_PER_OBJECT; } - virtual size_t Dimension() + virtual int state_dimension() const { return state_.rows(); } @@ -240,17 +249,17 @@ class BrownianObjectMotionModel private: // conditionals - State state_; - std::vector > quaternion_map_; + mutable State state_; + mutable std::vector > quaternion_map_; // parameters - std::vector > rotation_center_; + mutable std::vector > rotation_center_; // processes - std::vector linear_process_; - std::vector angular_process_; + mutable std::vector linear_process_; + mutable std::vector angular_process_; - double delta_time_; + mutable double delta_time_; }; } diff --git a/include/dbot/models/process_models/damped_wiener_process_model.hpp b/include/dbot/models/process_models/damped_wiener_process_model.hpp index aea8191..55e002a 100644 --- a/include/dbot/models/process_models/damped_wiener_process_model.hpp +++ b/include/dbot/models/process_models/damped_wiener_process_model.hpp @@ -88,15 +88,17 @@ struct Traits > * \ingroup distributions * \ingroup process_models */ -template +template class DampedWienerProcessModel // :public internal::Traits >::ProcessModelBase // ,public internal::Traits >::GaussianMapBase { public: - typedef internal::Traits > Traits; + typedef internal::Traits > Traits; typedef typename Traits::Scalar Scalar; + typedef typename Traits::State State; + typedef typename Traits::SecondMoment SecondMoment; typedef typename Traits::Input Input; typedef typename Traits::Noise Noise; diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index 6d61676..70dd73e 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -54,9 +54,9 @@ template class RBCoordinateParticleFilter { public: - typedef typename internal::Traits::State State; - typedef typename internal::Traits::Input Input; - typedef typename internal::Traits::Noise Noise; + typedef typename ProcessModel::State State; + typedef typename ProcessModel::Input Input; + typedef typename ProcessModel::Noise Noise; typedef Eigen::Array StateArray; typedef Eigen::Array RealArray; From 578bd6f1e289c44ba3aeb6feb9b20e564a97f284 Mon Sep 17 00:00:00 2001 From: Claudia Pfreundt Date: Thu, 3 Sep 2015 16:47:35 +0200 Subject: [PATCH 043/131] cleaned up rasterizer time measuring code and refactored some stuff. --- .../object_rasterizer.hpp | 40 +- .../object_rasterizer.cpp | 407 ++++-------------- 2 files changed, 98 insertions(+), 349 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp index afe5e7d..14237c5 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp @@ -85,7 +85,7 @@ class ObjectRasterizer void set_number_of_poses(int n_poses); - GLuint get_combined_texture(); + GLuint get_framebuffer_texture(); int get_n_poses_x(); int get_n_renders(); void get_depth_values(std::vector > &intersect_indices, @@ -96,23 +96,13 @@ class ObjectRasterizer static constexpr float FAR_PLANE = 4.0f; // Kinect does not see anything further away than 7 meters static const int WINDOW_WIDTH = 80; // default values if not specified static const int WINDOW_HEIGHT = 60; // default values if not specified - static const int TIME_MEASUREMENTS_COUNT = 16; - static const int RENDER_TYPE_COUNT = 1; - enum time_measurement { SEND_MATRICES, CLEAR_SCREEN, RENDER, FILL_PBO, MAP_PBO, GET_DEPTH_VALUES, - SEND_CONSTANTS, SEND_TEXTURES, SUM_LIKELIHOODS, UPDATE_TEXTURES, - ATTACH_TEXTURE, ALLOCATE, SET_VIEWPORT, SEND_MODEL_MATRIX, DETACH_TEXTURE, - GL_FINISH}; - enum render_type { COMBINED_FAST }; + static const int NR_SUBROUTINES_TO_MEASURE = 4; + enum subroutines_to_measure { ATTACH_TEXTURE, CLEAR_SCREEN, RENDER, DETACH_TEXTURE}; std::vector enum_strings_; - std::vector > cpu_times_aggregate_; - std::vector > gpu_times_aggregate_; - std::vector poses_rendered_; - std::vector > calls_aggregate_; - std::vector combined_fast_set_viewport_queries_; - std::vector combined_fast_send_model_matrix_queries_; - std::vector combined_fast_render_queries_; + std::vector gpu_times_aggregate_; + int nr_calls_; - bool initial_[RENDER_TYPE_COUNT]; + bool initial_run_; // GPU constraints @@ -133,13 +123,8 @@ class ObjectRasterizer int n_max_poses_y_; // needed for openGL time observation - GLuint time_query_[TIME_MEASUREMENTS_COUNT]; - unsigned int time_elapsed_ns_; + GLuint time_query_[NR_SUBROUTINES_TO_MEASURE]; - // needed for cpu time observation - double start_time_; - double stop_time_; - double cpu_times_[TIME_MEASUREMENTS_COUNT]; // the paths to the respective shaders @@ -170,12 +155,12 @@ class ObjectRasterizer GLuint index_buffer_; // contains the indices of the object meshes passed in the constructor // PBO for copying results to CPU - GLuint combined_result_buffer_; + GLuint result_buffer_; // custom framebuffer and its textures for color and depth GLuint framebuffer_; - GLuint combined_texture_; - GLuint combined_depth_texture_; + GLuint framebuffer_texture_for_all_poses_; + GLuint texture_for_z_testing; @@ -188,13 +173,12 @@ class ObjectRasterizer void read_depth(std::vector > &intersect_indices, std::vector > &depth, GLuint pixel_buffer_object, - GLuint framebuffer_texture, - render_type calling_function); + GLuint framebuffer_texture); Eigen::Matrix4f get_model_matrix(const std::vector state); Eigen::Matrix4f GetProjectionMatrix(float n, float f, float l, float r, float t, float b); - void display_time_observations(std::map factors, render_type calling_function); + void store_time_measurements(); void reallocate_buffers(); void setup_view_matrix(); void setup_projection_matrix(const Eigen::Matrix3f camera_matrix); diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp index 5e8834a..82d44e0 100644 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp +++ b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp @@ -1,6 +1,8 @@ /** @author Claudia Pfreundt */ -//#define PROFILING_ACTIVE +/* Note: Profiling slows down the rendering process. Use only to display the runtimes + * of the different subroutines inside the render call. */ +#define PROFILING_ACTIVE #include @@ -219,16 +221,11 @@ ObjectRasterizer::ObjectRasterizer(const std::vector zero_calls (TIME_MEASUREMENTS_COUNT, 0); - vector times (TIME_MEASUREMENTS_COUNT, 0); - for (int i = 0; i < RENDER_TYPE_COUNT; i++) { - calls_aggregate_.push_back(vector(zero_calls)); - cpu_times_aggregate_.push_back(vector(times)); - gpu_times_aggregate_.push_back(vector(times)); - poses_rendered_.push_back(0); - } + glGenQueries(NR_SUBROUTINES_TO_MEASURE, time_query_); - for (int i = 0; i < TIME_MEASUREMENTS_COUNT; i++) { - cpu_times_[i] = -1; - } - for (int i = 0; i < RENDER_TYPE_COUNT; i++) { - initial_[i] = true; - } + nr_calls_ = 0; + gpu_times_aggregate_ = vector (NR_SUBROUTINES_TO_MEASURE, 0); + initial_run_ = true; - enum_strings_.push_back("SEND_MATRICES"); + enum_strings_.push_back("ATTACH_TEXTURE"); enum_strings_.push_back("CLEAR_SCREEN"); enum_strings_.push_back("RENDER"); - enum_strings_.push_back("FILL_PBO"); - enum_strings_.push_back("MAP_PBO"); - enum_strings_.push_back("GET_DEPTH_VALUES"); - enum_strings_.push_back("SEND_CONSTANTS"); - enum_strings_.push_back("SEND_TEXTURES"); - enum_strings_.push_back("SUM_LIKELIHOODS"); - enum_strings_.push_back("UPDATE_TEXTURES"); - enum_strings_.push_back("ATTACH_TEXTURE"); - enum_strings_.push_back("ALLOCATE"); - enum_strings_.push_back("SET_VIEWPORT"); - enum_strings_.push_back("SEND_MODEL_MATRIX"); enum_strings_.push_back("DETACH_TEXTURE"); - enum_strings_.push_back("GL_FINISH"); + + // ========== SETUP PROJECTION MATRIX AND SHADER TO USE =========== // setup_projection_matrix(camera_matrix); glUseProgram(shader_ID_); @@ -338,171 +312,71 @@ void ObjectRasterizer::render(const std::vector > void ObjectRasterizer::render(const std::vector > > states) { #ifdef PROFILING_ACTIVE - combined_fast_set_viewport_queries_.resize(n_poses_); - combined_fast_send_model_matrix_queries_.resize(n_poses_); - combined_fast_render_queries_.resize(n_poses_); - glGenQueries(n_poses_, &combined_fast_set_viewport_queries_[0]); - glGenQueries(n_poses_, &combined_fast_send_model_matrix_queries_[0]); - glGenQueries(n_poses_, &combined_fast_render_queries_[0]); glBeginQuery(GL_TIME_ELAPSED, time_query_[ATTACH_TEXTURE]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[COMBINED_FAST][ATTACH_TEXTURE]++; + nr_calls_++; #endif glFramebufferTexture2D(GL_FRAMEBUFFER, // 1. fbo target: GL_FRAMEBUFFER GL_COLOR_ATTACHMENT0, // 2. attachment point GL_TEXTURE_2D, // 3. tex target: GL_TEXTURE_2D - combined_texture_, // 4. tex ID + framebuffer_texture_for_all_poses_, // 4. tex ID 0); #ifdef PROFILING_ACTIVE - + glFinish(); glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[ATTACH_TEXTURE] = stop_time_ - start_time_; - glBeginQuery(GL_TIME_ELAPSED, time_query_[CLEAR_SCREEN]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[COMBINED_FAST][CLEAR_SCREEN]++; #endif glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT); #ifdef PROFILING_ACTIVE - + glFinish(); glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[CLEAR_SCREEN] = stop_time_ - start_time_; - - glBeginQuery(GL_TIME_ELAPSED, time_query_[SEND_MATRICES]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[COMBINED_FAST][SEND_MATRICES]++; + glBeginQuery(GL_TIME_ELAPSED, time_query_[RENDER]); #endif glUniformMatrix4fv(projection_matrix_ID_, 1, GL_FALSE, projection_matrix_.data()); Matrix4f model_view_matrix; -#ifdef PROFILING_ACTIVE - - glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[SEND_MATRICES] = stop_time_ - start_time_; -#endif - - for (int i = 0; i < n_poses_y_ -1; i++) { for (int j = 0; j < n_poses_x_; j++) { - #ifdef PROFILING_ACTIVE - glBeginQuery(GL_TIME_ELAPSED, combined_fast_set_viewport_queries_[i * n_poses_x_ + j]); -// glBeginQuery(GL_TIME_ELAPSED, time_query_[SET_VIEWPORT]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[COMBINED_FAST][SET_VIEWPORT]++; - #endif - glViewport(j * n_cols_, (n_poses_y_ - 1 - i) * n_rows_, n_cols_, n_rows_); - #ifdef PROFILING_ACTIVE - - glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[SET_VIEWPORT] = stop_time_ - start_time_; - #endif - for (size_t k = 0; k < object_numbers_.size(); k++) { int index = object_numbers_[k]; - #ifdef PROFILING_ACTIVE - glBeginQuery(GL_TIME_ELAPSED, combined_fast_send_model_matrix_queries_[i * n_poses_x_ + j]); -// glBeginQuery(GL_TIME_ELAPSED, time_query_[SEND_MODEL_MATRIX]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[COMBINED_FAST][SEND_MODEL_MATRIX]++; - #endif - model_view_matrix = view_matrix_ * get_model_matrix(states[n_poses_x_ * i + j][index]); glUniformMatrix4fv(model_view_matrix_ID_, 1, GL_FALSE, model_view_matrix.data()); - #ifdef PROFILING_ACTIVE - glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[SEND_MODEL_MATRIX] = stop_time_ - start_time_; - glBeginQuery(GL_TIME_ELAPSED, combined_fast_render_queries_[i * n_poses_x_ + j]); -// glBeginQuery(GL_TIME_ELAPSED, time_query_[RENDER]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[COMBINED_FAST][RENDER]++; - #endif - - glDrawElements(GL_TRIANGLES, indices_per_object_[index], GL_UNSIGNED_INT, (void*) (start_position_[index] * sizeof(uint))); - - #ifdef PROFILING_ACTIVE - - glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[RENDER] = stop_time_ - start_time_; - #endif + glDrawElements(GL_TRIANGLES, indices_per_object_[index], GL_UNSIGNED_INT, (void*) (start_position_[index] * sizeof(uint))); } } } + // render last row of poses for (int j = 0; j < n_poses_ - (n_poses_x_ * (n_poses_y_ - 1)); j++) { -#ifdef PROFILING_ACTIVE - glBeginQuery(GL_TIME_ELAPSED, combined_fast_set_viewport_queries_[(n_poses_x_ * (n_poses_y_ - 1)) + j]); -// glBeginQuery(GL_TIME_ELAPSED, time_query_[SET_VIEWPORT]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[COMBINED_FAST][SET_VIEWPORT]++; -#endif - glViewport(j * n_cols_, 0, n_cols_, n_rows_); -#ifdef PROFILING_ACTIVE - - glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[SET_VIEWPORT] = stop_time_ - start_time_; -#endif - for (size_t k = 0; k < object_numbers_.size(); k++) { int index = object_numbers_[k]; -#ifdef PROFILING_ACTIVE - glBeginQuery(GL_TIME_ELAPSED, combined_fast_send_model_matrix_queries_[(n_poses_x_ * (n_poses_y_ - 1)) + j]); -// glBeginQuery(GL_TIME_ELAPSED, time_query_[SEND_MODEL_MATRIX]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[COMBINED_FAST][SEND_MODEL_MATRIX]++; -#endif + model_view_matrix = view_matrix_ * get_model_matrix(states[n_poses_x_ * (n_poses_y_ - 1) + j][index]); glUniformMatrix4fv(model_view_matrix_ID_, 1, GL_FALSE, model_view_matrix.data()); -#ifdef PROFILING_ACTIVE - - glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[SEND_MODEL_MATRIX] = stop_time_ - start_time_; - glBeginQuery(GL_TIME_ELAPSED, combined_fast_render_queries_[(n_poses_x_ * (n_poses_y_ - 1)) + j]); -// glBeginQuery(GL_TIME_ELAPSED, time_query_[RENDER]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[COMBINED_FAST][RENDER]++; -#endif - glDrawElements(GL_TRIANGLES, indices_per_object_[index], GL_UNSIGNED_INT, (void*) (start_position_[index] * sizeof(uint))); - -#ifdef PROFILING_ACTIVE - - glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[RENDER] = stop_time_ - start_time_; -#endif - } } #ifdef PROFILING_ACTIVE + glFinish(); + glEndQuery(GL_TIME_ELAPSED); glBeginQuery(GL_TIME_ELAPSED, time_query_[DETACH_TEXTURE]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[COMBINED_FAST][DETACH_TEXTURE]++; #endif glFramebufferTexture2D(GL_FRAMEBUFFER, // 1. fbo target: GL_FRAMEBUFFER @@ -512,48 +386,24 @@ void ObjectRasterizer::render(const std::vector > 0); #ifdef PROFILING_ACTIVE + glFinish(); glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[DETACH_TEXTURE] = stop_time_ - start_time_; - glBeginQuery(GL_TIME_ELAPSED, time_query_[GL_FINISH]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[COMBINED_FAST][GL_FINISH]++; + store_time_measurements(); #endif /* TODO should be unnecessary when texture is previously detached from framebuffer.. * would like to find evidence that this detaching really introduces a synchronization though*/ glFinish(); -#ifdef PROFILING_ACTIVE - glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[GL_FINISH] = stop_time_ - start_time_; - poses_rendered_[COMBINED_FAST] += n_poses_ * object_numbers_.size(); - map factors; - factors[SET_VIEWPORT] = n_poses_; - factors[SEND_MODEL_MATRIX] = n_poses_ * object_numbers_.size(); - factors[RENDER] = n_poses_ * object_numbers_.size(); - DisplayTimeObservations(factors, COMBINED_FAST); -#endif } - - void ObjectRasterizer::read_depth(vector > &intersect_indices, vector > &depth, GLuint pixel_buffer_object, - GLuint framebuffer_texture, - render_type calling_function) { - - + GLuint framebuffer_texture) { -#ifdef PROFILING_ACTIVE - glBeginQuery(GL_TIME_ELAPSED, time_query_[FILL_PBO]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[calling_function][FILL_PBO]++; -#endif // ===================== COPY VALUES FROM GPU TO CPU ===================== // @@ -563,15 +413,6 @@ void ObjectRasterizer::read_depth(vector > &intersect_indices, GLfloat *pixel_depth = (GLfloat*) glMapBuffer(GL_PIXEL_PACK_BUFFER, GL_READ_ONLY); -#ifdef PROFILING_ACTIVE - glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[FILL_PBO] = stop_time_ - start_time_; - glBeginQuery(GL_TIME_ELAPSED, time_query_[GET_DEPTH_VALUES]); - start_time_ = sf::hf::get_wall_time(); - calls_aggregate_[calling_function][GET_DEPTH_VALUES]++; -#endif - if (pixel_depth != (GLfloat*) NULL) { vector depth_image(n_rows_ * n_cols_ * n_poses_x_ * n_poses_y_, numeric_limits::max()); @@ -630,11 +471,6 @@ void ObjectRasterizer::read_depth(vector > &intersect_indices, glBindBuffer(GL_PIXEL_PACK_BUFFER, 0); -#ifdef PROFILING_ACTIVE - glEndQuery(GL_TIME_ELAPSED); - stop_time_ = sf::hf::get_wall_time(); - cpu_times_[GET_DEPTH_VALUES] = stop_time_ - start_time_; -#endif } @@ -710,7 +546,7 @@ void ObjectRasterizer::reallocate_buffers() { // ======================= REALLOCATE PBO ======================= // - glBindBuffer(GL_PIXEL_PACK_BUFFER, combined_result_buffer_); + glBindBuffer(GL_PIXEL_PACK_BUFFER, result_buffer_); // TODO PERFORMANCE: try GL_STREAM_READ instead of GL_DYNAMIC_READ // the NULL means this buffer is uninitialized, since I only want to copy values back to the CPU that will be written by the GPU glBufferData(GL_PIXEL_PACK_BUFFER, n_max_poses_x_* n_cols_ * n_max_poses_y_ * n_rows_ * sizeof(GLfloat), NULL, GL_STREAM_READ); @@ -732,13 +568,13 @@ void ObjectRasterizer::reallocate_buffers() { cout << "reallocating combined texture..." << endl; - glBindTexture(GL_TEXTURE_2D, combined_texture_); + glBindTexture(GL_TEXTURE_2D, framebuffer_texture_for_all_poses_); glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, n_max_poses_x_ * n_cols_, n_max_poses_y_ * n_rows_, 0, GL_RED, GL_FLOAT, 0); glBindTexture(GL_TEXTURE_2D, 0); cout << "reallocating combined depth texture..." << endl; - glBindRenderbuffer(GL_RENDERBUFFER, combined_depth_texture_); + glBindRenderbuffer(GL_RENDERBUFFER, texture_for_z_testing); glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT, n_max_poses_x_* n_cols_, n_max_poses_y_ * n_rows_); glBindRenderbuffer(GL_RENDERBUFFER, 0); @@ -747,11 +583,11 @@ void ObjectRasterizer::reallocate_buffers() { glFramebufferRenderbuffer(GL_FRAMEBUFFER, // 1. fbo target: GL_FRAMEBUFFER GL_DEPTH_ATTACHMENT, // 2. attachment point GL_RENDERBUFFER, // 3. rbo target: GL_RENDERBUFFER - combined_depth_texture_); // 4. rbo ID + texture_for_z_testing); // 4. rbo ID glFramebufferTexture2D(GL_FRAMEBUFFER, // 1. fbo target: GL_FRAMEBUFFER GL_COLOR_ATTACHMENT0, // 2. attachment point GL_TEXTURE_2D, // 3. tex target: GL_TEXTURE_2D - combined_texture_, // 4. tex ID + framebuffer_texture_for_all_poses_, // 4. tex ID 0); @@ -826,132 +662,38 @@ void ObjectRasterizer::setup_view_matrix() { view_matrix_ = view_matrix_transform.matrix(); } -void ObjectRasterizer::display_time_observations(map factors, render_type calling_function) { - +void ObjectRasterizer::store_time_measurements() { #ifdef PROFILING_ACTIVE - string function; - switch (calling_function) { - case COMBINED_FAST: function = "COMBINED_FAST"; break; - default: function = "default"; - } - - - for (int i = 0; i < TIME_MEASUREMENTS_COUNT; i++) { - - if (cpu_times_[i] != -1) { - GLint available = 0; - double time_elapsed_s; - - if (calling_function == COMBINED_FAST) { - if (i == SET_VIEWPORT) { - for (int j = 0; j < combined_fast_set_viewport_queries_.size(); j++) { - available = 0; - while (!available) { - glGetQueryObjectiv(combined_fast_set_viewport_queries_[j], GL_QUERY_RESULT_AVAILABLE, &available); - } - glGetQueryObjectuiv(combined_fast_set_viewport_queries_[j], GL_QUERY_RESULT, &time_elapsed_ns_); - time_elapsed_s = time_elapsed_ns_ / (double) 1e9; - - gpu_times_aggregate_[calling_function][i] += time_elapsed_s; - } - } else if (i == SEND_MODEL_MATRIX) { - for (int j = 0; j < combined_fast_send_model_matrix_queries_.size(); j++) { - available = 0; - while (!available) { - glGetQueryObjectiv(combined_fast_send_model_matrix_queries_[j], GL_QUERY_RESULT_AVAILABLE, &available); - } - glGetQueryObjectuiv(combined_fast_send_model_matrix_queries_[j], GL_QUERY_RESULT, &time_elapsed_ns_); - time_elapsed_s = time_elapsed_ns_ / (double) 1e9; - - gpu_times_aggregate_[calling_function][i] += time_elapsed_s; - } - } else if (i == RENDER) { - for (int j = 0; j < combined_fast_render_queries_.size(); j++) { - available = 0; - while (!available) { - glGetQueryObjectiv(combined_fast_render_queries_[j], GL_QUERY_RESULT_AVAILABLE, &available); - } - glGetQueryObjectuiv(combined_fast_render_queries_[j], GL_QUERY_RESULT, &time_elapsed_ns_); - time_elapsed_s = time_elapsed_ns_ / (double) 1e9; - - gpu_times_aggregate_[calling_function][i] += time_elapsed_s; - } - } else { - available = 0; - while (!available) { - glGetQueryObjectiv(time_query_[i], GL_QUERY_RESULT_AVAILABLE, &available); - } - glGetQueryObjectuiv(time_query_[i], GL_QUERY_RESULT, &time_elapsed_ns_); - time_elapsed_s = time_elapsed_ns_ / (double) 1e9; - gpu_times_aggregate_[calling_function][i] += time_elapsed_s; - } - } else { - available = 0; - while (!available) { - glGetQueryObjectiv(time_query_[i], GL_QUERY_RESULT_AVAILABLE, &available); - } - glGetQueryObjectuiv(time_query_[i], GL_QUERY_RESULT, &time_elapsed_ns_); - time_elapsed_s = time_elapsed_ns_ / (double) 1e9; - gpu_times_aggregate_[calling_function][i] += time_elapsed_s; - } - - cpu_times_aggregate_[calling_function][i] += cpu_times_[i]; + // retrieve times from OpenGL and store them + for (int i = 0; i < NR_SUBROUTINES_TO_MEASURE; i++) { + GLint available = 0; + double time_elapsed_s; + unsigned int time_elapsed_ns; - cpu_times_[i] = -1; - } - } - - if (poses_rendered_[calling_function] == n_poses_ && initial_[calling_function]) { - cout << "reset after initial poses" << endl; - initial_[calling_function] = false; - for (int i = 0; i < TIME_MEASUREMENTS_COUNT; i++) { - if (calls_aggregate_[calling_function][i] != 0) { - cpu_times_aggregate_[calling_function][i] = 0; - gpu_times_aggregate_[calling_function][i] = 0; - } - calls_aggregate_[calling_function][i] = 0; + while (!available) { + glGetQueryObjectiv(time_query_[i], GL_QUERY_RESULT_AVAILABLE, &available); } + glGetQueryObjectuiv(time_query_[i], GL_QUERY_RESULT, &time_elapsed_ns); + time_elapsed_s = time_elapsed_ns / (double) 1e9; + gpu_times_aggregate_[i] += time_elapsed_s; - poses_rendered_[calling_function] = 0; } - const int ROUNDS = 500; - if (poses_rendered_[calling_function] >= n_poses_ * ROUNDS) { - - cout << endl << "Time observations for " << function << " averaged over " << poses_rendered_[calling_function] << " poses:" << endl << endl; - - - double total_time = 0; - - for (int i = 0; i < TIME_MEASUREMENTS_COUNT; i++) { - if (calls_aggregate_[calling_function][i] != 0) { - - total_time += max(gpu_times_aggregate_[calling_function][i], cpu_times_aggregate_[calling_function][i]); - - int n_calls = calls_aggregate_[calling_function][i]; - cout << getTextForEnum(i) << ": " - << "\t (GPU) " << gpu_times_aggregate_[calling_function][i] / n_calls - << "\t (CPU) " << cpu_times_aggregate_[calling_function][i] / n_calls - << "\t (x" << n_calls /* / ROUNDS */ << ") " << max(gpu_times_aggregate_[calling_function][i], cpu_times_aggregate_[calling_function][i]) /* / ROUNDS*/ - << "\t TOTAL_TIME: " << total_time /* / ROUNDS*/ << ", calls_aggregate: " << n_calls << endl; - cpu_times_aggregate_[calling_function][i] = 0; - gpu_times_aggregate_[calling_function][i] = 0; - } - - calls_aggregate_[calling_function][i] = 0; + // the first run should not count. Reset all the counters. + if (initial_run_) { + initial_run_ = false; + for (int i = 0; i < NR_SUBROUTINES_TO_MEASURE; i++) { + gpu_times_aggregate_[i] = 0; } - cout << "TOTAL_TIME: \t" << total_time / poses_rendered_[calling_function] << "\t(x" << poses_rendered_[calling_function] << ") " << total_time / ROUNDS << endl << endl; - poses_rendered_[calling_function] = 0; + nr_calls_ = 0; } - #endif - } -GLuint ObjectRasterizer::get_combined_texture() { - return combined_texture_; +GLuint ObjectRasterizer::get_framebuffer_texture() { + return framebuffer_texture_for_all_poses_; } int ObjectRasterizer::get_n_poses_x() { @@ -968,12 +710,12 @@ void ObjectRasterizer::get_depth_values(std::vector > &intersec glFramebufferTexture2D(GL_FRAMEBUFFER, // 1. fbo target: GL_FRAMEBUFFER GL_COLOR_ATTACHMENT0, // 2. attachment point GL_TEXTURE_2D, // 3. tex target: GL_TEXTURE_2D - combined_texture_, // 4. tex ID + framebuffer_texture_for_all_poses_, // 4. tex ID 0); // ===================== TRANSFERS DEPTH VALUES TO CPU == SLOW!!! ================ // - read_depth(intersect_indices, depth, combined_result_buffer_, combined_texture_, COMBINED_FAST); + read_depth(intersect_indices, depth, result_buffer_, framebuffer_texture_for_all_poses_); // vector intersect_indices_all; // vector depth_all; @@ -988,7 +730,7 @@ void ObjectRasterizer::get_depth_values(std::vector > &intersec //// int n_rows_old = n_rows_; //// n_cols_ = n_poses_x_ * n_cols_; //// n_rows_ = n_poses_y_ * n_rows_; -// ReadDepth(intersect_indices_all, depth_all, combined_result_buffer_, combined_texture_, COMBINED_FAST); +// ReadDepth(intersect_indices_all, depth_all, combined_result_buffer_, combined_texture_); //// n_cols_ = n_cols_old; //// n_rows_ = n_rows_old; @@ -1079,6 +821,35 @@ bool ObjectRasterizer::check_framebuffer_status() { ObjectRasterizer::~ObjectRasterizer() { + +#ifdef PROFILING_ON + + if (nr_calls_ != 0) { + cout << endl << "Time measurements for the different steps of the rendering process averaged over " << nr_calls_ << " render calls:" << endl << endl; + + double total_time_per_render = 0; + + for (int i = 0; i < NR_SUBROUTINES_TO_MEASURE; i++) { + total_time_per_render += gpu_times_aggregate_[i]; + } + total_time_per_render /= nr_calls_; + + for (int i = 0; i < NR_SUBROUTINES_TO_MEASURE; i++) { + double time_per_subroutine = gpu_times_aggregate_[i] / nr_calls_; + + cout << get_text_for_enum(i) << ": " + << "\t " << time_per_subroutine << " s \t " << setprecision(1) + << time_per_subroutine * 100 / total_time_per_render << " %" << setprecision(9) << endl; + } + + cout << "TOTAL TIME PER RENDER CALL : " << total_time_per_render << endl << endl; + } else { + cout << "The render() function was never called, so there are no time measurements of it available." << endl; + } + + glDeleteQueries(NR_SUBROUTINES_TO_MEASURE, time_query_); +#endif + cout << "clean up OpenGL.." << endl; glDisableVertexAttribArray(0); @@ -1086,19 +857,13 @@ ObjectRasterizer::~ObjectRasterizer() glDeleteBuffers(1, &vertex_buffer_); glDeleteBuffers(1, &index_buffer_); - glDeleteBuffers(1, &combined_result_buffer_); + glDeleteBuffers(1, &result_buffer_); glDeleteFramebuffers(1, &framebuffer_); - glDeleteTextures(1, &combined_texture_); - glDeleteRenderbuffers(1, &combined_depth_texture_); + glDeleteTextures(1, &framebuffer_texture_for_all_poses_); + glDeleteRenderbuffers(1, &texture_for_z_testing); glDeleteProgram(shader_ID_); - glDeleteQueries(21, time_query_); -#ifdef PROFILING_ON - glDeleteQueries(n_poses_, &combined_fast_set_viewport_queries_[0]); - glGenQueries(n_poses_, &combined_fast_send_model_matrix_queries_[0]); - glGenQueries(n_poses_, &combined_fast_render_queries_[0]); -#endif } From 4cea7b7ecf483b805cca284a5d31ae709602a508 Mon Sep 17 00:00:00 2001 From: Claudia Pfreundt Date: Thu, 3 Sep 2015 17:41:24 +0200 Subject: [PATCH 044/131] refactored some functions and variables in object_rasterizer --- .../object_rasterizer.hpp | 60 ++- .../object_rasterizer.cpp | 354 ++++++++---------- 2 files changed, 178 insertions(+), 236 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp index 14237c5..3d0f994 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp @@ -86,8 +86,7 @@ class ObjectRasterizer GLuint get_framebuffer_texture(); - int get_n_poses_x(); - int get_n_renders(); + int get_nr_poses_per_row(); void get_depth_values(std::vector > &intersect_indices, std::vector > &depth); @@ -96,35 +95,32 @@ class ObjectRasterizer static constexpr float FAR_PLANE = 4.0f; // Kinect does not see anything further away than 7 meters static const int WINDOW_WIDTH = 80; // default values if not specified static const int WINDOW_HEIGHT = 60; // default values if not specified - static const int NR_SUBROUTINES_TO_MEASURE = 4; - enum subroutines_to_measure { ATTACH_TEXTURE, CLEAR_SCREEN, RENDER, DETACH_TEXTURE}; - std::vector enum_strings_; - std::vector gpu_times_aggregate_; - int nr_calls_; - - bool initial_run_; // GPU constraints - GLint max_dimension_; // values initialized to WINDOW_WIDTH, WINDOW_HEIGHT in constructor. May be changed by user with set_resolution(). - int n_rows_; - int n_cols_; + int nr_rows_; + int nr_cols_; // number of poses to render - int n_poses_; - int n_poses_x_; - int n_poses_y_; + int nr_poses_; + int nr_poses_per_row_; + int nr_poses_per_column_; // number of maximum poses - int n_max_poses_; - int n_max_poses_x_; - int n_max_poses_y_; + int nr_max_poses_; + int nr_max_poses_per_row_; + int nr_max_poses_per_column_; // needed for openGL time observation + static const int NR_SUBROUTINES_TO_MEASURE = 4; GLuint time_query_[NR_SUBROUTINES_TO_MEASURE]; - + enum subroutines_to_measure { ATTACH_TEXTURE, CLEAR_SCREEN, RENDER, DETACH_TEXTURE}; + std::vector strings_for_subroutines; + std::vector gpu_times_aggregate_; + int nr_calls_; + bool initial_run_; // the paths to the respective shaders @@ -144,12 +140,12 @@ class ObjectRasterizer Eigen::Matrix4f projection_matrix_; Eigen::Matrix4f view_matrix_; - // shader program ID and uniform IDs to pass variables to it + // shader program ID and matrix uniform IDs to pass variables to them GLuint shader_ID_; GLuint model_view_matrix_ID_; // ID to which we pass the modelview matrix GLuint projection_matrix_ID_; // ID to which we pass the projection matrix - // VAO, VBO and element array are needed to store the object meshes + // VAO, VBO and element arrays are needed to store the object meshes GLuint vertex_array_; // The vertex array contains the vertex and index buffers GLuint vertex_buffer_; // contains the vertices of the object meshes passed in the constructor GLuint index_buffer_; // contains the indices of the object meshes passed in the constructor @@ -167,21 +163,21 @@ class ObjectRasterizer // ====================== PRIVATE FUNCTIONS ====================== // - std::string get_text_for_enum( int enumVal ); - void check_GL_errors(const char *label); - bool check_framebuffer_status(); - void read_depth(std::vector > &intersect_indices, - std::vector > &depth, - GLuint pixel_buffer_object, - GLuint framebuffer_texture); + void reallocate_buffers(); + // set up model-, view- and projection-matrix Eigen::Matrix4f get_model_matrix(const std::vector state); - Eigen::Matrix4f GetProjectionMatrix(float n, float f, float l, float r, float t, float b); + void setup_view_matrix(); + void setup_projection_matrix(const Eigen::Matrix3f camera_matrix); + Eigen::Matrix4f get_projection_matrix(float n, float f, float l, float r, float t, float b); + // functions for time measurement void store_time_measurements(); - void reallocate_buffers(); - void setup_view_matrix(); - void setup_projection_matrix(const Eigen::Matrix3f camera_matrix); + std::string get_text_for_enum( int enumVal ); + + // functions for error checking + void check_GL_errors(const char *label); + bool check_framebuffer_status(); }; diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp index 82d44e0..e99c71c 100644 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp +++ b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp @@ -32,8 +32,8 @@ ObjectRasterizer::ObjectRasterizer(const std::vector (NR_SUBROUTINES_TO_MEASURE, 0); initial_run_ = true; - enum_strings_.push_back("ATTACH_TEXTURE"); - enum_strings_.push_back("CLEAR_SCREEN"); - enum_strings_.push_back("RENDER"); - enum_strings_.push_back("DETACH_TEXTURE"); + strings_for_subroutines.push_back("ATTACH_TEXTURE"); + strings_for_subroutines.push_back("CLEAR_SCREEN"); + strings_for_subroutines.push_back("RENDER"); + strings_for_subroutines.push_back("DETACH_TEXTURE"); // ========== SETUP PROJECTION MATRIX AND SHADER TO USE =========== // @@ -340,15 +340,15 @@ void ObjectRasterizer::render(const std::vector > Matrix4f model_view_matrix; - for (int i = 0; i < n_poses_y_ -1; i++) { - for (int j = 0; j < n_poses_x_; j++) { + for (int i = 0; i < nr_poses_per_column_ -1; i++) { + for (int j = 0; j < nr_poses_per_row_; j++) { - glViewport(j * n_cols_, (n_poses_y_ - 1 - i) * n_rows_, n_cols_, n_rows_); + glViewport(j * nr_cols_, (nr_poses_per_column_ - 1 - i) * nr_rows_, nr_cols_, nr_rows_); for (size_t k = 0; k < object_numbers_.size(); k++) { int index = object_numbers_[k]; - model_view_matrix = view_matrix_ * get_model_matrix(states[n_poses_x_ * i + j][index]); + model_view_matrix = view_matrix_ * get_model_matrix(states[nr_poses_per_row_ * i + j][index]); glUniformMatrix4fv(model_view_matrix_ID_, 1, GL_FALSE, model_view_matrix.data()); glDrawElements(GL_TRIANGLES, indices_per_object_[index], GL_UNSIGNED_INT, (void*) (start_position_[index] * sizeof(uint))); @@ -357,14 +357,14 @@ void ObjectRasterizer::render(const std::vector > } // render last row of poses - for (int j = 0; j < n_poses_ - (n_poses_x_ * (n_poses_y_ - 1)); j++) { + for (int j = 0; j < nr_poses_ - (nr_poses_per_row_ * (nr_poses_per_column_ - 1)); j++) { - glViewport(j * n_cols_, 0, n_cols_, n_rows_); + glViewport(j * nr_cols_, 0, nr_cols_, nr_rows_); for (size_t k = 0; k < object_numbers_.size(); k++) { int index = object_numbers_[k]; - model_view_matrix = view_matrix_ * get_model_matrix(states[n_poses_x_ * (n_poses_y_ - 1) + j][index]); + model_view_matrix = view_matrix_ * get_model_matrix(states[nr_poses_per_row_ * (nr_poses_per_column_ - 1) + j][index]); glUniformMatrix4fv(model_view_matrix_ID_, 1, GL_FALSE, model_view_matrix.data()); glDrawElements(GL_TRIANGLES, indices_per_object_[index], GL_UNSIGNED_INT, (void*) (start_position_[index] * sizeof(uint))); @@ -399,48 +399,127 @@ void ObjectRasterizer::render(const std::vector > } -void ObjectRasterizer::read_depth(vector > &intersect_indices, - vector > &depth, - GLuint pixel_buffer_object, - GLuint framebuffer_texture) { +void ObjectRasterizer::set_objects(vector object_numbers) { + // TODO does it copy the vector or set a reference? + object_numbers_ = object_numbers; +} + +void ObjectRasterizer::set_number_of_max_poses(int n_poses) { +// if (n_poses_ != n_poses) { + nr_max_poses_ = n_poses; + nr_poses_ = n_poses; + +// // TODO max_dimension[0], [1], at the moment they are identical + float sqrt_poses = sqrt(nr_poses_); + // TODO this can be done smarter. I want to only increment sqrt_poses, if it is not an int, i.e. 10.344 instead of 10) + if (sqrt_poses * sqrt_poses != nr_poses_) sqrt_poses = (int) ceil(sqrt_poses); + // TODO max_dimension[0], [1], at the moment they are identical + nr_max_poses_per_row_ = min((int) sqrt_poses, (max_dimension_ / nr_cols_)); + int y_poses = nr_max_poses_ / nr_max_poses_per_row_; + if (y_poses * nr_max_poses_per_row_ < nr_max_poses_) y_poses++; + nr_max_poses_per_column_ = min(y_poses, (max_dimension_ / nr_rows_)); + + nr_poses_per_row_ = nr_max_poses_per_row_; + nr_poses_per_column_ = nr_max_poses_per_column_; + + reallocate_buffers(); +// } +} + +void ObjectRasterizer::set_number_of_poses(int n_poses) { + if (n_poses <= nr_max_poses_) { + nr_poses_ = n_poses; + + +// // TODO max_dimension[0], [1], at the moment they are identical + float sqrt_poses = sqrt(nr_poses_); + // TODO this can be done smarter. I want to only increment sqrt_poses, if it is not an int, i.e. 10.344 instead of 10) + if (sqrt_poses * sqrt_poses != nr_poses_) sqrt_poses = (int) ceil(sqrt_poses); + // TODO max_dimension[0], [1], at the moment they are identical + nr_poses_per_row_ = min((int) sqrt_poses, (max_dimension_ / nr_cols_)); + int y_poses = nr_poses_ / nr_poses_per_row_; + if (y_poses * nr_poses_per_row_ < nr_poses_) y_poses++; + nr_poses_per_column_ = min(y_poses, (max_dimension_ / nr_rows_)); + + if (nr_poses_per_row_ > nr_max_poses_per_row_ || nr_poses_per_column_ > nr_max_poses_per_column_) { + cout << "WARNING: You tried to evaluate more poses in a row or in a column than was allocated in the beginning." + << endl << "Check set_number_of_poses() functions in object_rasterizer.cpp" << endl; + } + + } else { + cout << "WARNING: You tried to evaluate more poses than specified by max_poses" << endl; + } +} + +void ObjectRasterizer::set_resolution(const int n_rows, + const int n_cols) { +// if (n_rows_ != n_rows || n_cols_ != n_cols) { + nr_rows_ = n_rows; + nr_cols_ = n_cols; + + // recalculate n_poses_x_ and n_poses_y_ depending on the resolution + set_number_of_max_poses(nr_max_poses_); + + +// } +} + +GLuint ObjectRasterizer::get_framebuffer_texture() { + return framebuffer_texture_for_all_poses_; +} + +int ObjectRasterizer::get_nr_poses_per_row() { + return nr_poses_per_row_; +} - // ===================== COPY VALUES FROM GPU TO CPU ===================== // - glBindBuffer(GL_PIXEL_PACK_BUFFER, pixel_buffer_object); - glBindTexture(GL_TEXTURE_2D, framebuffer_texture); +void ObjectRasterizer::get_depth_values(std::vector > &intersect_indices, + std::vector > &depth) { + + // ===================== ATTACH TEXTURE TO FRAMEBUFFER ================ // + + glFramebufferTexture2D(GL_FRAMEBUFFER, // 1. fbo target: GL_FRAMEBUFFER + GL_COLOR_ATTACHMENT0, // 2. attachment point + GL_TEXTURE_2D, // 3. tex target: GL_TEXTURE_2D + framebuffer_texture_for_all_poses_, // 4. tex ID + 0); + + // ===================== TRANSFER DEPTH VALUES FROM GPU TO CPU == SLOW!!! ================ // + + glBindBuffer(GL_PIXEL_PACK_BUFFER, result_buffer_); + glBindTexture(GL_TEXTURE_2D, framebuffer_texture_for_all_poses_); glGetTexImage(GL_TEXTURE_2D, 0, GL_RED, GL_FLOAT, 0); GLfloat *pixel_depth = (GLfloat*) glMapBuffer(GL_PIXEL_PACK_BUFFER, GL_READ_ONLY); if (pixel_depth != (GLfloat*) NULL) { - vector depth_image(n_rows_ * n_cols_ * n_poses_x_ * n_poses_y_, numeric_limits::max()); + vector depth_image(nr_rows_ * nr_cols_ * nr_poses_per_row_ * nr_poses_per_column_, numeric_limits::max()); - int pixels_per_row_texture = n_max_poses_x_ * n_cols_; - int pixels_per_row_extracted = n_poses_x_ * n_cols_; - int highest_pixel_per_col = (n_poses_y_ * n_rows_) - 1; + int pixels_per_row_texture = nr_max_poses_per_row_ * nr_cols_; + int pixels_per_row_extracted = nr_poses_per_row_ * nr_cols_; + int highest_pixel_per_col = (nr_poses_per_column_ * nr_rows_) - 1; // reading OpenGL texture into an array on the CPU (inverted rows) - for (int row = 0; row < n_poses_y_ * n_rows_; row++) { + for (int row = 0; row < nr_poses_per_column_ * nr_rows_; row++) { int inverted_row = highest_pixel_per_col - row; - for (int col = 0; col < n_poses_x_ * n_cols_; col++) { + for (int col = 0; col < nr_poses_per_row_ * nr_cols_; col++) { depth_image[row * pixels_per_row_extracted + col] = pixel_depth[inverted_row * pixels_per_row_texture + col]; } } - glUnmapBuffer(GL_PIXEL_PACK_BUFFER); // subdividing this array into an array per pose - vector > depth_image_per_pose (n_poses_, vector (n_rows_ * n_cols_, 0)); + vector > depth_image_per_pose (nr_poses_, vector (nr_rows_ * nr_cols_, 0)); - for (int pose_y = 0; pose_y < n_poses_y_; pose_y++) { - for (int pose_x = 0; pose_x < n_poses_x_ && pose_y * n_poses_x_ + pose_x < n_poses_; pose_x++) { - for (int i = 0; i < n_rows_ * n_cols_; i++) { + for (int pose_y = 0; pose_y < nr_poses_per_column_; pose_y++) { + for (int pose_x = 0; pose_x < nr_poses_per_row_ && pose_y * nr_poses_per_row_ + pose_x < nr_poses_; pose_x++) { + for (int i = 0; i < nr_rows_ * nr_cols_; i++) { - depth_image_per_pose[pose_y * n_poses_x_ + pose_x][i] = depth_image[(pose_y * n_rows_ + (i / n_cols_)) * pixels_per_row_extracted + pose_x * n_cols_ + (i % n_cols_)]; + depth_image_per_pose[pose_y * nr_poses_per_row_ + pose_x][i] = depth_image[(pose_y * nr_rows_ + (i / nr_cols_)) * pixels_per_row_extracted + pose_x * nr_cols_ + (i % nr_cols_)]; } } } @@ -449,8 +528,8 @@ void ObjectRasterizer::read_depth(vector > &intersect_indices, vector tmp_indices; vector tmp_depth; - for (int state = 0; state < n_poses_; state++) { - for (int i = 0; i < n_rows_ * n_cols_; i++) { + for (int state = 0; state < nr_poses_; state++) { + for (int i = 0; i < nr_rows_ * nr_cols_; i++) { if (depth_image_per_pose[state][i] != 0) { tmp_indices.push_back(i); tmp_depth.push_back(depth_image_per_pose[state][i]); @@ -462,94 +541,43 @@ void ObjectRasterizer::read_depth(vector > &intersect_indices, tmp_depth.resize(0); } - - - } else { cout << "WARNING: Could not map Pixel Pack Buffer." << endl; } glBindBuffer(GL_PIXEL_PACK_BUFFER, 0); -} - - - - + // ===================== DETACH TEXTURE FROM FRAMEBUFFER ================ // -void ObjectRasterizer::set_objects(vector object_numbers) { - // TODO does it copy the vector or set a reference? - object_numbers_ = object_numbers; + glFramebufferTexture2D(GL_FRAMEBUFFER, // 1. fbo target: GL_FRAMEBUFFER + GL_COLOR_ATTACHMENT0, // 2. attachment point + GL_TEXTURE_2D, // 3. tex target: GL_TEXTURE_2D + 0, // 4. tex ID + 0); } -void ObjectRasterizer::set_number_of_max_poses(int n_poses) { -// if (n_poses_ != n_poses) { - n_max_poses_ = n_poses; - n_poses_ = n_poses; -// // TODO max_dimension[0], [1], at the moment they are identical - float sqrt_poses = sqrt(n_poses_); - // TODO this can be done smarter. I want to only increment sqrt_poses, if it is not an int, i.e. 10.344 instead of 10) - if (sqrt_poses * sqrt_poses != n_poses_) sqrt_poses = (int) ceil(sqrt_poses); - // TODO max_dimension[0], [1], at the moment they are identical - n_max_poses_x_ = min((int) sqrt_poses, (max_dimension_ / n_cols_)); - int y_poses = n_max_poses_ / n_max_poses_x_; - if (y_poses * n_max_poses_x_ < n_max_poses_) y_poses++; - n_max_poses_y_ = min(y_poses, (max_dimension_ / n_rows_)); - - n_poses_x_ = n_max_poses_x_; - n_poses_y_ = n_max_poses_y_; - reallocate_buffers(); -// } -} -void ObjectRasterizer::set_number_of_poses(int n_poses) { - if (n_poses <= n_max_poses_) { - n_poses_ = n_poses; - - -// // TODO max_dimension[0], [1], at the moment they are identical - float sqrt_poses = sqrt(n_poses_); - // TODO this can be done smarter. I want to only increment sqrt_poses, if it is not an int, i.e. 10.344 instead of 10) - if (sqrt_poses * sqrt_poses != n_poses_) sqrt_poses = (int) ceil(sqrt_poses); - // TODO max_dimension[0], [1], at the moment they are identical - n_poses_x_ = min((int) sqrt_poses, (max_dimension_ / n_cols_)); - int y_poses = n_poses_ / n_poses_x_; - if (y_poses * n_poses_x_ < n_poses_) y_poses++; - n_poses_y_ = min(y_poses, (max_dimension_ / n_rows_)); - if (n_poses_x_ > n_max_poses_x_ || n_poses_y_ > n_max_poses_y_) { - cout << "WARNING: You tried to evaluate more poses in a row or in a column than was allocated in the beginning." - << endl << "Check set_number_of_poses() functions in object_rasterizer.cpp" << endl; - } - } else { - cout << "WARNING: You tried to evaluate more poses than specified by max_poses" << endl; - } -} -void ObjectRasterizer::set_resolution(const int n_rows, - const int n_cols) { -// if (n_rows_ != n_rows || n_cols_ != n_cols) { - n_rows_ = n_rows; - n_cols_ = n_cols; - // recalculate n_poses_x_ and n_poses_y_ depending on the resolution - set_number_of_max_poses(n_max_poses_); +// ================================================================= // +// ================================================================= // +// ======================= PRIVATE FUNCTIONS ======================= // +// ================================================================= // +// ================================================================= // -// } -} void ObjectRasterizer::reallocate_buffers() { // ======================= REALLOCATE PBO ======================= // glBindBuffer(GL_PIXEL_PACK_BUFFER, result_buffer_); - // TODO PERFORMANCE: try GL_STREAM_READ instead of GL_DYNAMIC_READ // the NULL means this buffer is uninitialized, since I only want to copy values back to the CPU that will be written by the GPU - glBufferData(GL_PIXEL_PACK_BUFFER, n_max_poses_x_* n_cols_ * n_max_poses_y_ * n_rows_ * sizeof(GLfloat), NULL, GL_STREAM_READ); + glBufferData(GL_PIXEL_PACK_BUFFER, nr_max_poses_per_row_* nr_cols_ * nr_max_poses_per_column_ * nr_rows_ * sizeof(GLfloat), NULL, GL_STREAM_READ); glBindBuffer(GL_PIXEL_PACK_BUFFER, 0); // ======================= DETACH TEXTURES FROM FRAMEBUFFER ======================= // @@ -566,16 +594,12 @@ void ObjectRasterizer::reallocate_buffers() { // ======================= REALLOCATE FRAMEBUFFER TEXTURES ======================= // - cout << "reallocating combined texture..." << endl; - glBindTexture(GL_TEXTURE_2D, framebuffer_texture_for_all_poses_); - glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, n_max_poses_x_ * n_cols_, n_max_poses_y_ * n_rows_, 0, GL_RED, GL_FLOAT, 0); + glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, nr_max_poses_per_row_ * nr_cols_, nr_max_poses_per_column_ * nr_rows_, 0, GL_RED, GL_FLOAT, 0); glBindTexture(GL_TEXTURE_2D, 0); - cout << "reallocating combined depth texture..." << endl; - glBindRenderbuffer(GL_RENDERBUFFER, texture_for_z_testing); - glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT, n_max_poses_x_* n_cols_, n_max_poses_y_ * n_rows_); + glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT, nr_max_poses_per_row_* nr_cols_, nr_max_poses_per_column_ * nr_rows_); glBindRenderbuffer(GL_RENDERBUFFER, 0); // ======================= ATTACH NEW TEXTURES TO FRAMEBUFFER ======================= // @@ -598,9 +622,6 @@ void ObjectRasterizer::reallocate_buffers() { - - - Matrix4f ObjectRasterizer::get_model_matrix(const vector state) { // Model matrix equals the state of the object. Position and Quaternion just have to be expressed as a matrix. // note: state = (q.w, q.x, q.y, q.z, v.x, v.y, v.z) @@ -616,17 +637,14 @@ Matrix4f ObjectRasterizer::get_model_matrix(const vector state) { } -Matrix4f ObjectRasterizer::GetProjectionMatrix(float n, float f, float l, float r, float t, float b) { - Matrix4f projection_matrix = Matrix4f::Zero(); - projection_matrix(0,0) = 2 * n / (r - l); - projection_matrix(0,2) = (r + l) / (r - l); - projection_matrix(1,1) = 2 * n / (t - b); - projection_matrix(1,2) = (t + b) / (t - b); - projection_matrix(2,2) = -(f + n) / (f - n); - projection_matrix(2,3) = - 2 * f * n / (f - n); - projection_matrix(3,2) = -1; +void ObjectRasterizer::setup_view_matrix() { + // =========================== VIEW MATRIX =========================== // - return projection_matrix; + // OpenGL camera is rotated 180 degrees around the X-Axis compared to the Kinect camera + view_matrix_ = Matrix4f::Identity(); + Transform view_matrix_transform; + view_matrix_transform = AngleAxisf(M_PI, Vector3f::UnitX()); + view_matrix_ = view_matrix_transform.matrix(); } void ObjectRasterizer::setup_projection_matrix(const Eigen::Matrix3f camera_matrix) { @@ -636,7 +654,7 @@ void ObjectRasterizer::setup_projection_matrix(const Eigen::Matrix3f camera_matr Eigen::Matrix3f camera_matrix_inverse = camera_matrix.inverse(); Vector3f boundaries_min = camera_matrix_inverse * Vector3f(-0.5, -0.5, 1); - Vector3f boundaries_max = camera_matrix_inverse * Vector3f(float(n_cols_)-0.5, float(n_rows_)-0.5, 1); + Vector3f boundaries_max = camera_matrix_inverse * Vector3f(float(nr_cols_)-0.5, float(nr_rows_)-0.5, 1); float near = NEAR_PLANE; float far = FAR_PLANE; @@ -648,18 +666,22 @@ void ObjectRasterizer::setup_projection_matrix(const Eigen::Matrix3f camera_matr // ======================== PROJECTION MATRIX ======================== // // Projection Matrix takes into account our view frustum and projects the (3D)-scene onto a 2D image - projection_matrix_ = GetProjectionMatrix(near, far, left, right, top, bottom); + projection_matrix_ = get_projection_matrix(near, far, left, right, top, bottom); } -void ObjectRasterizer::setup_view_matrix() { - // =========================== VIEW MATRIX =========================== // - // OpenGL camera is rotated 180 degrees around the X-Axis compared to the Kinect camera - view_matrix_ = Matrix4f::Identity(); - Transform view_matrix_transform; - view_matrix_transform = AngleAxisf(M_PI, Vector3f::UnitX()); - view_matrix_ = view_matrix_transform.matrix(); +Matrix4f ObjectRasterizer::get_projection_matrix(float n, float f, float l, float r, float t, float b) { + Matrix4f projection_matrix = Matrix4f::Zero(); + projection_matrix(0,0) = 2 * n / (r - l); + projection_matrix(0,2) = (r + l) / (r - l); + projection_matrix(1,1) = 2 * n / (t - b); + projection_matrix(1,2) = (t + b) / (t - b); + projection_matrix(2,2) = -(f + n) / (f - n); + projection_matrix(2,3) = - 2 * f * n / (f - n); + projection_matrix(3,2) = -1; + + return projection_matrix; } void ObjectRasterizer::store_time_measurements() { @@ -692,85 +714,9 @@ void ObjectRasterizer::store_time_measurements() { #endif } -GLuint ObjectRasterizer::get_framebuffer_texture() { - return framebuffer_texture_for_all_poses_; -} - -int ObjectRasterizer::get_n_poses_x() { - return n_poses_x_; -} - - - -void ObjectRasterizer::get_depth_values(std::vector > &intersect_indices, - std::vector > &depth) { - - // ===================== ATTACH TEXTURE TO FRAMEBUFFER ================ // - - glFramebufferTexture2D(GL_FRAMEBUFFER, // 1. fbo target: GL_FRAMEBUFFER - GL_COLOR_ATTACHMENT0, // 2. attachment point - GL_TEXTURE_2D, // 3. tex target: GL_TEXTURE_2D - framebuffer_texture_for_all_poses_, // 4. tex ID - 0); - - // ===================== TRANSFERS DEPTH VALUES TO CPU == SLOW!!! ================ // - - read_depth(intersect_indices, depth, result_buffer_, framebuffer_texture_for_all_poses_); - -// vector intersect_indices_all; -// vector depth_all; - -// int width = n_cols_ * n_poses_x_; -// int index, index_x, index_y, pose_x, pose_y, local_index, pose; - -// vector intersect_indices_tmp[n_poses_]; -// vector depth_tmp[n_poses_]; - -//// int n_cols_old = n_cols_; -//// int n_rows_old = n_rows_; -//// n_cols_ = n_poses_x_ * n_cols_; -//// n_rows_ = n_poses_y_ * n_rows_; -// ReadDepth(intersect_indices_all, depth_all, combined_result_buffer_, combined_texture_); -//// n_cols_ = n_cols_old; -//// n_rows_ = n_rows_old; - -// cout << "intersect_indices_all size: " << intersect_indices_all.size() << endl; - -// for (size_t i = 0; i < intersect_indices_all.size(); i++) { -// index = intersect_indices_all[i]; -// index_x = index % width; -// index_y = index / width; -// pose_x = index_x / n_cols_; -// pose_y = index_y / n_rows_; - -// local_index = (index_x % n_cols_) + (index_y % n_rows_) * n_cols_; -// pose = pose_y * n_poses_x_ + pose_x; - -// intersect_indices_tmp[pose].push_back(local_index); -// depth_tmp[pose].push_back(depth_all[i]); - -// } - -// for (int i = 0; i < n_poses_; i++) { -// intersect_indices.push_back(intersect_indices_tmp[i]); -// depth.push_back(depth_tmp[i]); -// cout << "rasterizer indices: [" << i << "]: " << intersect_indices_tmp[i].size() << endl; -// } - - - // ===================== DETACH TEXTURE FROM FRAMEBUFFER ================ // - - glFramebufferTexture2D(GL_FRAMEBUFFER, // 1. fbo target: GL_FRAMEBUFFER - GL_COLOR_ATTACHMENT0, // 2. attachment point - GL_TEXTURE_2D, // 3. tex target: GL_TEXTURE_2D - 0, // 4. tex ID - 0); - -} - string ObjectRasterizer::get_text_for_enum( int enumVal ) { - return enum_strings_[enumVal]; + return strings_for_subroutines[enumVal]; } From bab110d6f53e56249a240bc6fe21c5e3c4ad71e6 Mon Sep 17 00:00:00 2001 From: issac Date: Sun, 6 Sep 2015 15:37:38 +0200 Subject: [PATCH 045/131] Extended interface by RigidBodyRenderer() constructor taking vertices, indices, camera_matrix and image size. Render() taking only the depth vector parameters() setter --- include/dbot/utils/rigid_body_renderer.hpp | 27 ++- src/dbot/utils/rigid_body_renderer.cpp | 256 ++++++++++++--------- 2 files changed, 174 insertions(+), 109 deletions(-) diff --git a/include/dbot/utils/rigid_body_renderer.hpp b/include/dbot/utils/rigid_body_renderer.hpp index addd103..ec10b41 100644 --- a/include/dbot/utils/rigid_body_renderer.hpp +++ b/include/dbot/utils/rigid_body_renderer.hpp @@ -36,14 +36,14 @@ along with this program. If not, see . #include -namespace ff +namespace dbot { class RigidBodyRenderer { public: typedef boost::shared_ptr Ptr; - typedef ff::RigidBodiesState State; + typedef dbot::RigidBodiesState State; typedef Eigen::Vector3d Vector; typedef Eigen::Matrix3d Matrix; typedef typename Eigen::Transform Affine; @@ -53,6 +53,13 @@ class RigidBodyRenderer const std::vector >& vertices, const std::vector > >& indices); + RigidBodyRenderer( + const std::vector >& vertices, + const std::vector > >& indices, + Matrix camera_matrix, + int n_rows, + int n_cols); + virtual ~RigidBodyRenderer(); void Render(Matrix camera_matrix, @@ -66,6 +73,8 @@ class RigidBodyRenderer int n_cols, std::vector &depth_image) const; + void Render(std::vector& depth_image) const; + std::vector > vertices() const; virtual void set_poses(const std::vector& rotations, @@ -73,7 +82,13 @@ class RigidBodyRenderer virtual void set_poses(const std::vector& poses); + void parameters(Matrix camera_matrix, int n_rows, int n_cols); +private: + /** + * Because c++0x on gcc.4.6 does not implement delegating constructors + */ + void init(); protected: // triangles @@ -84,6 +99,14 @@ class RigidBodyRenderer // state std::vector R_; std::vector t_; + + // cached center of mass + std::vector coms_; + std::vector com_weights_; + + Matrix camera_matrix_; + int n_rows_; + int n_cols_; }; } diff --git a/src/dbot/utils/rigid_body_renderer.cpp b/src/dbot/utils/rigid_body_renderer.cpp index 2e29832..b16806a 100644 --- a/src/dbot/utils/rigid_body_renderer.cpp +++ b/src/dbot/utils/rigid_body_renderer.cpp @@ -36,12 +36,36 @@ along with this program. If not, see . using namespace std; using namespace Eigen; -using namespace ff; +using namespace dbot; RigidBodyRenderer::RigidBodyRenderer( - const std::vector >& vertices, - const std::vector > >& indices) - :vertices_(vertices), indices_(indices) + const std::vector >& vertices, + const std::vector > >& indices) + : n_rows_(0), + n_cols_(0), + vertices_(vertices), + indices_(indices) +{ + camera_matrix_.setZero(); + init(); +} + +RigidBodyRenderer::RigidBodyRenderer( + const std::vector >& vertices, + const std::vector > >& indices, + Matrix camera_matrix, + int n_rows, + int n_cols) + : camera_matrix_(camera_matrix), + n_rows_(n_rows), + n_cols_(n_cols), + vertices_(vertices), + indices_(indices) +{ + init(); +} + +void RigidBodyRenderer::init() { /// initialize poses ******************************************************* R_.resize(vertices_.size()); @@ -56,28 +80,28 @@ RigidBodyRenderer::RigidBodyRenderer( /// compute normals ******************************************************** normals_.clear(); for(size_t part_index = 0; part_index < indices_.size(); part_index++) - { + { vector part_normals(indices_[part_index].size()); for(int triangle_index = 0; triangle_index < int(part_normals.size()); triangle_index++) - { - //compute the three cross products and make sure that they yield the same normal - vector temp_normals(3); - for(int vertex_index = 0; vertex_index < 3; vertex_index++) + { + //compute the three cross products and make sure that they yield the same normal + vector temp_normals(3); + for(int vertex_index = 0; vertex_index < 3; vertex_index++) temp_normals[vertex_index] = ((vertices_[part_index][ indices_[part_index][triangle_index][(vertex_index+1)%3] ]-vertices_[part_index][ indices_[part_index][triangle_index][vertex_index] ]).cross( vertices_[part_index][ indices_[part_index][triangle_index][(vertex_index+2)%3] ]-vertices_[part_index][ indices_[part_index][triangle_index][(vertex_index+1)%3] ])).normalized(); - for(int vertex_index = 0; vertex_index < 3; vertex_index++) - if(!temp_normals[vertex_index].isApprox(temp_normals[(vertex_index+1)%3])) - { + for(int vertex_index = 0; vertex_index < 3; vertex_index++) + if(!temp_normals[vertex_index].isApprox(temp_normals[(vertex_index+1)%3])) + { cout << "error, part_normals are not equal, probably the triangle is degenerate."<< endl; - cout << "normal 1 " << endl << temp_normals[vertex_index] << endl; - cout << "normal 2 " << endl << temp_normals[(vertex_index+1)%3] << endl; - exit(-1); - } + cout << "normal 1 " << endl << temp_normals[vertex_index] << endl; + cout << "normal 2 " << endl << temp_normals[(vertex_index+1)%3] << endl; + exit(-1); + } part_normals[triangle_index] = temp_normals[0]; - } + } normals_.push_back(part_normals); - } + } } RigidBodyRenderer::~RigidBodyRenderer() {} @@ -88,125 +112,125 @@ void RigidBodyRenderer::Render( Matrix camera_matrix, int n_cols, std::vector& depth_image) const { - Matrix3d inv_camera_matrix = camera_matrix.inverse(); + Matrix3d inv_camera_matrix = camera_matrix.inverse(); - // we project all the points into image space -------------------------------------------------------- + // we project all the points into image space -------------------------------------------------------- vector > trans_vertices(vertices_.size()); vector > image_vertices(vertices_.size()); for(int part_index = 0; part_index < int(vertices_.size()); part_index++) - { + { image_vertices[part_index].resize(vertices_[part_index].size()); trans_vertices[part_index].resize(vertices_[part_index].size()); for(int point_index = 0; point_index < int(vertices_[part_index].size()); point_index++) - { + { trans_vertices[part_index][point_index] = R_[part_index] * vertices_[part_index][point_index] + t_[part_index]; image_vertices[part_index][point_index] = (camera_matrix * trans_vertices[part_index][point_index]/trans_vertices[part_index][point_index](2)).topRows(2); - } - } + } + } - // we find the intersections with the triangles and the depths --------------------------------------------------- + // we find the intersections with the triangles and the depths --------------------------------------------------- depth_image = vector(n_rows*n_cols, numeric_limits::infinity()); for(int part_index = 0; part_index < int(indices_.size()); part_index++) - { + { for(int triangle_index = 0; triangle_index < int(indices_[part_index].size()); triangle_index++) - { - vector vertices(3); - Vector2d center(Vector2d::Zero()); - - // find the min and max indices to be checked ------------------------------------------------------------ - int min_row = numeric_limits::max(); - int max_row = -numeric_limits::max(); - int min_col = numeric_limits::max(); - int max_col = -numeric_limits::max(); + { + vector vertices(3); + Vector2d center(Vector2d::Zero()); + + // find the min and max indices to be checked ------------------------------------------------------------ + int min_row = numeric_limits::max(); + int max_row = -numeric_limits::max(); + int min_col = numeric_limits::max(); + int max_col = -numeric_limits::max(); bool behind_camera = false; - for(int i = 0; i < 3; i++) - { + for(int i = 0; i < 3; i++) + { vertices[i] = image_vertices[part_index][indices_[part_index][triangle_index][i]]; - center += vertices[i]/3.; - min_row = ceil(float(vertices[i](1))) < min_row ? ceil(float(vertices[i](1))) : min_row; - max_row = floor(float(vertices[i](1))) > max_row ? floor(float(vertices[i](1))) : max_row; - min_col = ceil(float(vertices[i](0))) < min_col ? ceil(float(vertices[i](0))) : min_col; - max_col = floor(float(vertices[i](0))) > max_col ? floor(float(vertices[i](0))) : max_col; + center += vertices[i]/3.; + min_row = ceil(float(vertices[i](1))) < min_row ? ceil(float(vertices[i](1))) : min_row; + max_row = floor(float(vertices[i](1))) > max_row ? floor(float(vertices[i](1))) : max_row; + min_col = ceil(float(vertices[i](0))) < min_col ? ceil(float(vertices[i](0))) : min_col; + max_col = floor(float(vertices[i](0))) > max_col ? floor(float(vertices[i](0))) : max_col; // how should this be handled properly? for now if some vertex in a triangle comes to lie behind camera // we just discard that triangle. if(trans_vertices[part_index][indices_[part_index][triangle_index][i]](2) < 0.001) behind_camera = true; - } + } if(behind_camera) continue; - // make sure all of them are inside of image ----------------------------------------------------------------- - min_row = min_row >= 0 ? min_row : 0; - max_row = max_row < n_rows ? max_row : (n_rows - 1); - min_col = min_col >= 0 ? min_col : 0; - max_col = max_col < n_cols ? max_col : (n_cols - 1); - - - // check whether triangle is inside image ---------------------------------------------------------------------- - if(max_row < 0 || min_row >= n_rows || max_col < 0 || min_col >= n_cols || max_row < min_row || max_col < min_col) - continue; - - // we find the line params of the triangle sides --------------------------------------------------------------- - vector slopes(3); - vector boundary_type(3); const bool upper = true; const bool lower = false; - - for(int i = 0; i < 3; i++) - { - Vector2d side = vertices[(i+1)%3]-vertices[i]; - slopes[i] = side(1)/side(0); - - // we determine whether the line limits the triangle on top or on the bottom - if(vertices[i](1) + slopes[i]*(center(0)-vertices[i](0)) > center(1)) boundary_type[i] = upper; - else boundary_type[i] = lower; - } - - if(boundary_type[0] == boundary_type[1] && boundary_type[0] == boundary_type[2]) //if triangle is degenerate we continue - continue; - - for(int col = min_col; col <= max_col; col++) - { - float min_row_given_col = -numeric_limits::max(); - float max_row_given_col = numeric_limits::max(); - - // the min_row is the max lower boundary at that column, and the max_row is the min upper boundary at that column - for(int i = 0; i < 3; i++) - { - if(boundary_type[i] == lower) - { - float lowe_Rboundary = ceil(float(vertices[i](1) + slopes[i]*(float(col)-vertices[i](0))) ); - min_row_given_col = lowe_Rboundary > min_row_given_col ? lowe_Rboundary : min_row_given_col; - } - else - { - float upper_boundary = floor(float(vertices[i](1) + slopes[i]*(float(col)-vertices[i](0)) )); - max_row_given_col = upper_boundary < max_row_given_col ? upper_boundary : max_row_given_col; - } - } - - - // we push back the indices of the intersections and the corresponding depths ------------------------------------ + // make sure all of them are inside of image ----------------------------------------------------------------- + min_row = min_row >= 0 ? min_row : 0; + max_row = max_row < n_rows ? max_row : (n_rows - 1); + min_col = min_col >= 0 ? min_col : 0; + max_col = max_col < n_cols ? max_col : (n_cols - 1); + + + // check whether triangle is inside image ---------------------------------------------------------------------- + if(max_row < 0 || min_row >= n_rows || max_col < 0 || min_col >= n_cols || max_row < min_row || max_col < min_col) + continue; + + // we find the line params of the triangle sides --------------------------------------------------------------- + vector slopes(3); + vector boundary_type(3); const bool upper = true; const bool lower = false; + + for(int i = 0; i < 3; i++) + { + Vector2d side = vertices[(i+1)%3]-vertices[i]; + slopes[i] = side(1)/side(0); + + // we determine whether the line limits the triangle on top or on the bottom + if(vertices[i](1) + slopes[i]*(center(0)-vertices[i](0)) > center(1)) boundary_type[i] = upper; + else boundary_type[i] = lower; + } + + if(boundary_type[0] == boundary_type[1] && boundary_type[0] == boundary_type[2]) //if triangle is degenerate we continue + continue; + + for(int col = min_col; col <= max_col; col++) + { + float min_row_given_col = -numeric_limits::max(); + float max_row_given_col = numeric_limits::max(); + + // the min_row is the max lower boundary at that column, and the max_row is the min upper boundary at that column + for(int i = 0; i < 3; i++) + { + if(boundary_type[i] == lower) + { + float lowe_Rboundary = ceil(float(vertices[i](1) + slopes[i]*(float(col)-vertices[i](0))) ); + min_row_given_col = lowe_Rboundary > min_row_given_col ? lowe_Rboundary : min_row_given_col; + } + else + { + float upper_boundary = floor(float(vertices[i](1) + slopes[i]*(float(col)-vertices[i](0)) )); + max_row_given_col = upper_boundary < max_row_given_col ? upper_boundary : max_row_given_col; + } + } + + + // we push back the indices of the intersections and the corresponding depths ------------------------------------ Vector3d normal = R_[part_index]*normals_[part_index][triangle_index]; float offset = normal.dot(trans_vertices[part_index][indices_[part_index][triangle_index][0]]); - for(int row = int(min_row_given_col); row <= int(max_row_given_col); row++) - if(row >= 0 && row < n_rows && col >= 0 && col < n_cols) + for(int row = int(min_row_given_col); row <= int(max_row_given_col); row++) + if(row >= 0 && row < n_rows && col >= 0 && col < n_cols) { - // intersec_tindices.push_back(row*n_cols + col); - // we find the intersection between the ray and the triangle -------------------------------------------- - Vector3d line_vector = inv_camera_matrix * Vector3d(col, row, 1); // the depth is the z component + // intersec_tindices.push_back(row*n_cols + col); + // we find the intersection between the ray and the triangle -------------------------------------------- + Vector3d line_vector = inv_camera_matrix * Vector3d(col, row, 1); // the depth is the z component float depth = std::fabs(offset/normal.dot(line_vector)); - //if(depth > 0.5) - depth_image[row*n_cols + col] = + //if(depth > 0.5) + depth_image[row*n_cols + col] = depth < depth_image[row*n_cols + col] ? depth : depth_image[row*n_cols + col]; } - } - } - } + } + } + } } @@ -244,20 +268,29 @@ void RigidBodyRenderer::Render(Matrix camera_matrix, } +void RigidBodyRenderer::Render(std::vector& depth_image) const +{ + assert(!camera_matrix_.isZero()); + assert(n_rows_ > 0); + assert(n_cols_ > 0); + + Render(camera_matrix_, n_rows_, n_cols_, depth_image); +} + std::vector > - RigidBodyRenderer::vertices() const +RigidBodyRenderer::vertices() const { vector > trans_vertices(vertices_.size()); for(int o = 0; o < int(vertices_.size()); o++) - { + { trans_vertices[o].resize(vertices_[o].size()); for(int p = 0; p < int(vertices_[o].size()); p++) { trans_vertices[o][p] = R_[o] * vertices_[o][p] + t_[o]; } - } - return trans_vertices; + } + return trans_vertices; } @@ -267,6 +300,7 @@ void RigidBodyRenderer::set_poses(const std::vector& rotations, R_ = rotations; t_ = translations; } + void RigidBodyRenderer::set_poses(const std::vector& poses) { R_.resize(poses.size()); @@ -278,6 +312,14 @@ void RigidBodyRenderer::set_poses(const std::vector& poses) } } +void RigidBodyRenderer::parameters(Matrix camera_matrix, int n_rows, int n_cols) +{ + camera_matrix_ = camera_matrix; + n_rows_ = n_rows; + n_cols_ = n_cols; +} + + // test the enchilada From d0c3260846e92bdb93c850908a974c29d5732e6a Mon Sep 17 00:00:00 2001 From: issac Date: Sun, 6 Sep 2015 15:38:16 +0200 Subject: [PATCH 046/131] Added fast pose functor for which can be used in stl's containers --- include/dbot/utils/pose_hashing.hpp | 58 +++++++++++++++++++++++++++++ 1 file changed, 58 insertions(+) create mode 100644 include/dbot/utils/pose_hashing.hpp diff --git a/include/dbot/utils/pose_hashing.hpp b/include/dbot/utils/pose_hashing.hpp new file mode 100644 index 0000000..d68cc82 --- /dev/null +++ b/include/dbot/utils/pose_hashing.hpp @@ -0,0 +1,58 @@ +/* + * This is part of the FL library, a C++ Bayesian filtering library + * (https://github.com/filtering-library) + * + * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com) + * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com) + * + * Max-Planck Institute for Intelligent Systems, AMD Lab + * University of Southern California, CLMC Lab + * + * This Source Code Form is subject to the terms of the MIT License (MIT). + * A copy of the license can be found in the LICENSE file distributed with this + * source code. + */ + +/** + * \date January 2015 + * \author Jan Issac (jan.issac@gmail.com) + */ + +#pragma once + + +#include +#include + +#include +#include + +template class PoseHash; + +template <> class PoseHash +{ +public: + std::size_t operator()(const fl::PoseVector & s) const + { + /* primes */ + static constexpr int p1 = 15487457; + static constexpr int p2 = 24092821; + static constexpr int p3 = 73856093; + static constexpr int p4 = 19349663; + static constexpr int p5 = 83492791; + static constexpr int p6 = 17353159; + + /* map size */ + static constexpr int n = 1200; + + /* precision */ + static constexpr int c = 1000000; + + return ((int(s(0, 0) * c) * p1) ^ + (int(s(1, 0) * c) * p2) ^ + (int(s(2, 0) * c) * p3) ^ + (int(s(3, 0) * c) * p4) ^ + (int(s(4, 0) * c) * p5) ^ + (int(s(5, 0) * c) * p6) % n); + } +}; From 1cc6742f67510c1ee833befc46c373345d0b3628 Mon Sep 17 00:00:00 2001 From: issac Date: Sun, 6 Sep 2015 15:38:50 +0200 Subject: [PATCH 047/131] namespace change ff -> dbot --- .../kinect_image_observation_model_cpu.hpp | 20 +++++++++---------- .../kinect_image_observation_model_gpu.hpp | 2 +- .../kinect_pixel_observation_model.hpp | 2 +- .../rao_blackwell_observation_model.hpp | 2 +- .../brownian_object_motion_model.hpp | 4 ++-- .../damped_wiener_process_model.hpp | 2 +- ...integrated_damped_wiener_process_model.hpp | 2 +- .../occlusion_process_model.hpp | 2 +- ...o_blackwell_coordinate_particle_filter.hpp | 2 +- .../free_floating_rigid_bodies_state.hpp | 9 ++++++--- include/dbot/states/rigid_bodies_state.hpp | 2 +- include/dbot/utils/helper_functions.hpp | 4 ++-- include/dbot/utils/traits.hpp | 2 +- 13 files changed, 29 insertions(+), 26 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index baf1c0f..d7de0de 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -42,7 +42,7 @@ along with this program. If not, see . #include #include -namespace ff +namespace dbot { @@ -63,9 +63,9 @@ struct Traits > typedef RBObservationModel ObservationModelBase; - typedef boost::shared_ptr ObjectRendererPtr; - typedef boost::shared_ptr PixelObservationModelPtr; - typedef boost::shared_ptr OcclusionProcessModelPtr; + typedef boost::shared_ptr ObjectRendererPtr; + typedef boost::shared_ptr PixelObservationModelPtr; + typedef boost::shared_ptr OcclusionProcessModelPtr; }; } @@ -97,10 +97,10 @@ class KinectImageObservationModelCPU: // TODO: DO WE NEED ALL OF THIS IN THE CONSTRUCTOR?? KinectImageObservationModelCPU( - const Eigen::Matrix3d& camera_matrix, - const size_t& n_rows, - const size_t& n_cols, - const size_t& max_sample_count, + const Eigen::Matrix3d& camera_matrix, + const size_t& n_rows, + const size_t& n_cols, + const size_t& max_sample_count, const ObjectRendererPtr object_renderer, const PixelObservationModelPtr observation_model, const OcclusionProcessModelPtr occlusion_process_model, @@ -268,8 +268,8 @@ class KinectImageObservationModelCPU: std::vector > occlusion_times_; // observed data - std::vector observations_; - double observation_time_; + std::vector observations_; + double observation_time_; }; diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index b336eb2..c066768 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -20,7 +20,7 @@ #include -namespace ff +namespace dbot { // Forward declarations diff --git a/include/dbot/models/observation_models/kinect_pixel_observation_model.hpp b/include/dbot/models/observation_models/kinect_pixel_observation_model.hpp index e4e841d..9bc039f 100644 --- a/include/dbot/models/observation_models/kinect_pixel_observation_model.hpp +++ b/include/dbot/models/observation_models/kinect_pixel_observation_model.hpp @@ -40,7 +40,7 @@ along with this program. If not, see . //#include #include -namespace ff +namespace dbot { // Forward declarations diff --git a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp index 727a6db..fc689e7 100644 --- a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp +++ b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp @@ -40,7 +40,7 @@ along with this program. If not, see . -namespace ff +namespace dbot { diff --git a/include/dbot/models/process_models/brownian_object_motion_model.hpp b/include/dbot/models/process_models/brownian_object_motion_model.hpp index 14b55cf..302db17 100644 --- a/include/dbot/models/process_models/brownian_object_motion_model.hpp +++ b/include/dbot/models/process_models/brownian_object_motion_model.hpp @@ -57,7 +57,7 @@ #include -namespace ff +namespace dbot { // Forward declarations @@ -196,7 +196,7 @@ class BrownianObjectMotionModel: state_ = state; for(size_t i = 0; i < state_.count(); i++) { - quaternion_map_[i] = ff::hf::QuaternionMatrix(state_.component(i).orientation().quaternion().coeffs()); + quaternion_map_[i] = dbot::hf::QuaternionMatrix(state_.component(i).orientation().quaternion().coeffs()); // transform the state, which is the pose and velocity with respect to to the origin, // into internal representation, which is the position and velocity of the center diff --git a/include/dbot/models/process_models/damped_wiener_process_model.hpp b/include/dbot/models/process_models/damped_wiener_process_model.hpp index 55e002a..3979e08 100644 --- a/include/dbot/models/process_models/damped_wiener_process_model.hpp +++ b/include/dbot/models/process_models/damped_wiener_process_model.hpp @@ -54,7 +54,7 @@ //#include #include -namespace ff +namespace dbot { // Forward declarations diff --git a/include/dbot/models/process_models/integrated_damped_wiener_process_model.hpp b/include/dbot/models/process_models/integrated_damped_wiener_process_model.hpp index be56658..5d52494 100644 --- a/include/dbot/models/process_models/integrated_damped_wiener_process_model.hpp +++ b/include/dbot/models/process_models/integrated_damped_wiener_process_model.hpp @@ -59,7 +59,7 @@ //TODO: THIS IS A LINEAR GAUSSIAN PROCESS, THIS CLASS SHOULD DISAPPEAR -namespace ff +namespace dbot { // Forward declarations diff --git a/include/dbot/models/process_models/occlusion_process_model.hpp b/include/dbot/models/process_models/occlusion_process_model.hpp index e9ee127..8a48818 100644 --- a/include/dbot/models/process_models/occlusion_process_model.hpp +++ b/include/dbot/models/process_models/occlusion_process_model.hpp @@ -32,7 +32,7 @@ along with this program. If not, see . //#include // TODO: THIS IS JUST A LINEAR GAUSSIAN PROCESS WITH NO NOISE, SHOULD DISAPPEAR -namespace ff +namespace dbot { /** diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index 70dd73e..45dbcf2 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -47,7 +47,7 @@ along with this program. If not, see . -namespace ff +namespace dbot { template diff --git a/include/dbot/states/free_floating_rigid_bodies_state.hpp b/include/dbot/states/free_floating_rigid_bodies_state.hpp index ba89e24..78eaea8 100644 --- a/include/dbot/states/free_floating_rigid_bodies_state.hpp +++ b/include/dbot/states/free_floating_rigid_bodies_state.hpp @@ -37,7 +37,7 @@ along with this program. If not, see . #include -namespace ff +namespace dbot { template @@ -91,13 +91,16 @@ class FreeFloatingRigidBodiesState: virtual Poses poses() const { Poses poses_(count()*POSE_SIZE); - for(size_t body_index = 0; body_index < count(); body_index++) + for(int body_index = 0; body_index < count(); body_index++) { poses_.template middleRows(body_index * POSE_SIZE) = component(body_index).pose(); } return poses_; } + + + int count() const { return this->size() / PoseVelocityBlock::SizeAtCompileTime; @@ -111,7 +114,7 @@ class FreeFloatingRigidBodiesState: } virtual void poses(const Poses& poses_) { - for(size_t body_index = 0; body_index < count(); body_index++) + for(int body_index = 0; body_index < count(); body_index++) { component(body_index).pose() = poses_.template middleRows(body_index * POSE_SIZE); diff --git a/include/dbot/states/rigid_bodies_state.hpp b/include/dbot/states/rigid_bodies_state.hpp index a0249ec..1bf71d6 100644 --- a/include/dbot/states/rigid_bodies_state.hpp +++ b/include/dbot/states/rigid_bodies_state.hpp @@ -32,7 +32,7 @@ along with this program. If not, see . #include #include -namespace ff +namespace dbot { template diff --git a/include/dbot/utils/helper_functions.hpp b/include/dbot/utils/helper_functions.hpp index ae9b475..1bf900f 100644 --- a/include/dbot/utils/helper_functions.hpp +++ b/include/dbot/utils/helper_functions.hpp @@ -51,7 +51,7 @@ along with this program. If not, see . // TODO: THIS HAS TO BE CLEANED, POSSIBLY SPLIT INTO SEVERAL FILES -namespace ff +namespace dbot { namespace hf @@ -995,7 +995,7 @@ class DiscreteSampler fibo_.seed(RANDOM_SEED); // compute the prob and normalize them - sorted_indices_ = ff::hf::SortDescend(log_prob); + sorted_indices_ = dbot::hf::SortDescend(log_prob); double max = log_prob[sorted_indices_[0]]; for(int i = 0; i < int(log_prob.size()); i++) log_prob[i] -= max; diff --git a/include/dbot/utils/traits.hpp b/include/dbot/utils/traits.hpp index 0b44bcd..4d4a6df 100644 --- a/include/dbot/utils/traits.hpp +++ b/include/dbot/utils/traits.hpp @@ -48,7 +48,7 @@ #include -namespace ff +namespace dbot { namespace internal From 5bdef8b2fc1aeca2bb55dff0be012a24571d221c Mon Sep 17 00:00:00 2001 From: issac Date: Sun, 6 Sep 2015 15:39:12 +0200 Subject: [PATCH 048/131] Implemented depth pixel model --- .../depth_pixel_observation_model.hpp | 196 ++++++++++++++++++ 1 file changed, 196 insertions(+) create mode 100644 include/dbot/models/observation_models/depth_pixel_observation_model.hpp diff --git a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp new file mode 100644 index 0000000..b8ddd2c --- /dev/null +++ b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp @@ -0,0 +1,196 @@ +/* + * This is part of the FL library, a C++ Bayesian filtering library + * (https://github.com/filtering-library) + * + * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com) + * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com) + * + * Max-Planck Institute for Intelligent Systems, AMD Lab + * University of Southern California, CLMC Lab + * + * This Source Code Form is subject to the terms of the MIT License (MIT). + * A copy of the license can be found in the LICENSE file distributed with this + * source code. + */ + +/** + * \file depth_pixel_observation_model.hpp + * \date September 2015 + * \author Jan Issac (jan.issac@gmail.com) + */ + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace fl +{ + +template +class DepthPixelObservationModel + : public ObservationFunction, + public ObservationDensity, + public Descriptor +{ +public: + typedef Vector1d Obsrv; + typedef Vector1d Noise; + typedef State_ State; + +public: + DepthPixelObservationModel( + const std::shared_ptr& renderer, + Real bg_depth, + Real fg_sigma, + Real bg_sigma, + int state_dim = DimensionOf::Value) + : state_dim_(state_dim), + renderer_(renderer), + id_(0) + { + // setup backgroud density which is N(y| bg_mean, ) + auto bg_mean = Obsrv(); + bg_mean(0) += bg_depth; + bg_density_.mean(bg_mean); + bg_density_.square_root(bg_density_.square_root() * bg_sigma); + + fg_density_.square_root(fg_density_.square_root() * fg_sigma); + } + + Real log_probability(const Obsrv& obsrv, const State& state) const override + { + return density(state).log_probability(obsrv); + } + + Real probability(const Obsrv& obsrv, const State& state) const override + { + return density(state).probability(obsrv); + } + + Obsrv observation(const State& state, const Noise& noise) const override + { + Obsrv y = density(state).map_standard_normal(noise); + return y; + } + + virtual int obsrv_dimension() const { return 1; } + virtual int noise_dimension() const { return 1; } + virtual int state_dimension() const { return state_dim_; } + + virtual int id() const { return id_; } + virtual void id(int new_id) { id_ = new_id; } + + void nominal_pose(const PoseVector& p) + { + render_cache_.clear(); + nominal_pose_= p; + } + + virtual std::string name() const + { + return "DepthPixelObservationModel"; + } + + virtual std::string description() const + { + return "DepthPixelObservationModel"; + } + +private: + /** \cond internal */ + void map(const PoseVector& pose, Eigen::VectorXd& obsrv_image) const + { + renderer_->set_poses({pose.affine()}); + renderer_->Render(depth_rendering_); + + convert(depth_rendering_, obsrv_image); + } + + void convert( + const std::vector& depth, + Eigen::VectorXd& obsrv_image) const + { + const int pixel_count = depth.size(); + obsrv_image.resize(pixel_count, 1); + + for (int i = 0; i < pixel_count; ++i) + { + if (!std::isinf(depth[i])) + { + obsrv_image(i, 0) = depth[i]; + } + else + { + obsrv_image(i, 0) = std::numeric_limits::infinity(); + } + } + } + + const Gaussian& density(const State& state) const + { + Obsrv y = depth(state); + + if (std::isinf(y(0))) + { + return bg_density_; + } + + fg_density_.mean(y); + return fg_density_; + } + + Obsrv depth(const State& current_state) const + { + if (render_cache_.find(current_state) == render_cache_.end()) + { + PoseVector current_pose; + current_pose.orientation() = + current_state.orientation() * nominal_pose_.orientation(); + + current_pose.position() = + current_state.position() + nominal_pose_.position(); + + map(current_pose, render_cache_[current_state]); + } + + assert (render_cache_.find(current_state) != render_cache_.end()); + + Obsrv depth; + depth(0) = render_cache_[current_state](id_); + + return depth; + } + /** \endcond */ + +public: + int state_dim_; + + mutable Gaussian fg_density_; + mutable Gaussian bg_density_; + + mutable std::vector depth_rendering_; + std::shared_ptr renderer_; + + mutable std::unordered_map< + PoseVector, + Eigen::VectorXd, + PoseHash + > render_cache_; + +public: + int id_; + mutable PoseVector nominal_pose_; +}; + +} From e416864da2eb2cd86950a6526a026686474129f2 Mon Sep 17 00:00:00 2001 From: issac Date: Sun, 6 Sep 2015 15:39:27 +0200 Subject: [PATCH 049/131] implemented uniform depth pixel model --- .../uniform_depth_pixel_observation_model.hpp | 92 +++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 include/dbot/models/observation_models/uniform_depth_pixel_observation_model.hpp diff --git a/include/dbot/models/observation_models/uniform_depth_pixel_observation_model.hpp b/include/dbot/models/observation_models/uniform_depth_pixel_observation_model.hpp new file mode 100644 index 0000000..d0ee7d1 --- /dev/null +++ b/include/dbot/models/observation_models/uniform_depth_pixel_observation_model.hpp @@ -0,0 +1,92 @@ +/* + * This is part of the FL library, a C++ Bayesian filtering library + * (https://github.com/filtering-library) + * + * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com) + * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com) + * + * Max-Planck Institute for Intelligent Systems, AMD Lab + * University of Southern California, CLMC Lab + * + * This Source Code Form is subject to the terms of the MIT License (MIT). + * A copy of the license can be found in the LICENSE file distributed with this + * source code. + */ + +/** + * \file uniform_depth_pixel_observation_model.hpp + * \date September 2015 + * \author Jan Issac (jan.issac@gmail.com) + */ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace fl +{ + +template +class UniformDepthPixelObservationModel + : public ObservationFunction, + public ObservationDensity, + public Descriptor +{ +public: + typedef Vector1d Obsrv; + typedef Vector1d Noise; + typedef State_ State; + +public: + UniformDepthPixelObservationModel( + Real min_depth, + Real max_depth, + int state_dim = DimensionOf::Value) + : state_dim_(state_dim), + density_(min_depth, max_depth) + { } + + Real log_probability(const Obsrv& obsrv, const State& state) const override + { + return density_.log_probability(obsrv); + } + + Real probability(const Obsrv& obsrv, const State& state) const override + { + return density_.probability(obsrv); + } + + Obsrv observation(const State& state, const Noise& noise) const override + { + Obsrv y = density_.map_standard_normal(noise); + return y; + } + + virtual int obsrv_dimension() const { return 1; } + virtual int noise_dimension() const { return 1; } + virtual int state_dimension() const { return state_dim_; } + + virtual std::string name() const + { + return "UniformDepthPixelObservationModel"; + } + + virtual std::string description() const + { + return "UniformDepthPixelObservationModel"; + } + + +private: + int state_dim_; + UniformDistribution density_; +}; + +} From 050b90ddbb197960e799114df36346a864a18619 Mon Sep 17 00:00:00 2001 From: manuel Date: Tue, 8 Sep 2015 18:55:34 +0200 Subject: [PATCH 050/131] change from ff to dbot namespace --- .../cuda_opengl_filter.cpp | 2 +- .../cuda_opengl_multiple_filter.cpp | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.cpp b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.cpp index 15e7b0b..685db15 100644 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.cpp +++ b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.cpp @@ -288,7 +288,7 @@ void CudaOpenglFilter::Evaluate( vector CudaOpenglFilter::Resample() { vector resampling_indices; - ff::hf::DiscreteSampler sampler(log_likelihoods_); + dbot::hf::DiscreteSampler sampler(log_likelihoods_); for (int i = 0; i < n_poses_; i++) { resampling_indices.push_back(sampler.Sample()); diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.cpp b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.cpp index 816da1e..c23180b 100644 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.cpp +++ b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.cpp @@ -36,34 +36,34 @@ void CudaOpenglMultipleFilter::EvaluateMultiple(const vector cuda_times; vector cpu_times; cudaEventRecord(start_event); - start = ff::hf::get_wall_time(); + start = dbot::hf::get_wall_time(); #endif cuda_->set_prev_sample_indices(prev_sample_indices.data()); #ifdef PROFILING_ACTIVE - stop = ff::hf::get_wall_time(); + stop = dbot::hf::get_wall_time(); cpu_times.push_back(stop - start); cudaEventRecord(stop_event); cudaEventSynchronize(stop_event); cudaEventElapsedTime(&milliseconds, start_event, stop_event); cuda_times.push_back(milliseconds); cudaEventRecord(start_event); - start = ff::hf::get_wall_time(); + start = dbot::hf::get_wall_time(); #endif opengl_->Render(states); #ifdef PROFILING_ACTIVE - stop = ff::hf::get_wall_time(); + stop = dbot::hf::get_wall_time(); cpu_times.push_back(stop - start); cudaEventRecord(stop_event); cudaEventSynchronize(stop_event); cudaEventElapsedTime(&milliseconds, start_event, stop_event); cuda_times.push_back(milliseconds); cudaEventRecord(start_event); - start = ff::hf::get_wall_time(); + start = dbot::hf::get_wall_time(); #endif // std::vector > intersect_indices; @@ -86,14 +86,14 @@ void CudaOpenglMultipleFilter::EvaluateMultiple(const vectorCompare(observation_time, false, log_likelihoods); // cuda_->CompareMultiple(observation_time, false, update, log_likelihoods); @@ -102,7 +102,7 @@ void CudaOpenglMultipleFilter::EvaluateMultiple(const vector Date: Wed, 9 Sep 2015 13:42:21 +0200 Subject: [PATCH 051/131] Added commented implementation of UniformDistribution tail density --- .../depth_pixel_observation_model.hpp | 49 ++++++++++++++++--- 1 file changed, 41 insertions(+), 8 deletions(-) diff --git a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp index b8ddd2c..1ae2412 100644 --- a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp +++ b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -56,6 +57,7 @@ class DepthPixelObservationModel Real bg_sigma, int state_dim = DimensionOf::Value) : state_dim_(state_dim), +// bg_density_(-bg_sigma + bg_depth, bg_sigma + bg_depth), renderer_(renderer), id_(0) { @@ -70,11 +72,13 @@ class DepthPixelObservationModel Real log_probability(const Obsrv& obsrv, const State& state) const override { +// return density_evaluation(state).log_probability(obsrv); return density(state).log_probability(obsrv); } Real probability(const Obsrv& obsrv, const State& state) const override { +// return density_evaluation(state).probability(obsrv); return density(state).probability(obsrv); } @@ -137,18 +141,46 @@ class DepthPixelObservationModel } } - const Gaussian& density(const State& state) const - { - Obsrv y = depth(state); +// const StandardGaussianMapping& density(const State& state) const +// { +// Obsrv y = depth(state); + +// if (std::isinf(y(0))) +// { +// return bg_density_; +// } + +// fg_density_.mean(y); +// return fg_density_; +// } + +// const Evaluation& density_evaluation(const State& state) const +// { +// Obsrv y = depth(state); + +// if (std::isinf(y(0))) +// { +// return bg_density_; +// } + +// fg_density_.mean(y); +// return fg_density_; +// } - if (std::isinf(y(0))) + + const Gaussian& density(const State& state) const { - return bg_density_; + Obsrv y = depth(state); + + if (std::isinf(y(0))) + { + return bg_density_; + } + + fg_density_.mean(y); + return fg_density_; } - fg_density_.mean(y); - return fg_density_; - } Obsrv depth(const State& current_state) const { @@ -178,6 +210,7 @@ class DepthPixelObservationModel mutable Gaussian fg_density_; mutable Gaussian bg_density_; +// mutable UniformDistribution bg_density_; mutable std::vector depth_rendering_; std::shared_ptr renderer_; From a3763174f752acdf40b67a235cff78a76f22412d Mon Sep 17 00:00:00 2001 From: Claudia Pfreundt Date: Wed, 9 Sep 2015 16:35:22 +0200 Subject: [PATCH 052/131] implemented automatic checks for GPU hardware limitations, such that user is always safe in entering any ridiculous amount of poses or resolutions. If there is a limit, the number of poses is automatically downgraded to the maximum possible amount. This will hopefully work safely on any CUDA graphics card now. --- .../cuda_filter.hpp | 32 +- .../kinect_image_observation_model_gpu.hpp | 127 +++++-- .../object_rasterizer.hpp | 11 +- .../cuda_filter.cu | 330 +++++++++++------- .../object_rasterizer.cpp | 95 +++-- 5 files changed, 400 insertions(+), 195 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp index e0f9d4a..7515d8a 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp @@ -39,9 +39,11 @@ class CudaFilter void set_observations(const float* observations); // not used outside, can be integrated into above void set_visibility_probabilities(const float* visibility_probabilities); void set_prev_sample_indices(const int* prev_sample_indices); - void set_resolution(int n_rows, int n_cols); - void set_number_of_max_poses(int n_poses, int n_poses_x); - void set_number_of_poses(int n_poses, int n_poses_x); + void set_resolution(const int n_rows, const int n_cols, int& nr_poses, int& nr_poses_per_row, int& nr_poses_per_column); + void allocate_memory_for_max_poses(int& allocated_poses, + int& allocated_poses_per_row, + int& allocated_poses_per_column); + void set_number_of_poses(int& nr_poses, int& nr_poses_per_row, int& nr_poses_per_column); void set_texture_array(cudaArray_t texture_array); // getters @@ -55,6 +57,7 @@ class CudaFilter // resolution values if not specified static const int WINDOW_WIDTH = 80; static const int WINDOW_HEIGHT = 60; + static const int DEFAULT_NR_THREADS = 128; // time observation static const int COUNT = 500; @@ -71,7 +74,6 @@ class CudaFilter float *d_log_likelihoods_; int *d_prev_sample_indices_; int *d_resampling_indices_; // not needed if resampling not on GPU - float *d_test_array_; // debugging. can go. curandStateMRG32k3a *d_mrg_states_; // for OpenGL interop @@ -82,20 +84,22 @@ class CudaFilter int n_rows_; // maximum number of poses and their arrangement in the OpenGL texture - int n_max_poses_; - int n_max_poses_x_; - int n_max_poses_y_; + int nr_max_poses_; + int nr_max_poses_per_row_; + int nr_max_poses_per_column_; // number of poses and their arrangement in the OpenGL texture - int n_poses_; - int n_poses_x_; - int n_poses_y_; + int nr_poses_; + int nr_poses_per_row_; + int nr_poses_per_column_; // number of features in a state vector int n_features_; // block and grid arrangement of the CUDA kernels - int n_threads_, n_blocks_; + int nr_threads_, n_blocks_; + dim3 grid_dimension_; + // system properties int warp_size_; @@ -114,8 +118,12 @@ class CudaFilter // booleans to describe the state of the cuda filter, to avoid wrong usage of the class bool n_poses_set_; + // CUDA device properties + cudaDeviceProp cuda_device_properties_; + - void set_default_kernel_config(); + void set_default_kernel_config(int& nr_poses_, int& nr_poses_per_row, int& nr_poses_per_column, + bool& nr_poses_changed); // helper functions template void allocate(T * &pointer, size_t size, std::string name); diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 2a9e507..0c35abd 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -19,6 +19,8 @@ #include #include + +#include #include namespace ff @@ -91,7 +93,7 @@ class KinectImageObservationModelGPU: camera_matrix_(camera_matrix), n_rows_(n_rows), n_cols_(n_cols), - max_sample_count_(max_sample_count), + nr_max_poses_(max_sample_count), indices_(indices), initial_visibility_prob_(1 - initial_occlusion_prob), p_visible_visible_(1.0 - p_occluded_visible), @@ -101,7 +103,7 @@ class KinectImageObservationModelGPU: sigma_factor_(sigma_factor), max_depth_(max_depth), exponential_rate_(exponential_rate), - n_poses_(max_sample_count), + nr_poses_(max_sample_count), observations_set_(false), resource_registered_(false), nr_calls_set_observation_(0), @@ -150,15 +152,58 @@ class KinectImageObservationModelGPU: cuda_ = boost::shared_ptr (new fil::CudaFilter()); - opengl_->set_number_of_max_poses(max_sample_count_); - n_poses_x_ = opengl_->get_n_poses_x(); - cuda_->set_number_of_max_poses(max_sample_count_, n_poses_x_); + // sets the dimensions of how many poses will be rendered per row and per column in a texture + opengl_->allocate_textures_for_max_poses(nr_max_poses_, nr_poses_per_row_, nr_poses_per_column_); + + std::cout << "OpenGL: allocated " << nr_max_poses_ << " poses in the form (" + << nr_poses_per_row_ << ", " << nr_poses_per_column_ << ")" << std::endl; + + int tmp_max_nr_poses = nr_max_poses_; + int tmp_nr_poses_per_row = nr_poses_per_row_; + int tmp_nr_poses_per_column = nr_poses_per_column_; + + cuda_->allocate_memory_for_max_poses(nr_max_poses_, nr_poses_per_row_, nr_poses_per_column_); + + std::cout << "CUDA: allocated " << nr_max_poses_ << " poses in the form (" + << nr_poses_per_row_ << ", " << nr_poses_per_column_ << ")" << std::endl; + + + // if number of poses gets limited by cuda, we have to reallocate the textures in OpenGL again + if (tmp_max_nr_poses != nr_max_poses_ || + tmp_nr_poses_per_row != nr_poses_per_row_ || + tmp_nr_poses_per_column != nr_poses_per_column_) { + opengl_->allocate_textures_for_max_poses(nr_max_poses_, nr_poses_per_row_, nr_poses_per_column_); + + std::cout << "OpenGL adapts to CUDA restrictions: allocated " << nr_max_poses_ << " poses in the form (" + << nr_poses_per_row_ << ", " << nr_poses_per_column_ << ")" << std::endl; + } std:: cout << "set resolution in cuda..." << std::endl; - opengl_->set_resolution(n_rows_, n_cols_); - cuda_->set_resolution(n_rows_, n_cols_); + opengl_->set_resolution(n_rows_, n_cols_, nr_max_poses_, nr_poses_per_row_, nr_poses_per_column_); + + std::cout << "OpenGL: setting resolution changes allocation to " << nr_max_poses_ << " poses in the form (" + << nr_poses_per_row_ << ", " << nr_poses_per_column_ << ")" << std::endl; + + tmp_max_nr_poses = nr_max_poses_; + tmp_nr_poses_per_row = nr_poses_per_row_; + tmp_nr_poses_per_column = nr_poses_per_column_; + + cuda_->set_resolution(n_rows_, n_cols_, nr_max_poses_, nr_poses_per_row_, nr_poses_per_column_); + + std::cout << "CUDA: setting resolution changes allocation to " << nr_max_poses_ << " poses in the form (" + << nr_poses_per_row_ << ", " << nr_poses_per_column_ << ")" << std::endl; + + // if number of poses gets limited by cuda, we have to reallocate the textures in OpenGL again + if (tmp_max_nr_poses != nr_max_poses_ || + tmp_nr_poses_per_row != nr_poses_per_row_ || + tmp_nr_poses_per_column != nr_poses_per_column_) { + opengl_->set_resolution(n_rows_, n_cols_, nr_max_poses_, nr_poses_per_row_, nr_poses_per_column_); + + std::cout << "OpenGL adapts to CUDA restrictions: setting resolution changes allocation to " << nr_max_poses_ << " poses in the form (" + << nr_poses_per_row_ << ", " << nr_poses_per_column_ << ")" << std::endl; + } register_resource(); @@ -176,12 +221,16 @@ class KinectImageObservationModelGPU: count_ = 0; - + render_time_ = 0; visibility_probs_.resize(n_rows_ * n_cols_); } - ~KinectImageObservationModelGPU() { } + ~KinectImageObservationModelGPU() { + + std::cout << "time for render: " << render_time_ / count_ << std::endl; + + } RealArray loglikes(const StateArray& deltas, @@ -195,9 +244,9 @@ class KinectImageObservationModelGPU: exit(-1); } - n_poses_ = deltas.size(); - std::vector flog_likelihoods (n_poses_, 0); - set_number_of_poses(n_poses_); + nr_poses_ = deltas.size(); + std::vector flog_likelihoods (nr_poses_, 0); + set_number_of_poses(nr_poses_); // transform occlusion indices from size_t to int std::vector occlusion_indices_transformed (occlusion_indices.size(), 0); @@ -212,14 +261,14 @@ class KinectImageObservationModelGPU: MEASURE("gpu: setting occlusion indices"); // convert to internal state format std::vector > > poses( - n_poses_, + nr_poses_, std::vector >(vertices_.size(), std::vector(7, 0))); MEASURE("gpu: creating state vectors"); - for(size_t i_state = 0; i_state < size_t(n_poses_); i_state++) + for(size_t i_state = 0; i_state < size_t(nr_poses_); i_state++) { for(size_t i_obj = 0; i_obj < vertices_.size(); i_obj++) { @@ -243,9 +292,16 @@ class KinectImageObservationModelGPU: MEASURE("gpu: converting state format"); + double before_render = ff::hf::get_wall_time(); + opengl_->render(poses); + double after_render = ff::hf::get_wall_time(); + render_time_ += after_render - before_render; + count_++; + + MEASURE("gpu: rendering"); @@ -325,7 +381,7 @@ class KinectImageObservationModelGPU: float default_visibility_probability = visibility_prob; if (visibility_prob == -1) default_visibility_probability = initial_visibility_prob_; - std::vector visibility_probabilities (n_rows_ * n_cols_ * n_poses_, default_visibility_probability); + std::vector visibility_probabilities (n_rows_ * n_cols_ * nr_poses_, default_visibility_probability); cuda_->set_visibility_probabilities(visibility_probabilities.data()); // TODO set update times if you want to use them @@ -335,14 +391,34 @@ class KinectImageObservationModelGPU: const size_t n_rows_; const size_t n_cols_; const float initial_visibility_prob_; - const size_t max_sample_count_; + int nr_max_poses_; - void set_number_of_poses(int n_poses){ + void set_number_of_poses(int nr_poses){ - n_poses_ = n_poses; - opengl_->set_number_of_poses(n_poses_); - n_poses_x_ = opengl_->get_n_poses_x(); - cuda_->set_number_of_poses(n_poses_, n_poses_x_); + nr_poses_ = nr_poses; + opengl_->set_number_of_poses(nr_poses_, nr_poses_per_row_, nr_poses_per_column_); + + std::cout << "OpenGL: set number of poses to " << nr_poses_ << " poses in the form (" + << nr_poses_per_row_ << ", " << nr_poses_per_column_ << ")" << std::endl; + + int tmp_nr_poses = nr_poses_; + int tmp_nr_poses_per_row = nr_poses_per_row_; + int tmp_nr_poses_per_column = nr_poses_per_column_; + + cuda_->set_number_of_poses(nr_poses_, nr_poses_per_row_, nr_poses_per_column_); + + std::cout << "CUDA: set number of poses to " << nr_poses_ << " poses in the form (" + << nr_poses_per_row_ << ", " << nr_poses_per_column_ << ")" << std::endl; + + // if number of poses gets limited by cuda, we have to tell OpenGL about it + if (tmp_nr_poses != nr_poses_ || + tmp_nr_poses_per_row != nr_poses_per_row_ || + tmp_nr_poses_per_column != nr_poses_per_column_) { + opengl_->set_number_of_poses(nr_poses_, nr_poses_per_row_, nr_poses_per_column_); + + std::cout << "OpenGL adapts to CUDA restrictions: set number of poses to " << nr_poses_ << " poses in the form (" + << nr_poses_per_row_ << ", " << nr_poses_per_column_ << ")" << std::endl; + } } @@ -369,7 +445,7 @@ class KinectImageObservationModelGPU: void register_resource() { if (!resource_registered_) { - combined_texture_opengl_ = opengl_->get_combined_texture(); + combined_texture_opengl_ = opengl_->get_framebuffer_texture(); cudaGraphicsGLRegisterImage(&combined_texture_resource_, combined_texture_opengl_, GL_TEXTURE_2D, cudaGraphicsRegisterFlagsReadOnly); check_cuda_error("cudaGraphicsGLRegisterImage)"); resource_registered_ = true; @@ -402,10 +478,11 @@ class KinectImageObservationModelGPU: std::string fragment_shader_path_; - double start_time_; + double render_time_; - int n_poses_; - int n_poses_x_; + int nr_poses_; + int nr_poses_per_row_; + int nr_poses_per_column_; int count_; diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp index 3d0f994..f3746fe 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.hpp @@ -77,12 +77,13 @@ class ObjectRasterizer * @param[in] n_rows the height of the image * @param[in] n_cols the width of the image */ - void set_resolution(const int n_rows, - const int n_cols); + void set_resolution(const int n_rows, const int n_cols, int& nr_poses, int& nr_poses_per_row, int& nr_poses_per_column); - void set_number_of_max_poses(int n_poses); + void allocate_textures_for_max_poses(int& allocated_poses, + int& allocated_poses_per_row, + int& allocated_poses_per_column); - void set_number_of_poses(int n_poses); + void set_number_of_poses(const int nr_poses, int& nr_poses_per_row, int& nr_poses_per_column); GLuint get_framebuffer_texture(); @@ -97,7 +98,7 @@ class ObjectRasterizer static const int WINDOW_HEIGHT = 60; // default values if not specified // GPU constraints - GLint max_dimension_; + GLint max_texture_size_; // values initialized to WINDOW_WIDTH, WINDOW_HEIGHT in constructor. May be changed by user with set_resolution(). int nr_rows_; diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu index 7194f15..fb66033 100644 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu +++ b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu @@ -537,7 +537,7 @@ __global__ void compare_kernel(float *observations, float* visibility_probs, int __global__ void compare_multiple_kernel(float *observations, float* old_visibility_probs, float* new_visibility_probs, int* occlusion_image_indices, int nr_pixels, - float *d_log_likelihoods, float delta_time, int n_poses, int n_rows, int n_cols, bool update, float* test_array) { + float *d_log_likelihoods, float delta_time, int n_poses, int n_rows, int n_cols, bool update) { int block_id = blockIdx.x + blockIdx.y * gridDim.x; if (block_id < n_poses) { @@ -612,7 +612,6 @@ __global__ void compare_multiple_kernel(float *observations, float* old_visibili local_sum_of_likelihoods += logf((p_obsIpred_vis + p_obsIpred_occl)/p_obsIinf); -// test_array[pixel_nr] = depth; if(update) { // we update the visibility (occlusion) probability with the observations @@ -711,7 +710,7 @@ CudaFilter::CudaFilter() : props.major = 2; props.minor = 0; cudaChooseDevice( &device_number, &props ); - check_cuda_error("choosing device"); + check_cuda_error("No device with compute capability > 2.0 found"); /* tell CUDA which device we will be using for graphic interop * requires that the CUDA device be specified by @@ -724,6 +723,8 @@ CudaFilter::CudaFilter() : warp_size_ = props.warpSize; // equals 32 for all current graphics cards, but might change in the future n_mps_ = props.multiProcessorCount; + cuda_device_properties_ = props; + cout << "Your device has the following properties: " << endl << "CUDA Version: " << props.major << "." << props.minor << endl << "Number of multiprocessors: " << n_mps_ << endl @@ -743,7 +744,6 @@ CudaFilter::CudaFilter() : d_mrg_states_ = NULL; d_resampling_indices_ = NULL; d_prev_sample_indices_ = NULL; - d_test_array_ = NULL; } @@ -766,7 +766,7 @@ void CudaFilter::init(vector > com_models, float angle_sigma, floa // allocate(d_prev_sample_indices_, sizeof(int) * n_poses_, "d_prev_sample_indices"); // allocate(d_resampling_indices_, sizeof(int) * n_poses_, "d_resampling_indices"); - cudaMemset(d_log_likelihoods_, 0, sizeof(float) * n_poses_); + cudaMemset(d_log_likelihoods_, 0, sizeof(float) * nr_poses_); #ifdef CHECK_ERRORS check_cuda_error("cudaMemset d_log_likelihoods"); #endif @@ -841,7 +841,7 @@ void CudaFilter::propagate(const float ¤t_time, vector > &st last_propagation_time_ = current_time; - propagate_kernel <<< n_blocks_, n_threads_ >>> (d_states_, n_poses_, n_features_, delta_time, d_mrg_states_); + propagate_kernel <<< n_blocks_, nr_threads_ >>> (d_states_, nr_poses_, n_features_, delta_time, d_mrg_states_); #ifdef CHECK_ERRORS check_cuda_error("propagate kernel call"); #endif @@ -856,14 +856,14 @@ void CudaFilter::propagate(const float ¤t_time, vector > &st - float *states_raw = (float*) malloc(n_poses_ * n_features_ * sizeof(float)); - cudaMemcpy(states_raw, d_states_, n_poses_ * n_features_ * sizeof(float), cudaMemcpyDeviceToHost); + float *states_raw = (float*) malloc(nr_poses_ * n_features_ * sizeof(float)); + cudaMemcpy(states_raw, d_states_, nr_poses_ * n_features_ * sizeof(float), cudaMemcpyDeviceToHost); #ifdef CHECK_ERRORS check_cuda_error("cudaMemcpy d_states -> states"); #endif - for (int i = 0; i < n_poses_; i++) { + for (int i = 0; i < nr_poses_; i++) { for (int j = 0; j < n_features_; j++) { states[i][j] = states_raw[i * n_features_ + j]; } @@ -881,8 +881,8 @@ void CudaFilter::propagate_multiple(const float ¤t_time, vector d_states"); #endif - propagate_multiple_kernel <<< n_blocks_, n_threads_ >>> (d_states_, n_poses_, n_objects, n_features_, delta_time, d_mrg_states_); + propagate_multiple_kernel <<< n_blocks_, nr_threads_ >>> (d_states_, nr_poses_, n_objects, n_features_, delta_time, d_mrg_states_); #ifdef CHECK_ERRORS check_cuda_error("propagate kernel call"); #endif @@ -912,13 +912,13 @@ void CudaFilter::propagate_multiple(const float ¤t_time, vector states"); #endif - for (int i = 0; i < n_poses_; i++) { + for (int i = 0; i < nr_poses_; i++) { for (int j = 0; j < n_objects; j++) { for (int k = 0; k < n_features_; k++) { states[i][j][k] = states_raw[(i * n_objects *n_features_) + j * n_features_ + k]; @@ -941,7 +941,7 @@ void CudaFilter::compare(float observation_time, bool constant_occlusion, vector float milliseconds; #endif - dim3 gridDim = dim3(n_poses_x_, n_poses_y_); + dim3 gridDim = dim3(nr_poses_per_row_, nr_poses_per_column_); // update observation time float delta_time = observation_time - occlusion_time_; @@ -954,7 +954,7 @@ void CudaFilter::compare(float observation_time, bool constant_occlusion, vector #endif compare_kernel <<< gridDim, 128 >>> (d_observations_, d_visibility_probs_, n_cols_ * n_rows_, - constant_occlusion, d_log_likelihoods_, delta_time, n_poses_, n_rows_, n_cols_); + constant_occlusion, d_log_likelihoods_, delta_time, nr_poses_, n_rows_, n_cols_); #ifdef CHECK_ERRORS check_cuda_error("compare kernel call"); #endif @@ -976,7 +976,7 @@ void CudaFilter::compare(float observation_time, bool constant_occlusion, vector cudaEventRecord(start); #endif - cudaMemcpy(&log_likelihoods[0], d_log_likelihoods_, n_poses_ * sizeof(float), cudaMemcpyDeviceToHost); + cudaMemcpy(&log_likelihoods[0], d_log_likelihoods_, nr_poses_ * sizeof(float), cudaMemcpyDeviceToHost); #ifdef CHECK_ERRORS check_cuda_error("cudaMemcpy d_log_likelihoods -> log_likelihoods"); #endif @@ -1006,12 +1006,6 @@ void CudaFilter::compare_multiple(bool update, vector &log_likelihoods) { cudaEventCreate(&start); cudaEventCreate(&stop); float milliseconds; -#endif - - dim3 gridDim = dim3(n_poses_x_, n_poses_y_); - - -#ifdef PROFILING_ACTIVE cudaEventRecord(start); #endif @@ -1019,16 +1013,9 @@ void CudaFilter::compare_multiple(bool update, vector &log_likelihoods) { if(update) occlusion_time_ = observation_time_; // cout << "delta time: " << delta_time << endl; - int TEST_SIZE = n_cols_ * n_rows_; - float* test_array = (float*) malloc( TEST_SIZE * sizeof(float)); - memset(test_array, 0, TEST_SIZE * sizeof(float)); - - - allocate(d_test_array_, TEST_SIZE * sizeof(float), "test_array"); - cudaMemset(d_test_array_, 0, TEST_SIZE * sizeof(float)); - compare_multiple_kernel <<< gridDim, 128 >>> (d_observations_, d_visibility_probs_, d_visibility_probs_copy_, d_prev_sample_indices_, n_cols_ * n_rows_, - d_log_likelihoods_, delta_time, n_poses_, n_rows_, n_cols_, update, d_test_array_); + compare_multiple_kernel <<< grid_dimension_, nr_threads_ >>> (d_observations_, d_visibility_probs_, d_visibility_probs_copy_, d_prev_sample_indices_, n_cols_ * n_rows_, + d_log_likelihoods_, delta_time, nr_poses_, n_rows_, n_cols_, update); #ifdef CHECK_ERRORS check_cuda_error("compare kernel call"); #endif @@ -1038,16 +1025,6 @@ void CudaFilter::compare_multiple(bool update, vector &log_likelihoods) { check_cuda_error("cudaDeviceSynchronize compare_multiple"); #endif - cudaMemcpy(test_array, d_test_array_, TEST_SIZE * sizeof(float), cudaMemcpyDeviceToHost); - #ifdef CHECK_ERRORS - check_cuda_error("cudaMemcpy d_log_likelihoods -> log_likelihoods"); - #endif - -// for (int i = 0; i < TEST_SIZE; i++) { -// if (test_array[i] != 0) { -// cout << "(GPU) index: " << i << ", depth: " << test_array[i] << endl; -// } -// } // switch to new / copied visibility probabilities @@ -1070,7 +1047,7 @@ void CudaFilter::compare_multiple(bool update, vector &log_likelihoods) { cudaEventRecord(start); #endif - cudaMemcpy(&log_likelihoods[0], d_log_likelihoods_, n_poses_ * sizeof(float), cudaMemcpyDeviceToHost); + cudaMemcpy(&log_likelihoods[0], d_log_likelihoods_, nr_poses_ * sizeof(float), cudaMemcpyDeviceToHost); #ifdef CHECK_ERRORS check_cuda_error("cudaMemcpy d_log_likelihoods -> log_likelihoods"); #endif @@ -1101,7 +1078,7 @@ void CudaFilter::resample(vector resampling_indices) { // cout << "resample <<< " << n_poses_ << ", " << 128 << " >>>" << endl; - cudaMemcpy(d_resampling_indices_, &resampling_indices[0], sizeof(int) * n_poses_, cudaMemcpyHostToDevice); + cudaMemcpy(d_resampling_indices_, &resampling_indices[0], sizeof(int) * nr_poses_, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS check_cuda_error("cudaMemcpy resampling_indices -> d_resampling_indices_"); #endif @@ -1118,7 +1095,7 @@ void CudaFilter::resample(vector resampling_indices) { int nr_pixels = n_rows_ * n_cols_; - resample_kernel <<< n_poses_, 128 >>> (d_visibility_probs_, d_visibility_probs_copy_, + resample_kernel <<< nr_poses_, 128 >>> (d_visibility_probs_, d_visibility_probs_copy_, d_states_, d_states_copy_, d_resampling_indices_, nr_pixels, n_features_); #ifdef CHECK_ERRORS @@ -1148,14 +1125,14 @@ void CudaFilter::resample(vector resampling_indices) { void CudaFilter::resample_multiple(vector resampling_indices) { - cudaMemcpy(d_resampling_indices_, &resampling_indices[0], sizeof(int) * n_poses_, cudaMemcpyHostToDevice); + cudaMemcpy(d_resampling_indices_, &resampling_indices[0], sizeof(int) * nr_poses_, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS check_cuda_error("cudaMemcpy resampling_indices -> d_resampling_indices_"); #endif int nr_pixels = n_rows_ * n_cols_; - resample_multiple_kernel <<< n_poses_, 128 >>> (d_visibility_probs_, d_visibility_probs_copy_, + resample_multiple_kernel <<< nr_poses_, 128 >>> (d_visibility_probs_, d_visibility_probs_copy_, d_resampling_indices_, nr_pixels); #ifdef CHECK_ERRORS check_cuda_error("resample kernel call"); @@ -1191,10 +1168,10 @@ void CudaFilter::set_states(std::vector > &states, int seed) * right now, each MP needs 7 values out of d_states_. 8 would be a much better number. */ n_features_ = states[0].size(); - int states_size = n_poses_ * n_features_ * sizeof(float); + int states_size = nr_poses_ * n_features_ * sizeof(float); float *states_raw = (float*) malloc(states_size); - for (size_t i = 0; i < n_poses_; i++) { + for (size_t i = 0; i < nr_poses_; i++) { for (size_t j = 0; j < n_features_; j++) { states_raw[i * n_features_ + j] = states[i][j]; } @@ -1211,9 +1188,9 @@ void CudaFilter::set_states(std::vector > &states, int seed) free(states_raw); // setup random number generators for each thread to be used in the propagate kernel - allocate(d_mrg_states_, n_poses_ * sizeof(curandStateMRG32k3a), "d_mrg_states"); + allocate(d_mrg_states_, nr_poses_ * sizeof(curandStateMRG32k3a), "d_mrg_states"); - setup_number_generators_kernel <<< n_blocks_, n_threads_ >>> (seed, d_mrg_states_, n_poses_); + setup_number_generators_kernel <<< n_blocks_, nr_threads_ >>> (seed, d_mrg_states_, nr_poses_); cudaDeviceSynchronize(); } else { @@ -1232,14 +1209,14 @@ void CudaFilter::set_states_multiple(int n_objects, int n_features, int seed) if (n_poses_set_) { n_features_ = n_features; - int states_size = n_poses_ * n_objects * n_features_ * sizeof(float); + int states_size = nr_poses_ * n_objects * n_features_ * sizeof(float); allocate(d_states_, states_size, "d_states"); // setup random number generators for each thread to be used in the propagate kernel - allocate(d_mrg_states_, n_poses_ * sizeof(curandStateMRG32k3a), "d_mrg_states"); + allocate(d_mrg_states_, nr_poses_ * sizeof(curandStateMRG32k3a), "d_mrg_states"); - setup_number_generators_kernel <<< n_blocks_, n_threads_ >>> (seed, d_mrg_states_, n_poses_); + setup_number_generators_kernel <<< n_blocks_, nr_threads_ >>> (seed, d_mrg_states_, nr_poses_); cudaDeviceSynchronize(); } else { @@ -1272,7 +1249,7 @@ void CudaFilter::set_observations(const float* observations) { void CudaFilter::set_prev_sample_indices(const int* prev_sample_indices) { - cudaMemcpy(d_prev_sample_indices_, prev_sample_indices, n_poses_ * sizeof(int), cudaMemcpyHostToDevice); + cudaMemcpy(d_prev_sample_indices_, prev_sample_indices, nr_poses_ * sizeof(int), cudaMemcpyHostToDevice); // cout << "when setting prev_sample_indices: n_poses: " << n_poses_ << ", max poses: " << n_max_poses_ << endl; #ifdef CHECK_ERRORS check_cuda_error("cudaMemcpy prev_sample_indices -> d_prev_sample_indices_"); @@ -1281,29 +1258,18 @@ void CudaFilter::set_prev_sample_indices(const int* prev_sample_indices) { } -void CudaFilter::set_resolution(int n_rows, int n_cols) { +void CudaFilter::set_resolution(const int n_rows, const int n_cols, int& nr_poses, int& nr_poses_per_row, int& nr_poses_per_column) { n_rows_ = n_rows; n_cols_ = n_cols; // reallocate buffers - allocate(d_observations_, n_cols_ * n_rows_ * sizeof(float), "d_observations"); - allocate(d_visibility_probs_, n_rows_ * n_cols_ * n_max_poses_ * sizeof(float), "d_visibility_probs"); - allocate(d_visibility_probs_copy_, n_rows_ * n_cols_ * n_max_poses_ * sizeof(float), "d_visibility_probs_copy"); - - // fill all pixels of new visibility probs texture with the default value - vector initial_visibility_probs (n_rows_ * n_cols_ * n_max_poses_, visibility_prob_default_); - - cudaMemcpy(d_visibility_probs_, &initial_visibility_probs[0], n_rows_ * n_cols_ * n_max_poses_ * sizeof(float), cudaMemcpyHostToDevice); - #ifdef CHECK_ERRORS - check_cuda_error("cudaMemcpy visibility_prob_default_ -> d_visibility_probs_"); - #endif - - cudaDeviceSynchronize(); + allocate(d_observations_, n_cols_ * n_rows_ * sizeof(float), "d_observations"); + allocate_memory_for_max_poses(nr_poses, nr_poses_per_row, nr_poses_per_column); } void CudaFilter::set_visibility_probabilities(const float* visibility_probabilities) { - cudaMemcpy(d_visibility_probs_, visibility_probabilities, n_rows_ * n_cols_ * n_poses_ * sizeof(float), cudaMemcpyHostToDevice); + cudaMemcpy(d_visibility_probs_, visibility_probabilities, n_rows_ * n_cols_ * nr_poses_ * sizeof(float), cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS check_cuda_error("cudaMemcpy visibility_probabilities -> d_visibility_probs_"); #endif @@ -1312,32 +1278,104 @@ void CudaFilter::set_visibility_probabilities(const float* visibility_probabilit -void CudaFilter::set_number_of_max_poses(int n_poses, int n_poses_x) { - n_max_poses_ = n_poses; - n_max_poses_x_ = n_poses_x; +void CudaFilter::allocate_memory_for_max_poses(int& allocated_poses, + int& allocated_poses_per_row, + int& allocated_poses_per_column) { - // determine n_max_poses_y_ - n_max_poses_y_ = n_max_poses_ / n_max_poses_x_; - if (n_poses % n_max_poses_x_ != 0) n_max_poses_y_++; + // check limitation by global memory + size_t size_of_log_likelihoods = sizeof(float) * allocated_poses; + size_t size_of_resampling_indices = sizeof(int) * allocated_poses; + size_t size_of_prev_sample_indices = sizeof(int) * allocated_poses; + size_t size_of_visibility_probs = n_rows_ * n_cols_ * allocated_poses * sizeof(float); + size_t size_of_opengl_textures = size_of_visibility_probs * 2; + size_t size_of_observations = n_cols_ * n_rows_ * sizeof(float); + + size_t total_size = size_of_log_likelihoods + size_of_resampling_indices + size_of_prev_sample_indices + + size_of_visibility_probs * 2 + size_of_opengl_textures + size_of_observations; + + if (total_size > cuda_device_properties_.totalGlobalMem) { + + std::cout << "The space (" << total_size << " B) for the number of maximum poses you requested (" << allocated_poses << ") cannot be allocated. " + << "The limit is global memory size (" << cuda_device_properties_.totalGlobalMem + << " B) retrieved from CUDA properties." << std::endl; + + size_t size_depending_on_nr_poses = (sizeof(float) + sizeof(int) * 2 + n_rows_ * n_cols_ * sizeof(float) * 4); + allocated_poses = min(allocated_poses, (int) floor((cuda_device_properties_.totalGlobalMem - size_of_observations) / size_depending_on_nr_poses)); + allocated_poses_per_column = ceil(allocated_poses / allocated_poses_per_row); + + std::cout << "Instead, space (" << allocated_poses * size_depending_on_nr_poses + size_of_observations << " B) for " << allocated_poses << " poses was allocated. " << std::endl; + } + + + // check limitation by texture size + if (cuda_device_properties_.maxTexture2D[0] <= allocated_poses_per_row * n_cols_) { + + std::cout << "The max poses you requested (" << allocated_poses << ") could not be allocated." << std::endl; + + allocated_poses_per_row = cuda_device_properties_.maxTexture2D[0] / n_cols_; + allocated_poses_per_column = ceil(allocated_poses / allocated_poses_per_row); + + if (cuda_device_properties_.maxTexture2D[1] <= allocated_poses_per_column * n_rows_) { + allocated_poses_per_column = cuda_device_properties_.maxTexture2D[1] / n_rows_; + } + + allocated_poses = min(allocated_poses, allocated_poses_per_row * allocated_poses_per_column); + + std::cout << "The limit is max texture size (" << cuda_device_properties_.maxTexture2D[0] + << ", " << cuda_device_properties_.maxTexture2D[1] << ") retrieved from CUDA properties. " + << "Number of poses was reduced to (" << allocated_poses_per_row << ", " + << allocated_poses_per_column << "), a total of " << allocated_poses << std::endl; + } + + nr_max_poses_ = allocated_poses; + nr_max_poses_per_row_ = allocated_poses_per_row; + nr_max_poses_per_column_ = allocated_poses_per_column; - n_poses_ = n_max_poses_; - n_poses_x_ = n_max_poses_x_; - n_poses_y_ = n_max_poses_y_; +/* + nr_max_poses_ = n_poses; + nr_max_poses_per_row_ = n_poses_x; + // determine n_max_poses_y_ + nr_max_poses_per_column_ = nr_max_poses_ / nr_max_poses_per_row_; + if (n_poses % nr_max_poses_per_row_ != 0) nr_max_poses_per_column_++; + + n_poses_ = nr_max_poses_; + n_poses_x_ = nr_max_poses_per_row_; + n_poses_y_ = nr_max_poses_per_column_; + +*/ n_poses_set_ = true; - set_default_kernel_config(); + + bool nr_poses_changed = false; + set_default_kernel_config(nr_max_poses_, nr_max_poses_per_row_, nr_max_poses_per_column_, nr_poses_changed); + + allocated_poses = nr_max_poses_; + allocated_poses_per_row = nr_max_poses_per_row_; + allocated_poses_per_column = nr_max_poses_per_column_; + + nr_poses_ = nr_max_poses_; + nr_poses_per_row_ = nr_max_poses_per_row_; + nr_poses_per_column_ = nr_max_poses_per_column_; + + if (nr_poses_changed) { + size_of_log_likelihoods = sizeof(float) * nr_max_poses_; + size_of_resampling_indices = sizeof(int) * nr_max_poses_; + size_of_prev_sample_indices = sizeof(int) * nr_max_poses_; + size_of_visibility_probs = n_rows_ * n_cols_ * nr_max_poses_ * sizeof(float); + } + // reallocate arrays - allocate(d_log_likelihoods_, sizeof(float) * n_max_poses_, "d_log_likelihoods"); - allocate(d_resampling_indices_, sizeof(int) * n_max_poses_, "d_resampling_indices"); - allocate(d_prev_sample_indices_, sizeof(int) * n_max_poses_, "d_prev_sample_indices"); - allocate(d_visibility_probs_, n_rows_ * n_cols_ * n_max_poses_ * sizeof(float), "d_visibility_probs"); - allocate(d_visibility_probs_copy_, n_rows_ * n_cols_ * n_max_poses_ * sizeof(float), "d_visibility_probs_copy"); + allocate(d_log_likelihoods_, size_of_log_likelihoods, "d_log_likelihoods"); + allocate(d_resampling_indices_, size_of_resampling_indices, "d_resampling_indices"); + allocate(d_prev_sample_indices_, size_of_prev_sample_indices, "d_prev_sample_indices"); + allocate(d_visibility_probs_, size_of_visibility_probs, "d_visibility_probs"); + allocate(d_visibility_probs_copy_, size_of_visibility_probs, "d_visibility_probs_copy"); // TODO maybe delete after set_visibility_probabilities is properly in use - vector initial_visibility_probs (n_rows_ * n_cols_ * n_max_poses_, visibility_prob_default_); - cudaMemcpy(d_visibility_probs_, &initial_visibility_probs[0], n_rows_ * n_cols_ * n_max_poses_ * sizeof(float), cudaMemcpyHostToDevice); + vector initial_visibility_probs (n_rows_ * n_cols_ * nr_max_poses_, visibility_prob_default_); + cudaMemcpy(d_visibility_probs_, &initial_visibility_probs[0], size_of_visibility_probs, cudaMemcpyHostToDevice); #ifdef CHECK_ERRORS check_cuda_error("cudaMemcpy visibility_prob_default_ -> d_visibility_probs_"); #endif @@ -1345,48 +1383,101 @@ void CudaFilter::set_number_of_max_poses(int n_poses, int n_poses_x) { cudaDeviceSynchronize(); #ifdef CHECK_ERRORS - check_cuda_error("cudaDeviceSynchronize set_number_of_states"); + check_cuda_error("cudaDeviceSynchronize allocate_memory_for_max_poses"); #endif } -void CudaFilter::set_number_of_poses(int n_poses, int n_poses_x) { - if (n_poses_ <= n_max_poses_) { - n_poses_ = n_poses; - n_poses_x_ = n_poses_x; +void CudaFilter::set_number_of_poses(int& nr_poses, int& nr_poses_per_row, int& nr_poses_per_column) { + if (nr_poses <= nr_max_poses_) { + + if (nr_max_poses_per_row_ < nr_poses_per_row) { + nr_poses_per_row = nr_max_poses_per_row_; + nr_poses_per_column = ceil(nr_poses / nr_poses_per_row); + if (nr_max_poses_per_column_ < nr_poses_per_column) { + nr_poses_per_column = nr_max_poses_per_column_; + } + + std::cout << "Number of poses was reduced to (" << nr_poses_per_row << ", " + << nr_poses_per_column << ") because of the maximum number of poses set in the beginning." << std::endl; + } + + nr_poses = min(nr_poses, nr_poses_per_row * nr_poses_per_column); + + nr_poses_ = nr_poses; + nr_poses_per_row_ = nr_poses_per_row; + nr_poses_per_column_ = nr_poses_per_column; + + /* + + + nr_poses_ = nr_poses; + nr_poses_per_row_ = nr_poses_per_row; // determine n_max_poses_y_ - n_poses_y_ = n_poses_ / n_poses_x_; - if (n_poses % n_poses_x_ != 0) n_poses_y_++; + nr_poses_per_column_ = nr_poses_ / nr_poses_per_row_; + if (nr_poses % nr_poses_per_row_ != 0) nr_poses_per_column_++; - if (n_poses_x_ > n_max_poses_x_ || n_poses_y_ > n_max_poses_y_) { + if (nr_poses_per_row_ > nr_max_poses_per_row_ || nr_poses_per_column_ > nr_max_poses_per_column_) { cout << "WARNING: You tried to evaluate more poses in a row or in a column than was allocated in the beginning." << endl << "Check set_number_of_poses() functions in object_rasterizer.cpp" << endl; - } + }*/ + + bool nr_poses_changed = false; + set_default_kernel_config(nr_poses_, nr_poses_per_row_, nr_poses_per_column_, nr_poses_changed); + + nr_poses = nr_poses_; + nr_poses_per_row = nr_poses_per_row_; + nr_poses_per_column = nr_poses_per_column_; - set_default_kernel_config(); } else { - cout << "WARNING: You tried to evaluate more poses than specified by max_poses" << endl; + cout << "ERROR (Cuda): You tried to evaluate more poses (" << nr_poses << ") than specified by max_poses (" << nr_max_poses_ << ")" << endl; + exit(-1); } } -void CudaFilter::set_default_kernel_config() { - if (n_poses_set_) { - /* determine n_threads_ and n_blocks_ - * n_threads_ should lie between 32 (warp_size) and 128 and all microprocessors should be busy */ - n_threads_ = ((n_poses_ / n_mps_) / warp_size_) * warp_size_; - if (n_threads_ == 0) n_threads_ = warp_size_; - if (n_threads_ > 4 * warp_size_) n_threads_ = 4 * warp_size_; - - n_blocks_ = n_poses_ / n_threads_; - if (n_blocks_ % n_poses_ != 0) n_blocks_++; - } else { - cout << "WARNING: set_default_kernel_config() was not executed, because n_poses_ has not been set previously" << endl; +void CudaFilter::set_default_kernel_config(int& nr_poses, int& nr_poses_per_row, int& nr_poses_per_column, + bool& nr_poses_changed) { + nr_threads_ = min(DEFAULT_NR_THREADS, cuda_device_properties_.maxThreadsDim[0]); + + // check for grid dimension limitations + if (cuda_device_properties_.maxGridSize[0] < nr_poses_per_row) { + nr_poses_per_row = cuda_device_properties_.maxGridSize[0]; + nr_poses_per_column = ceil(nr_poses / nr_poses_per_row); + if (cuda_device_properties_.maxGridSize[1] < nr_poses_per_column) { + nr_poses_per_column = cuda_device_properties_.maxGridSize[1]; + } + + nr_poses = min(nr_poses, nr_poses_per_row * nr_poses_per_column); + + nr_poses_changed = true; + + std::cout << "Number of poses was reduced to (" << nr_poses_per_row << ", " + << nr_poses_per_column << ") because of the maximum grid size (" + << cuda_device_properties_.maxGridSize[0] << ", " << cuda_device_properties_.maxGridSize[1] + << ") retrieved from CUDA properties." << std::endl; } + + + grid_dimension_ = dim3(nr_poses_per_row, nr_poses_per_column); + + + /* + + // determine n_threads_ and n_blocks_ + // n_threads_ should lie between 32 (warp_size) and 128 and all microprocessors should be busy + nr_threads_ = ((nr_poses_ / n_mps_) / warp_size_) * warp_size_; + if (nr_threads_ == 0) nr_threads_ = warp_size_; + if (nr_threads_ > 4 * warp_size_) nr_threads_ = 4 * warp_size_; + + n_blocks_ = nr_poses_ / nr_threads_; + if (n_blocks_ % nr_poses_ != 0) n_blocks_++; + + */ } @@ -1421,14 +1512,14 @@ vector CudaFilter::get_visibility_probabilities(int state_id) { vector > CudaFilter::get_visibility_probabilities() { - float* visibility_probabilities = (float*) malloc(n_poses_ * n_rows_ * n_cols_ * sizeof(float)); - cudaMemcpy(visibility_probabilities, d_visibility_probs_, n_poses_ * n_rows_ * n_cols_ * sizeof(float), cudaMemcpyDeviceToHost); + float* visibility_probabilities = (float*) malloc(nr_poses_ * n_rows_ * n_cols_ * sizeof(float)); + cudaMemcpy(visibility_probabilities, d_visibility_probs_, nr_poses_ * n_rows_ * n_cols_ * sizeof(float), cudaMemcpyDeviceToHost); #ifdef CHECK_ERRORS check_cuda_error("cudaMemcpy d_visibility_probabilities -> visibility_probabilities"); #endif vector > visibility_probabilities_vector; vector tmp_vector (n_rows_ * n_cols_); - for (int i = 0; i < n_poses_; i++) { + for (int i = 0; i < nr_poses_; i++) { for (int j = 0; j < n_rows_ * n_cols_; j++) { tmp_vector[j] = visibility_probabilities[i * n_rows_ * n_cols_ + j]; } @@ -1455,15 +1546,8 @@ template void CudaFilter::allocate(T * &pointer, size_t size, strin cudaMalloc((void **) &pointer, size); #ifdef CHECK_ERRORS cuMemGetInfo(&free_space_after, &total_space); - if (free_space_after < free_space_before) { - cout << "memory to allocate for " << name << ": " << size / 1e6 << " MB; free space: " << free_space_before / 1e6 - << "MB; --> allocated " << (free_space_before - free_space_after) / 1e6 << " MB, free space left: " << free_space_after / 1e6 << " MB" << endl; - } else if (free_space_after > free_space_before){ - cout << "ERROR: memory to allocate for " << name << ": " << size / 1e6 << " MB; free space: " << free_space_before / 1e6 - << "MB; --> allocation failed(!), freed " << (free_space_after - free_space_before) / 1e6 << " MB, free space now: " << free_space_after / 1e6 << " MB" << endl; - } else { -// cout << "memory reallocated for " << name << ": " << size / 1e6 << "MB, free space left: " << free_space_after / 1e6 << " MB" << endl; - } + std::cout << "memory to allocate for " << name << ": " << size / 1e6 << " MB; free space: " << free_space_before / 1e6 + << "MB; --> allocated " << (free_space_before - free_space_after) / 1e6 << " MB, free space left: " << free_space_after / 1e6 << " MB" << std::endl; check_cuda_error("cudaMalloc failed"); #endif } diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp index e99c71c..d00b5df 100644 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp +++ b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp @@ -2,7 +2,7 @@ /* Note: Profiling slows down the rendering process. Use only to display the runtimes * of the different subroutines inside the render call. */ -#define PROFILING_ACTIVE +//#define PROFILING_ACTIVE #include @@ -149,17 +149,15 @@ ObjectRasterizer::ObjectRasterizer(const std::vector object_numbers) { object_numbers_ = object_numbers; } -void ObjectRasterizer::set_number_of_max_poses(int n_poses) { -// if (n_poses_ != n_poses) { - nr_max_poses_ = n_poses; - nr_poses_ = n_poses; +void ObjectRasterizer::allocate_textures_for_max_poses(int& allocated_poses, + int& allocated_poses_per_row, + int& allocated_poses_per_column) { + int max_poses_per_row = floor(max_texture_size_ / nr_cols_); + int max_poses_per_column = floor(max_texture_size_ / nr_rows_); + + allocated_poses_per_row = min(max_poses_per_row, allocated_poses); + allocated_poses_per_column = min(max_poses_per_column, (int) ceil(allocated_poses / (float) allocated_poses_per_row)); + + if (allocated_poses > allocated_poses_per_row * allocated_poses_per_column) { + std::cout << "The space for the number of maximum poses you requested (" << allocated_poses << ") cannot be allocated. " + << "The limit is OpenGL texture size: " << max_texture_size_ << ". Current resolution is (" << nr_cols_ << ", " + << nr_rows_ << ") , which means a maximum of (" << max_poses_per_row << ", " << max_poses_per_column << ") poses. " + << "As a result, space for " << allocated_poses_per_row * allocated_poses_per_column << " poses will be allocated " + << "in the form of (" << allocated_poses_per_row << ", " << allocated_poses_per_column << ")." << std::endl; + } + + + allocated_poses = allocated_poses_per_row * allocated_poses_per_column; + + nr_max_poses_ = allocated_poses; + nr_poses_ = allocated_poses; + nr_max_poses_per_row_ = allocated_poses_per_row; + nr_poses_per_row_ = allocated_poses_per_row; + nr_max_poses_per_column_ = allocated_poses_per_column; + nr_poses_per_column_ = allocated_poses_per_column; + + reallocate_buffers(); + + + + /* +// if (nr_max_poses_ != nr_max_poses) { + nr_max_poses_ = nr_max_poses; + nr_poses_ = nr_max_poses; // // TODO max_dimension[0], [1], at the moment they are identical float sqrt_poses = sqrt(nr_poses_); // TODO this can be done smarter. I want to only increment sqrt_poses, if it is not an int, i.e. 10.344 instead of 10) if (sqrt_poses * sqrt_poses != nr_poses_) sqrt_poses = (int) ceil(sqrt_poses); // TODO max_dimension[0], [1], at the moment they are identical - nr_max_poses_per_row_ = min((int) sqrt_poses, (max_dimension_ / nr_cols_)); + nr_max_poses_per_row_ = min((int) sqrt_poses, (max_texture_size_ / nr_cols_)); int y_poses = nr_max_poses_ / nr_max_poses_per_row_; if (y_poses * nr_max_poses_per_row_ < nr_max_poses_) y_poses++; - nr_max_poses_per_column_ = min(y_poses, (max_dimension_ / nr_rows_)); + nr_max_poses_per_column_ = min(y_poses, (max_texture_size_ / nr_rows_)); nr_poses_per_row_ = nr_max_poses_per_row_; nr_poses_per_column_ = nr_max_poses_per_column_; reallocate_buffers(); -// } +// }*/ } -void ObjectRasterizer::set_number_of_poses(int n_poses) { - if (n_poses <= nr_max_poses_) { - nr_poses_ = n_poses; +void ObjectRasterizer::set_number_of_poses(const int nr_poses, int& nr_poses_per_row, int& nr_poses_per_column) { + if (nr_poses <= nr_max_poses_) { + nr_poses_per_row = min(nr_max_poses_per_row_, nr_poses); + nr_poses_per_column = min(nr_max_poses_per_column_, (int) ceil(nr_poses / (float) nr_poses_per_row)); + + nr_poses_ = nr_poses; + nr_poses_per_row_ = nr_poses_per_row; + nr_poses_per_column_ = nr_poses_per_column; + + + + /* + nr_poses_ = nr_poses; // // TODO max_dimension[0], [1], at the moment they are identical @@ -436,32 +475,28 @@ void ObjectRasterizer::set_number_of_poses(int n_poses) { // TODO this can be done smarter. I want to only increment sqrt_poses, if it is not an int, i.e. 10.344 instead of 10) if (sqrt_poses * sqrt_poses != nr_poses_) sqrt_poses = (int) ceil(sqrt_poses); // TODO max_dimension[0], [1], at the moment they are identical - nr_poses_per_row_ = min((int) sqrt_poses, (max_dimension_ / nr_cols_)); + nr_poses_per_row_ = min((int) sqrt_poses, (max_texture_size_ / nr_cols_)); int y_poses = nr_poses_ / nr_poses_per_row_; if (y_poses * nr_poses_per_row_ < nr_poses_) y_poses++; - nr_poses_per_column_ = min(y_poses, (max_dimension_ / nr_rows_)); + nr_poses_per_column_ = min(y_poses, (max_texture_size_ / nr_rows_)); if (nr_poses_per_row_ > nr_max_poses_per_row_ || nr_poses_per_column_ > nr_max_poses_per_column_) { cout << "WARNING: You tried to evaluate more poses in a row or in a column than was allocated in the beginning." << endl << "Check set_number_of_poses() functions in object_rasterizer.cpp" << endl; - } + }*/ } else { - cout << "WARNING: You tried to evaluate more poses than specified by max_poses" << endl; + cout << "ERROR (OpenGL): You tried to evaluate more poses (" << nr_poses << ") than specified by max_poses (" << nr_max_poses_ << ")" << endl; + exit(-1); } } -void ObjectRasterizer::set_resolution(const int n_rows, - const int n_cols) { -// if (n_rows_ != n_rows || n_cols_ != n_cols) { +void ObjectRasterizer::set_resolution(const int n_rows, const int n_cols, int& nr_poses, int& nr_poses_per_row, int& nr_poses_per_column) { nr_rows_ = n_rows; nr_cols_ = n_cols; - // recalculate n_poses_x_ and n_poses_y_ depending on the resolution - set_number_of_max_poses(nr_max_poses_); - - -// } + // reallocate textures + allocate_textures_for_max_poses(nr_poses, nr_poses_per_row, nr_poses_per_column); } GLuint ObjectRasterizer::get_framebuffer_texture() { From 279bab66a4258c71d9a4bd3247aefbaec2a7b842 Mon Sep 17 00:00:00 2001 From: issac Date: Thu, 10 Sep 2015 12:48:51 +0200 Subject: [PATCH 053/131] bg_depth can be inf or finite --- .../depth_pixel_observation_model.hpp | 64 +++++++++++++++---- 1 file changed, 51 insertions(+), 13 deletions(-) diff --git a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp index 1ae2412..31a0ffc 100644 --- a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp +++ b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -62,28 +63,64 @@ class DepthPixelObservationModel id_(0) { // setup backgroud density which is N(y| bg_mean, ) - auto bg_mean = Obsrv(); - bg_mean(0) += bg_depth; + auto bg_mean = Obsrv(1); + + //! \todo BG changes + bg_mean(0) = + bg_depth < 0. ? std::numeric_limits::infinity() : bg_depth; bg_density_.mean(bg_mean); bg_density_.square_root(bg_density_.square_root() * bg_sigma); +// bg_density_.location(bg_mean); +// bg_density_.scaling_matrix(bg_density_.covariance() * bg_sigma * bg_sigma); + fg_density_.square_root(fg_density_.square_root() * fg_sigma); } Real log_probability(const Obsrv& obsrv, const State& state) const override { -// return density_evaluation(state).log_probability(obsrv); +// Obsrv y = depth(state); + +// if (std::isinf(y(0))) +// { +// return bg_density_.log_probability(obsrv); +// } + +// fg_density_.mean(y); +// return fg_density_.log_probability(obsrv); + return density(state).log_probability(obsrv); } Real probability(const Obsrv& obsrv, const State& state) const override { +// Obsrv y = depth(state); + +// if (std::isinf(y(0))) +// { +// return bg_density_.probability(obsrv);; +// } + +// fg_density_.mean(y); +// return fg_density_.probability(obsrv); + // return density_evaluation(state).probability(obsrv); return density(state).probability(obsrv); } Obsrv observation(const State& state, const Noise& noise) const override { + +// Obsrv y = depth(state); + +// if (std::isinf(y(0))) +// { +// return bg_density_.map_standard_normal(noise); +// } + +// fg_density_.mean(y); +// return fg_density_.map_standard_normal(noise); + Obsrv y = density(state).map_standard_normal(noise); return y; } @@ -168,19 +205,19 @@ class DepthPixelObservationModel // } - const Gaussian& density(const State& state) const - { - Obsrv y = depth(state); - - if (std::isinf(y(0))) - { - return bg_density_; - } + const Gaussian& density(const State& state) const + { + Obsrv y = depth(state); - fg_density_.mean(y); - return fg_density_; + if (std::isinf(y(0))) + { + return bg_density_; } + fg_density_.mean(y); + return fg_density_; + } + Obsrv depth(const State& current_state) const { @@ -210,6 +247,7 @@ class DepthPixelObservationModel mutable Gaussian fg_density_; mutable Gaussian bg_density_; +// mutable CauchyDistribution bg_density_; // mutable UniformDistribution bg_density_; mutable std::vector depth_rendering_; From 986c79cf412805fe031056f45c5e2feb65cf711d Mon Sep 17 00:00:00 2001 From: issac Date: Sat, 12 Sep 2015 18:28:36 +0200 Subject: [PATCH 054/131] Added hashing for PoseVelocityVector --- include/dbot/utils/pose_hashing.hpp | 31 ++++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/include/dbot/utils/pose_hashing.hpp b/include/dbot/utils/pose_hashing.hpp index d68cc82..3279f35 100644 --- a/include/dbot/utils/pose_hashing.hpp +++ b/include/dbot/utils/pose_hashing.hpp @@ -26,13 +26,42 @@ #include #include +#include template class PoseHash; template <> class PoseHash { public: - std::size_t operator()(const fl::PoseVector & s) const + std::size_t operator()(const fl::PoseVector& s) const + { + /* primes */ + static constexpr int p1 = 15487457; + static constexpr int p2 = 24092821; + static constexpr int p3 = 73856093; + static constexpr int p4 = 19349663; + static constexpr int p5 = 83492791; + static constexpr int p6 = 17353159; + + /* map size */ + static constexpr int n = 1200; + + /* precision */ + static constexpr int c = 1000000; + + return ((int(s(0, 0) * c) * p1) ^ + (int(s(1, 0) * c) * p2) ^ + (int(s(2, 0) * c) * p3) ^ + (int(s(3, 0) * c) * p4) ^ + (int(s(4, 0) * c) * p5) ^ + (int(s(5, 0) * c) * p6) % n); + } +}; + +template <> class PoseHash +{ +public: + std::size_t operator()(const fl::PoseVelocityVector& s) const { /* primes */ static constexpr int p1 = 15487457; From fbacf3bebe6740d9c66bef7f1a697c0e53676d99 Mon Sep 17 00:00:00 2001 From: issac Date: Sat, 12 Sep 2015 18:30:07 +0200 Subject: [PATCH 055/131] PoseVector > State --- .../depth_pixel_observation_model.hpp | 83 +- include/dbot/utils/helper_functions.hpp | 1130 ++++++++--------- 2 files changed, 575 insertions(+), 638 deletions(-) diff --git a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp index 31a0ffc..0db143e 100644 --- a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp +++ b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp @@ -19,6 +19,8 @@ * \author Jan Issac (jan.issac@gmail.com) */ +#pragma once + #include #include #include @@ -58,69 +60,33 @@ class DepthPixelObservationModel Real bg_sigma, int state_dim = DimensionOf::Value) : state_dim_(state_dim), -// bg_density_(-bg_sigma + bg_depth, bg_sigma + bg_depth), renderer_(renderer), id_(0) { - // setup backgroud density which is N(y| bg_mean, ) + // setup backgroud density auto bg_mean = Obsrv(1); - //! \todo BG changes bg_mean(0) = bg_depth < 0. ? std::numeric_limits::infinity() : bg_depth; bg_density_.mean(bg_mean); bg_density_.square_root(bg_density_.square_root() * bg_sigma); -// bg_density_.location(bg_mean); -// bg_density_.scaling_matrix(bg_density_.covariance() * bg_sigma * bg_sigma); - + // setup backgroud density fg_density_.square_root(fg_density_.square_root() * fg_sigma); } Real log_probability(const Obsrv& obsrv, const State& state) const override { -// Obsrv y = depth(state); - -// if (std::isinf(y(0))) -// { -// return bg_density_.log_probability(obsrv); -// } - -// fg_density_.mean(y); -// return fg_density_.log_probability(obsrv); - return density(state).log_probability(obsrv); } Real probability(const Obsrv& obsrv, const State& state) const override { -// Obsrv y = depth(state); - -// if (std::isinf(y(0))) -// { -// return bg_density_.probability(obsrv);; -// } - -// fg_density_.mean(y); -// return fg_density_.probability(obsrv); - -// return density_evaluation(state).probability(obsrv); return density(state).probability(obsrv); } Obsrv observation(const State& state, const Noise& noise) const override { - -// Obsrv y = depth(state); - -// if (std::isinf(y(0))) -// { -// return bg_density_.map_standard_normal(noise); -// } - -// fg_density_.mean(y); -// return fg_density_.map_standard_normal(noise); - Obsrv y = density(state).map_standard_normal(noise); return y; } @@ -132,7 +98,7 @@ class DepthPixelObservationModel virtual int id() const { return id_; } virtual void id(int new_id) { id_ = new_id; } - void nominal_pose(const PoseVector& p) + void nominal_pose(const State& p) { render_cache_.clear(); nominal_pose_= p; @@ -150,7 +116,7 @@ class DepthPixelObservationModel private: /** \cond internal */ - void map(const PoseVector& pose, Eigen::VectorXd& obsrv_image) const + void map(const State& pose, Eigen::VectorXd& obsrv_image) const { renderer_->set_poses({pose.affine()}); renderer_->Render(depth_rendering_); @@ -178,33 +144,6 @@ class DepthPixelObservationModel } } -// const StandardGaussianMapping& density(const State& state) const -// { -// Obsrv y = depth(state); - -// if (std::isinf(y(0))) -// { -// return bg_density_; -// } - -// fg_density_.mean(y); -// return fg_density_; -// } - -// const Evaluation& density_evaluation(const State& state) const -// { -// Obsrv y = depth(state); - -// if (std::isinf(y(0))) -// { -// return bg_density_; -// } - -// fg_density_.mean(y); -// return fg_density_; -// } - - const Gaussian& density(const State& state) const { Obsrv y = depth(state); @@ -223,7 +162,7 @@ class DepthPixelObservationModel { if (render_cache_.find(current_state) == render_cache_.end()) { - PoseVector current_pose; + State current_pose; current_pose.orientation() = current_state.orientation() * nominal_pose_.orientation(); @@ -247,21 +186,19 @@ class DepthPixelObservationModel mutable Gaussian fg_density_; mutable Gaussian bg_density_; -// mutable CauchyDistribution bg_density_; -// mutable UniformDistribution bg_density_; mutable std::vector depth_rendering_; std::shared_ptr renderer_; mutable std::unordered_map< - PoseVector, + State, Eigen::VectorXd, - PoseHash + PoseHash > render_cache_; public: int id_; - mutable PoseVector nominal_pose_; + mutable State nominal_pose_; }; } diff --git a/include/dbot/utils/helper_functions.hpp b/include/dbot/utils/helper_functions.hpp index 1bf900f..691aad8 100644 --- a/include/dbot/utils/helper_functions.hpp +++ b/include/dbot/utils/helper_functions.hpp @@ -144,62 +144,62 @@ template void Output(const T value, template int IndexNextReal( - const std::vector& data, - const int& current_index = -1, - const int& step_size = 1) + const std::vector& data, + const int& current_index = -1, + const int& step_size = 1) { - int index_next_real = current_index + step_size; - while( - index_next_real < int(data.size()) && - index_next_real >= 0 && - !std::isfinite(data[index_next_real])) index_next_real+=step_size; + int index_next_real = current_index + step_size; + while( + index_next_real < int(data.size()) && + index_next_real >= 0 && + !std::isfinite(data[index_next_real])) index_next_real+=step_size; - return index_next_real; + return index_next_real; } // various useful functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> class Structurer2D { public: - Structurer2D(){} - - ~Structurer2D() {} - - template std::vector Flatten(const std::vector >& deep_structure) - { - sizes_.resize(deep_structure.size()); - size_t global_size = 0; - for(size_t i = 0; i < deep_structure.size(); i++) - { - sizes_[i] = deep_structure[i].size(); - global_size += deep_structure[i].size(); - } - - std::vector flat_structure(global_size); - size_t global_index = 0; - for(size_t i = 0; i < deep_structure.size(); i++) - for(size_t j = 0; j < deep_structure[i].size(); j++) - flat_structure[global_index++] = deep_structure[i][j]; - - return flat_structure; - } - - template std::vector > Deepen(const std::vector& flat_structure) - { - std::vector > deep_structure(sizes_.size()); - - size_t global_index = 0; - for(size_t i = 0; i < deep_structure.size(); i++) - { - deep_structure[i].resize(sizes_[i]); - for(size_t j = 0; j < deep_structure[i].size(); j++) - deep_structure[i][j] = flat_structure[global_index++]; - } - return deep_structure; - } + Structurer2D(){} + + ~Structurer2D() {} + + template std::vector Flatten(const std::vector >& deep_structure) + { + sizes_.resize(deep_structure.size()); + size_t global_size = 0; + for(size_t i = 0; i < deep_structure.size(); i++) + { + sizes_[i] = deep_structure[i].size(); + global_size += deep_structure[i].size(); + } + + std::vector flat_structure(global_size); + size_t global_index = 0; + for(size_t i = 0; i < deep_structure.size(); i++) + for(size_t j = 0; j < deep_structure[i].size(); j++) + flat_structure[global_index++] = deep_structure[i][j]; + + return flat_structure; + } + + template std::vector > Deepen(const std::vector& flat_structure) + { + std::vector > deep_structure(sizes_.size()); + + size_t global_index = 0; + for(size_t i = 0; i < deep_structure.size(); i++) + { + deep_structure[i].resize(sizes_[i]); + for(size_t j = 0; j < deep_structure[i].size(); j++) + deep_structure[i][j] = flat_structure[global_index++]; + } + return deep_structure; + } private: - std::vector sizes_; + std::vector sizes_; }; @@ -207,53 +207,53 @@ class Structurer2D class Structurer3D { public: - Structurer3D(){} - - ~Structurer3D() {} - - template std::vector Flatten(const std::vector > >& deep_structure) - { - size_t global_size = 0; - sizes_.resize(deep_structure.size()); - for(size_t i = 0; i < deep_structure.size(); i++) - { - sizes_[i].resize(deep_structure[i].size()); - for(size_t j = 0; j < deep_structure[i].size(); j++) - { - sizes_[i][j] = deep_structure[i][j].size(); - global_size += deep_structure[i][j].size(); - } - } - - std::vector flat_structure(global_size); - size_t global_index = 0; - for(size_t i = 0; i < deep_structure.size(); i++) - for(size_t j = 0; j < deep_structure[i].size(); j++) - for(size_t k = 0; k < deep_structure[i][j].size(); k++) - flat_structure[global_index++] = deep_structure[i][j][k]; - - return flat_structure; - } - - template std::vector > > Deepen(const std::vector& flat_structure) - { - size_t global_index = 0; - std::vector > > deep_structure(sizes_.size()); - for(size_t i = 0; i < deep_structure.size(); i++) - { - deep_structure[i].resize(sizes_[i].size()); - for(size_t j = 0; j < deep_structure[i].size(); j++) - { - deep_structure[i][j].resize(sizes_[i][j]); - for(size_t k = 0; k < deep_structure[i][j].size(); k++) - deep_structure[i][j][k] = flat_structure[global_index++]; - } - } - return deep_structure; - } + Structurer3D(){} + + ~Structurer3D() {} + + template std::vector Flatten(const std::vector > >& deep_structure) + { + size_t global_size = 0; + sizes_.resize(deep_structure.size()); + for(size_t i = 0; i < deep_structure.size(); i++) + { + sizes_[i].resize(deep_structure[i].size()); + for(size_t j = 0; j < deep_structure[i].size(); j++) + { + sizes_[i][j] = deep_structure[i][j].size(); + global_size += deep_structure[i][j].size(); + } + } + + std::vector flat_structure(global_size); + size_t global_index = 0; + for(size_t i = 0; i < deep_structure.size(); i++) + for(size_t j = 0; j < deep_structure[i].size(); j++) + for(size_t k = 0; k < deep_structure[i][j].size(); k++) + flat_structure[global_index++] = deep_structure[i][j][k]; + + return flat_structure; + } + + template std::vector > > Deepen(const std::vector& flat_structure) + { + size_t global_index = 0; + std::vector > > deep_structure(sizes_.size()); + for(size_t i = 0; i < deep_structure.size(); i++) + { + deep_structure[i].resize(sizes_[i].size()); + for(size_t j = 0; j < deep_structure[i].size(); j++) + { + deep_structure[i][j].resize(sizes_[i][j]); + for(size_t k = 0; k < deep_structure[i][j].size(); k++) + deep_structure[i][j][k] = flat_structure[global_index++]; + } + } + return deep_structure; + } private: - std::vector > sizes_; + std::vector > sizes_; }; @@ -266,238 +266,238 @@ class Structurer3D template void LinearlyInterpolate(std::vector& data) { - std::vector limits; - limits.push_back(0); - limits.push_back(data.size()-1); - std::vector step_direction; - step_direction.push_back(1); - step_direction.push_back(-1); + std::vector limits; + limits.push_back(0); + limits.push_back(data.size()-1); + std::vector step_direction; + step_direction.push_back(1); + step_direction.push_back(-1); - // extrapolate - for(int i = 0; i < 2; i++) - { - int index_first_real = IndexNextReal(data, limits[i] - step_direction[i], step_direction[i]); - int index_next_real = IndexNextReal(data, index_first_real, step_direction[i]); - if(index_next_real >= int(data.size()) || index_next_real < 0) - return; + // extrapolate + for(int i = 0; i < 2; i++) + { + int index_first_real = IndexNextReal(data, limits[i] - step_direction[i], step_direction[i]); + int index_next_real = IndexNextReal(data, index_first_real, step_direction[i]); + if(index_next_real >= int(data.size()) || index_next_real < 0) + return; - double slope = - double(data[index_next_real] - data[index_first_real]) / - double(index_next_real - index_first_real); + double slope = + double(data[index_next_real] - data[index_first_real]) / + double(index_next_real - index_first_real); - for(int j = limits[i]; j != index_first_real; j += step_direction[i]) - data[j] = data[index_first_real] + (j - index_first_real) * slope; - } + for(int j = limits[i]; j != index_first_real; j += step_direction[i]) + data[j] = data[index_first_real] + (j - index_first_real) * slope; + } - // interpolate - int index_current_real = IndexNextReal(data, limits[0] - step_direction[0], step_direction[0]); - int index_next_real = IndexNextReal(data, index_current_real, step_direction[0]); + // interpolate + int index_current_real = IndexNextReal(data, limits[0] - step_direction[0], step_direction[0]); + int index_next_real = IndexNextReal(data, index_current_real, step_direction[0]); - while(index_next_real < int(data.size())) - { - double slope = - double(data[index_next_real] - data[index_current_real]) / - double(index_next_real - index_current_real); + while(index_next_real < int(data.size())) + { + double slope = + double(data[index_next_real] - data[index_current_real]) / + double(index_next_real - index_current_real); - for(int i = index_current_real + 1; i < index_next_real; i++) - data[i] = data[index_current_real] + (i - index_current_real) * slope; + for(int i = index_current_real + 1; i < index_next_real; i++) + data[i] = data[index_current_real] + (i - index_current_real) * slope; - index_current_real = index_next_real; - index_next_real = IndexNextReal(data, index_next_real, step_direction[0]); - } + index_current_real = index_next_real; + index_next_real = IndexNextReal(data, index_next_real, step_direction[0]); + } } template void PrintVector(std::vector v) { - for(size_t i = 0; i < v.size(); i++) - std::cout << "(" << i << ": " << v[i] << ") "; - std::cout << std::endl; + for(size_t i = 0; i < v.size(); i++) + std::cout << "(" << i << ": " << v[i] << ") "; + std::cout << std::endl; } template void PrintVector(std::vector > v) { - for(size_t i = 0; i < v.size(); i++) - { - std::cout << i << " --------------------------------" << std::endl; - PrintVector(v[i]); - } + for(size_t i = 0; i < v.size(); i++) + { + std::cout << i << " --------------------------------" << std::endl; + PrintVector(v[i]); + } } template void PrintVector(std::vector > > v) { - for(size_t i = 0; i < v.size(); i++) - { - std::cout << i << " ================================" << std::endl; - PrintVector(v[i]); - } + for(size_t i = 0; i < v.size(); i++) + { + std::cout << i << " ================================" << std::endl; + PrintVector(v[i]); + } } template void Swap(T& a, T& b) { - T temp = a; - a = b; - b = temp; + T temp = a; + a = b; + b = temp; } template Eigen::Matrix Std2Eigen(const std::vector &std) { - Eigen::Matrix eigen(std.size()); - for(size_t i = 0; i < std.size(); i++) - eigen(i) = std[i]; - return eigen; + Eigen::Matrix eigen(std.size()); + for(size_t i = 0; i < std.size(); i++) + eigen(i) = std[i]; + return eigen; } template std::string Eigen2Mathematica(const Eigen::MatrixBase& eigen, const std::string &name = "") { - std::string mathematica; - if(!name.empty()) - mathematica += name + " = "; - - mathematica += "{"; - for(int row = 0; row < eigen.rows(); row++) - { - mathematica += "{"; - for(int col = 0; col < eigen.cols(); col++) - { - mathematica += boost::lexical_cast(eigen(row, col)); - if(col != eigen.cols() -1) - mathematica += ", "; - } - mathematica += "}"; - if(row != eigen.rows()-1) - mathematica += ", "; - } - mathematica += "}"; - if(!name.empty()) - mathematica += ";"; - - return mathematica; + std::string mathematica; + if(!name.empty()) + mathematica += name + " = "; + + mathematica += "{"; + for(int row = 0; row < eigen.rows(); row++) + { + mathematica += "{"; + for(int col = 0; col < eigen.cols(); col++) + { + mathematica += boost::lexical_cast(eigen(row, col)); + if(col != eigen.cols() -1) + mathematica += ", "; + } + mathematica += "}"; + if(row != eigen.rows()-1) + mathematica += ", "; + } + mathematica += "}"; + if(!name.empty()) + mathematica += ";"; + + return mathematica; } // returns the first index where the two vectors differ, if there is no difference then -1 is returned. template int DifferenceAt( - const std::vector >& a, - const std::vector >& b, - const T& epsilon) -{ - if(a.size() < b.size()) - return a.size(); - else if(b.size() < a.size()) - return b.size(); - - for(size_t i = 0; i < a.size(); i++) - for(size_t row = 0; row < n_rows; row++) - for(size_t col = 0; col < n_cols; col++) - { - if(isnan(a[i](row, col)) && isnan(b[i](row, col))) - continue; - else if(isnan(a[i](row, col)) || isnan(b[i](row, col))) - return i; - else if( (a[i](row, col) - b[i](row, col) > epsilon) || (b[i](row, col) - a[i](row, col) > epsilon) ) - return i; - } - - return -1; + const std::vector >& a, + const std::vector >& b, + const T& epsilon) +{ + if(a.size() < b.size()) + return a.size(); + else if(b.size() < a.size()) + return b.size(); + + for(size_t i = 0; i < a.size(); i++) + for(size_t row = 0; row < n_rows; row++) + for(size_t col = 0; col < n_cols; col++) + { + if(isnan(a[i](row, col)) && isnan(b[i](row, col))) + continue; + else if(isnan(a[i](row, col)) || isnan(b[i](row, col))) + return i; + else if( (a[i](row, col) - b[i](row, col) > epsilon) || (b[i](row, col) - a[i](row, col) > epsilon) ) + return i; + } + + return -1; } // returns the first index where the two vectors differ, if there is no difference then -1 is returned. template int DifferenceAt( - const std::vector& a, - const std::vector& b, - const T& epsilon) + const std::vector& a, + const std::vector& b, + const T& epsilon) { - if(a.size() < b.size()) - return a.size(); - else if(b.size() < a.size()) - return b.size(); + if(a.size() < b.size()) + return a.size(); + else if(b.size() < a.size()) + return b.size(); - for(size_t i = 0; i < a.size(); i++) - { - if(isnan(a[i]) && isnan(b[i])) - continue; - else if(isnan(a[i]) || isnan(b[i])) - return i; - else if( (a[i] - b[i] > epsilon) || (b[i] - a[i] > epsilon) ) - return i; - } + for(size_t i = 0; i < a.size(); i++) + { + if(isnan(a[i]) && isnan(b[i])) + continue; + else if(isnan(a[i]) || isnan(b[i])) + return i; + else if( (a[i] - b[i] > epsilon) || (b[i] - a[i] > epsilon) ) + return i; + } - return -1; + return -1; } template std::vector ReverseOrder(std::vector ordered) { - std::vector reversed(ordered.size()); - for(size_t i = 0; i < ordered.size(); i++) - reversed[i] = ordered[ordered.size() - 1 - i]; + std::vector reversed(ordered.size()); + for(size_t i = 0; i < ordered.size(); i++) + reversed[i] = ordered[ordered.size() - 1 - i]; } template std::vector Count(T from, T to, T increment = 1) { - std::vector count(size_t((to - from)/increment)); - count[0] = from; - for(size_t i = 1; i < count.size(); i++) - count[i] = count[i-1] + increment; + std::vector count(size_t((to - from)/increment)); + count[0] = from; + for(size_t i = 1; i < count.size(); i++) + count[i] = count[i-1] + increment; } template struct ValuesIndex { - std::vector values; - int index; - - bool operator < (const ValuesIndex& right_side) const - { - for(size_t i = 0; i < values.size(); i++) - { - if(values[i] < right_side.values[i]) - return true; - if(values[i] > right_side.values[i]) - return false; - } - return false; - } - - - bool operator != (const ValuesIndex& right_side) const - { - for(size_t i = 0; i < values.size(); i++) - if(values[i] != right_side.values[i]) - return true; - - return false; - } + std::vector values; + int index; + + bool operator < (const ValuesIndex& right_side) const + { + for(size_t i = 0; i < values.size(); i++) + { + if(values[i] < right_side.values[i]) + return true; + if(values[i] > right_side.values[i]) + return false; + } + return false; + } + + + bool operator != (const ValuesIndex& right_side) const + { + for(size_t i = 0; i < values.size(); i++) + if(values[i] != right_side.values[i]) + return true; + + return false; + } }; template void SortAndCollapse( - std::vector >& values, - std::vector& multiplicities) + std::vector >& values, + std::vector& multiplicities) { - std::vector > values_indices(values.size()); - for(size_t i = 0; i < values.size(); i++) - { - values_indices[i].index = i; - values_indices[i].values = values[i]; - } + std::vector > values_indices(values.size()); + for(size_t i = 0; i < values.size(); i++) + { + values_indices[i].index = i; + values_indices[i].values = values[i]; + } - std::sort(values_indices.begin(), values_indices.end()); + std::sort(values_indices.begin(), values_indices.end()); - std::vector > temp_values = values; - multiplicities = std::vector(values.size(), 0); - size_t distinct_index = 0; - values[0] = temp_values[values_indices[0].index]; - for(size_t i = 0; i < values.size(); i++) - { - if(i > 0 && temp_values[values_indices[i-1].index] != temp_values[values_indices[i].index]) - values[++distinct_index] = temp_values[values_indices[i].index]; - multiplicities[distinct_index]++; - } - values.resize(distinct_index+1); - multiplicities.resize(distinct_index+1); + std::vector > temp_values = values; + multiplicities = std::vector(values.size(), 0); + size_t distinct_index = 0; + values[0] = temp_values[values_indices[0].index]; + for(size_t i = 0; i < values.size(); i++) + { + if(i > 0 && temp_values[values_indices[i-1].index] != temp_values[values_indices[i].index]) + values[++distinct_index] = temp_values[values_indices[i].index]; + multiplicities[distinct_index]++; + } + values.resize(distinct_index+1); + multiplicities.resize(distinct_index+1); } @@ -511,42 +511,42 @@ SortAndCollapse( template struct ValueIndex { - T value; - int index; + T value; + int index; - bool operator < (const ValueIndex& str) const - { - return (value < str.value); - } + bool operator < (const ValueIndex& str) const + { + return (value < str.value); + } }; template std::vector SortAscend(const std::vector &values) { - std::vector indices(values.size()); + std::vector indices(values.size()); - std::vector > values_indices(values.size()); - for(int i = 0; i < int(values.size()); i++) - { - values_indices[i].index = i; - values_indices[i].value = values[i]; - } + std::vector > values_indices(values.size()); + for(int i = 0; i < int(values.size()); i++) + { + values_indices[i].index = i; + values_indices[i].value = values[i]; + } - std::sort(values_indices.begin(), values_indices.end()); + std::sort(values_indices.begin(), values_indices.end()); - for(int i = 0; i < int(indices.size()); i++) - indices[i] = values_indices[i].index; + for(int i = 0; i < int(indices.size()); i++) + indices[i] = values_indices[i].index; - return indices; + return indices; } template std::vector SortDescend(const std::vector &values) { - std::vector ascend_indices = SortAscend(values); - std::vector descend_indices(ascend_indices.size()); + std::vector ascend_indices = SortAscend(values); + std::vector descend_indices(ascend_indices.size()); - for(int i = 0; i < int(ascend_indices.size()); i++) - descend_indices[i] = ascend_indices[ascend_indices.size()-1-i]; + for(int i = 0; i < int(ascend_indices.size()); i++) + descend_indices[i] = ascend_indices[ascend_indices.size()-1-i]; - return descend_indices; + return descend_indices; } @@ -567,124 +567,124 @@ template void Sort(std::vector &ref_values, bool order = 0) template void SortAscend(std::vector &ref_values, std::vector &values) { - std::vector temp_ref_values = ref_values; - std::vector temp_values = values; + std::vector temp_ref_values = ref_values; + std::vector temp_values = values; - std::vector indices = SortAscend(temp_ref_values); + std::vector indices = SortAscend(temp_ref_values); - for(int i = 0; i < int(indices.size()); i++) - { - ref_values[i] = temp_ref_values[indices[i]]; - values[i] = temp_values[indices[i]]; - } + for(int i = 0; i < int(indices.size()); i++) + { + ref_values[i] = temp_ref_values[indices[i]]; + values[i] = temp_values[indices[i]]; + } } template void SortDescend(std::vector &ref_values, std::vector &values) { - std::vector temp_ref_values = ref_values; - std::vector temp_values = values; + std::vector temp_ref_values = ref_values; + std::vector temp_values = values; - std::vector indices = SortDescend(temp_ref_values); + std::vector indices = SortDescend(temp_ref_values); - for(int i = 0; i < int(indices.size()); i++) - { - ref_values[i] = temp_ref_values[indices[i]]; - values[i] = temp_values[indices[i]]; - } + for(int i = 0; i < int(indices.size()); i++) + { + ref_values[i] = temp_ref_values[indices[i]]; + values[i] = temp_values[indices[i]]; + } } template void SortAscend(std::vector &ref_values, - std::vector &values1, std::vector &values2) + std::vector &values1, std::vector &values2) { - std::vector temp_ref_values = ref_values; - std::vector temp_values1 = values1; - std::vector temp_values2 = values2; + std::vector temp_ref_values = ref_values; + std::vector temp_values1 = values1; + std::vector temp_values2 = values2; - std::vector indices = SortAscend(temp_ref_values); + std::vector indices = SortAscend(temp_ref_values); - for(int i = 0; i < int(indices.size()); i++) - { - ref_values[i] = temp_ref_values[indices[i]]; - values1[i] = temp_values1[indices[i]]; - values2[i] = temp_values2[indices[i]]; - } + for(int i = 0; i < int(indices.size()); i++) + { + ref_values[i] = temp_ref_values[indices[i]]; + values1[i] = temp_values1[indices[i]]; + values2[i] = temp_values2[indices[i]]; + } } template void SortDescend(std::vector &ref_values, - std::vector &values1, std::vector &values2) + std::vector &values1, std::vector &values2) { - std::vector temp_ref_values = ref_values; - std::vector temp_values1 = values1; - std::vector temp_values2 = values2; + std::vector temp_ref_values = ref_values; + std::vector temp_values1 = values1; + std::vector temp_values2 = values2; - std::vector indices = SortDescend(temp_ref_values); + std::vector indices = SortDescend(temp_ref_values); - for(int i = 0; i < int(indices.size()); i++) - { - ref_values[i] = temp_ref_values[indices[i]]; - values1[i] = temp_values1[indices[i]]; - values2[i] = temp_values2[indices[i]]; - } + for(int i = 0; i < int(indices.size()); i++) + { + ref_values[i] = temp_ref_values[indices[i]]; + values1[i] = temp_values1[indices[i]]; + values2[i] = temp_values2[indices[i]]; + } } template int BoundIndex(const std::vector &values, bool bound_type) // bound type 1 for max and 0 for min { - int BoundIndex = 0; - T bound_value = bound_type ? -std::numeric_limits::max() : std::numeric_limits::max(); + int BoundIndex = 0; + T bound_value = bound_type ? -std::numeric_limits::max() : std::numeric_limits::max(); - for(int i = 0; i < int(values.size()); i++) - if(bound_type ? (values[i] > bound_value) : (values[i] < bound_value) ) - { - BoundIndex = i; - bound_value = values[i]; - } + for(int i = 0; i < int(values.size()); i++) + if(bound_type ? (values[i] > bound_value) : (values[i] < bound_value) ) + { + BoundIndex = i; + bound_value = values[i]; + } - return BoundIndex; + return BoundIndex; } template T bound_value(const std::vector &values, bool bound_type) // bound type 1 for max and 0 for min { - return values[BoundIndex(values, bound_type)]; + return values[BoundIndex(values, bound_type)]; } template std::vector Apply(const std::vector &input, Tout(*f)(Tin)) { - std::vector output(input.size()); - for(size_t i = 0; i < output.size(); i++) - output[i] = (*f)(input[i]); + std::vector output(input.size()); + for(size_t i = 0; i < output.size(); i++) + output[i] = (*f)(input[i]); - return output; + return output; } template std::vector SetSum(const std::vector &input, T sum) { - T old_sum = 0; - for(size_t i = 0; i < input.size(); i++) - old_sum += input[i]; - T factor = sum/old_sum; + T old_sum = 0; + for(size_t i = 0; i < input.size(); i++) + old_sum += input[i]; + T factor = sum/old_sum; - std::vector output(input.size()); - for(size_t i = 0; i < input.size(); i++) - output[i] = factor*input[i]; + std::vector output(input.size()); + for(size_t i = 0; i < input.size(); i++) + output[i] = factor*input[i]; - return output; + return output; } // geometry functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> template Eigen::Quaternion Delta2Quaternion(const Eigen::MatrixBase& delta) { - typename T::Scalar angle = delta.norm(); - Eigen::Matrix axis = delta.normalized(); - Eigen::Quaternion q; - if(std::isfinite(axis.norm())) - q = Eigen::AngleAxisd(angle, axis); - else - q = Eigen::Quaterniond::Identity(); + typename T::Scalar angle = delta.norm(); + Eigen::Matrix axis = delta.normalized(); + Eigen::Quaternion q; + if(std::isfinite(axis.norm())) + q = Eigen::AngleAxisd(angle, axis); + else + q = Eigen::Quaterniond::Identity(); - return q; + return q; } // this function finds the intersection between two lines. a is a point on line and d_a is the direction @@ -692,115 +692,115 @@ Eigen::Quaternion Delta2Quaternion(const Eigen::MatrixBase Eigen::Matrix Intersection( - const Eigen::MatrixBase& a, - const Eigen::MatrixBase& d_a_in, - const Eigen::MatrixBase& b, - const Eigen::MatrixBase& d_b_in) + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& d_a_in, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& d_b_in) { - const Eigen::Matrix d_a = d_a_in.normalized(); - const Eigen::Matrix d_b = d_b_in.normalized(); + const Eigen::Matrix d_a = d_a_in.normalized(); + const Eigen::Matrix d_b = d_b_in.normalized(); - double c_a = d_a.dot((b-a) - d_b.dot(b-a)*d_b)/ - (1.-pow(d_b.dot(d_a),2)); - double c_b = d_b.dot((a-b) - d_a.dot(a-b)*d_a)/ - (1.-pow(d_a.dot(d_b),2)); + double c_a = d_a.dot((b-a) - d_b.dot(b-a)*d_b)/ + (1.-pow(d_b.dot(d_a),2)); + double c_b = d_b.dot((a-b) - d_a.dot(a-b)*d_a)/ + (1.-pow(d_a.dot(d_b),2)); - return((b + c_b*d_b)+(a + c_a*d_a))/2.; + return((b + c_b*d_b)+(a + c_a*d_a))/2.; } template typename T1::Scalar Angle( - const Eigen::MatrixBase& a, - const Eigen::MatrixBase& b) + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b) { - return atan2(a.cross(b).norm(), a.dot(b)); + return atan2(a.cross(b).norm(), a.dot(b)); } template Eigen::Matrix OrthogonalUnitVector(const Eigen::MatrixBase& v) { - Eigen::Matrix n = - (v.cross(Eigen::Matrix(1.0, 0, 0))).normalized(); - if(!std::isfinite(n.norm())) - n = (v.cross(Eigen::Matrix(0, 1.0, 0))).normalized(); + Eigen::Matrix n = + (v.cross(Eigen::Matrix(1.0, 0, 0))).normalized(); + if(!std::isfinite(n.norm())) + n = (v.cross(Eigen::Matrix(0, 1.0, 0))).normalized(); - return n; + return n; } // returns the angle and axis which rotate a onto b template Eigen::AngleAxis AngleAxis( - const Eigen::MatrixBase& a, - const Eigen::MatrixBase& b) + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b) { - Eigen::Matrix axis = a.cross(b); - typename T1::Scalar axis_norm = axis.norm(); - axis /= axis_norm; + Eigen::Matrix axis = a.cross(b); + typename T1::Scalar axis_norm = axis.norm(); + axis /= axis_norm; - // if the axis is inf then the angle is either 0 or pi, so the axis has to be just some orthogonal vector - if(!std::isfinite(axis.norm())) - axis = OrthogonalUnitVector(a); + // if the axis is inf then the angle is either 0 or pi, so the axis has to be just some orthogonal vector + if(!std::isfinite(axis.norm())) + axis = OrthogonalUnitVector(a); - typename T1::Scalar angle = atan2(axis_norm, a.dot(b)); + typename T1::Scalar angle = atan2(axis_norm, a.dot(b)); - return Eigen::AngleAxis(angle, axis); + return Eigen::AngleAxis(angle, axis); } // the input quaternion has the order xyzw inline Eigen::Matrix QuaternionMatrix(const Eigen::Matrix& q_xyzw) { - Eigen::Matrix Q; - Q << q_xyzw(3), q_xyzw(2), -q_xyzw(1), - -q_xyzw(2), q_xyzw(3), q_xyzw(0), - q_xyzw(1), -q_xyzw(0), q_xyzw(3), - -q_xyzw(0), -q_xyzw(1), -q_xyzw(2); - return 0.5*Q; + Eigen::Matrix Q; + Q << q_xyzw(3), q_xyzw(2), -q_xyzw(1), + -q_xyzw(2), q_xyzw(3), q_xyzw(0), + q_xyzw(1), -q_xyzw(0), q_xyzw(3), + -q_xyzw(0), -q_xyzw(1), -q_xyzw(2); + return 0.5*Q; } template class TransformationSequence { public: - TransformationSequence( - const Eigen::Matrix &R = Eigen::Matrix::Identity(), - const Eigen::Matrix &t = Eigen::Matrix::Zero()): R_(R), t_(t) {} - - ~TransformationSequence(){} - - void PreRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) - { - t_ = t_ - R_*R*c + R_*c; - R_ = R_*R; - } - void PreTranslate(const Eigen::Matrix &t) - { - t_ = t_ + R_*t; - } - void PostRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) - { - t_ = R*t_ - R*c + c; - R_ = R*R_; - } - void PostTranslate(const Eigen::Matrix &t) - { - t_ = t_ + t; - } - - void get(Eigen::Matrix &R, Eigen::Matrix &t) - { - R = R_; - t = t_; - } - - void NormalizeQuat() - { - Eigen::Quaternion q; q = R_; - q.normalize(); - R_ = q.toRotationMatrix(); - } + TransformationSequence( + const Eigen::Matrix &R = Eigen::Matrix::Identity(), + const Eigen::Matrix &t = Eigen::Matrix::Zero()): R_(R), t_(t) {} + + ~TransformationSequence(){} + + void PreRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) + { + t_ = t_ - R_*R*c + R_*c; + R_ = R_*R; + } + void PreTranslate(const Eigen::Matrix &t) + { + t_ = t_ + R_*t; + } + void PostRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) + { + t_ = R*t_ - R*c + c; + R_ = R*R_; + } + void PostTranslate(const Eigen::Matrix &t) + { + t_ = t_ + t; + } + + void get(Eigen::Matrix &R, Eigen::Matrix &t) + { + R = R_; + t = t_; + } + + void NormalizeQuat() + { + Eigen::Quaternion q; q = R_; + q.normalize(); + R_ = q.toRotationMatrix(); + } private: - Eigen::Matrix R_; - Eigen::Matrix t_; + Eigen::Matrix R_; + Eigen::Matrix t_; }; @@ -810,7 +810,7 @@ template Eigen::Matrix CartCoord2ImageCoord(const Eigen::Matrix& cart, const Eigen::Matrix& camera_matrix) { - return (camera_matrix * cart/cart(2)).topRows(2); + return (camera_matrix * cart/cart(2)).topRows(2); } // converts from cartesian space to image index, (row, col) @@ -818,7 +818,7 @@ template Eigen::Matrix CartCoord2ImageIndex(const Eigen::Matrix& cart, const Eigen::Matrix& camera_matrix) { - Eigen::Matrix image = CartCoord2ImageCoord(cart, camera_matrix); + Eigen::Matrix image = CartCoord2ImageCoord(cart, camera_matrix); Eigen::Matrix image_index; image_index(0) = floor(image(1)+0.5); image_index(1) = floor(image(0)+0.5); @@ -845,10 +845,10 @@ ImageIndex2CartCoord(const Eigen::Matrix& image_index, const T& depth, const Eigen::Matrix& camera_matrix_inverse) { - Eigen::Matrix image; + Eigen::Matrix image; image(0) = image_index(1); image(1) = image_index(0); - return ImageCoord2CartCoord(image, depth, camera_matrix_inverse); + return ImageCoord2CartCoord(image, depth, camera_matrix_inverse); } @@ -876,45 +876,45 @@ DepthImage2CartVectors(const std::vector& image, const size_t& n_rows, const size_t& n_cols, const Eigen::Matrix& camera_matrix) { - Eigen::Matrix camera_matrix_inverse = camera_matrix.inverse(); + Eigen::Matrix camera_matrix_inverse = camera_matrix.inverse(); - std::vector > vectors(n_rows*n_cols); - for(size_t row = 0; row < n_rows; row++) - for(size_t col = 0; col < n_cols; col++) - vectors[row*n_cols + col] = - ImageIndex2CartCoord( - Eigen::Vector2i(row, col), - image[row*n_cols + col], - camera_matrix_inverse); + std::vector > vectors(n_rows*n_cols); + for(size_t row = 0; row < n_rows; row++) + for(size_t col = 0; col < n_cols; col++) + vectors[row*n_cols + col] = + ImageIndex2CartCoord( + Eigen::Vector2i(row, col), + image[row*n_cols + col], + camera_matrix_inverse); - return vectors; + return vectors; } template std::vector CartVectors2DepthImage( - const std::vector >& vectors, - const int& n_rows, const int& n_cols, - const Eigen::Matrix& camera_matrix) + const std::vector >& vectors, + const int& n_rows, const int& n_cols, + const Eigen::Matrix& camera_matrix) { - std::vector image(n_rows*n_cols, NAN); + std::vector image(n_rows*n_cols, NAN); - for(size_t i = 0; i < vectors.size(); i++) - { - if( isnan(vectors[i](0)) || isnan(vectors[i](1)) || isnan(vectors[i](2)) ) - continue; + for(size_t i = 0; i < vectors.size(); i++) + { + if( isnan(vectors[i](0)) || isnan(vectors[i](1)) || isnan(vectors[i](2)) ) + continue; - Eigen::Matrix index = CartCoord2ImageIndex(vectors[i], camera_matrix); + Eigen::Matrix index = CartCoord2ImageIndex(vectors[i], camera_matrix); - if(index(0) < 0 || index(0) >= n_rows || index(1) < 0 || index(1) >= n_cols) - { - std::cout << "ERROR: in CartVectors2DepthImage vector comes to lie outside of image " << std::endl; - exit(-1); - } + if(index(0) < 0 || index(0) >= n_rows || index(1) < 0 || index(1) >= n_cols) + { + std::cout << "ERROR: in CartVectors2DepthImage vector comes to lie outside of image " << std::endl; + exit(-1); + } - image[index(0)*n_cols + index(1)] = vectors[i](2); - } + image[index(0)*n_cols + index(1)] = vectors[i](2); + } - return image; + return image; } @@ -922,68 +922,68 @@ CartVectors2DepthImage( // numerically stable implementation of log(sum(exp(xi))) >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> inline double log_sum_exp(double a, double b) { - if(a>b) return a + log(1+exp(b-a)); - else return b + log(1+exp(a-b)); + if(a>b) return a + log(1+exp(b-a)); + else return b + log(1+exp(a-b)); } inline double log_sum_exp(double a, double b, double c) { - if(a>b && a>c) return a + log(1+exp(b-a)+exp(c-a)); - if(b>c && b>a) return b + log(1+exp(a-b)+exp(c-b)); - else return c + log(1+exp(a-c)+exp(b-c)); + if(a>b && a>c) return a + log(1+exp(b-a)+exp(c-a)); + if(b>c && b>a) return b + log(1+exp(a-b)+exp(c-b)); + else return c + log(1+exp(a-c)+exp(b-c)); } inline double log_sum_exp(double a, double b, double c, double d) { - if(a>b && a>c && a>d) return a + log(1+exp(b-a)+exp(c-a)+exp(d-a)); - if(b>a && b>c && b>d) return b + log(1+exp(a-b)+exp(c-b)+exp(d-b)); - if(c>a && c>b && c>d) return c + log(1+exp(a-c)+exp(b-c)+exp(d-c)); - else return d + log(1+exp(a-d)+exp(b-d)+exp(c-d)); + if(a>b && a>c && a>d) return a + log(1+exp(b-a)+exp(c-a)+exp(d-a)); + if(b>a && b>c && b>d) return b + log(1+exp(a-b)+exp(c-b)+exp(d-b)); + if(c>a && c>b && c>d) return c + log(1+exp(a-c)+exp(b-c)+exp(d-c)); + else return d + log(1+exp(a-d)+exp(b-d)+exp(c-d)); } template T log_sum_exp(std::vector exponents) { - T max_exponent = -std::numeric_limits::max(); - for(size_t i = 0; i < exponents.size(); i++) - if(exponents[i] > max_exponent) max_exponent = exponents[i]; + T max_exponent = -std::numeric_limits::max(); + for(size_t i = 0; i < exponents.size(); i++) + if(exponents[i] > max_exponent) max_exponent = exponents[i]; - for(size_t i = 0; i < exponents.size(); i++) - exponents[i] -= max_exponent; + for(size_t i = 0; i < exponents.size(); i++) + exponents[i] -= max_exponent; - T sum = 0; - for(size_t i = 0; i < exponents.size(); i++) - sum += exp(exponents[i]); + T sum = 0; + for(size_t i = 0; i < exponents.size(); i++) + sum += exp(exponents[i]); - return max_exponent + log(sum); + return max_exponent + log(sum); } template class LogSumExp { public: - LogSumExp(){} - ~LogSumExp(){} + LogSumExp(){} + ~LogSumExp(){} - void add_exponent(T exponent) - { - exponents_.push_back(exponent); - } + void add_exponent(T exponent) + { + exponents_.push_back(exponent); + } - T Compute() - { - T max_exponent = -std::numeric_limits::max(); - for(int i = 0; i < int(exponents_.size()); i++) - if(exponents_[i] > max_exponent) max_exponent = exponents_[i]; + T Compute() + { + T max_exponent = -std::numeric_limits::max(); + for(int i = 0; i < int(exponents_.size()); i++) + if(exponents_[i] > max_exponent) max_exponent = exponents_[i]; - for(int i = 0; i < int(exponents_.size()); i++) - exponents_[i] -= max_exponent; + for(int i = 0; i < int(exponents_.size()); i++) + exponents_[i] -= max_exponent; - T sum = 0; - for(int i = 0; i < int(exponents_.size()); i++) - sum += exp(exponents_[i]); + T sum = 0; + for(int i = 0; i < int(exponents_.size()); i++) + sum += exp(exponents_[i]); - return max_exponent + log(sum); - } + return max_exponent + log(sum); + } private: - std::vector exponents_; + std::vector exponents_; }; // sampling class >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> @@ -991,8 +991,8 @@ class DiscreteSampler { public: template DiscreteSampler(std::vector log_prob) - { - fibo_.seed(RANDOM_SEED); + { + fibo_.seed(RANDOM_SEED); // compute the prob and normalize them sorted_indices_ = dbot::hf::SortDescend(log_prob); @@ -1001,12 +1001,12 @@ class DiscreteSampler log_prob[i] -= max; std::vector prob(log_prob.size()); - double sum = 0; + double sum = 0; for(int i = 0; i < int(log_prob.size()); i++) - { + { prob[i] = exp(log_prob[i]); sum += prob[i]; - } + } for(int i = 0; i < int(prob.size()); i++) prob[i] /= sum; @@ -1015,18 +1015,18 @@ class DiscreteSampler cumulative_prob_[0] = prob[sorted_indices_[0]]; for(int i = 1; i < int(log_prob.size()); i++) cumulative_prob_[i] = cumulative_prob_[i-1] + prob[sorted_indices_[i]]; - } + } - ~DiscreteSampler() {} + ~DiscreteSampler() {} int Sample() - { + { double uniform_sample = fibo_(); - int sample_index = 0; + int sample_index = 0; while(uniform_sample > cumulative_prob_[sample_index]) sample_index++; - return sorted_indices_[sample_index]; - } + return sorted_indices_[sample_index]; + } @@ -1042,8 +1042,8 @@ class DiscreteSampler } private: - boost::lagged_fibonacci607 fibo_; - std::vector sorted_indices_; + boost::lagged_fibonacci607 fibo_; + std::vector sorted_indices_; std::vector cumulative_prob_; }; @@ -1137,149 +1137,149 @@ class DiscreteDistribution template void Vector2TranslAndQuat(const std::vector &v, Eigen::Matrix &t, Eigen::Quaternion &q) { - Eigen::Quaternion quat(); - q.w() = v[0]; q.x() = v[1]; q.y() = v[2]; q.z() = v[3]; - t(0) = v[4]; t(1) = v[5]; t(2) = v[6]; + Eigen::Quaternion quat(); + q.w() = v[0]; q.x() = v[1]; q.y() = v[2]; q.z() = v[3]; + t(0) = v[4]; t(1) = v[5]; t(2) = v[6]; } template void TranslAndQuat2Vector(const Eigen::Quaternion &q, const Eigen::Matrix &t, std::vector &v) { - v.resize(7); + v.resize(7); - v[0] = q.w(); v[1] = q.x(); v[2] = q.y(); v[3] = q.z(); - v[4] = t(0); v[5] = t(1); v[6] = t(2); + v[0] = q.w(); v[1] = q.x(); v[2] = q.y(); v[3] = q.z(); + v[4] = t(0); v[5] = t(1); v[6] = t(2); } template void Vector2TranslAndRot(const std::vector &v, Eigen::Matrix &R, Eigen::Matrix &t) { - Eigen::Quaternion q; + Eigen::Quaternion q; - Vector2QuatAndTransl(v, q, t); - R = q; + Vector2QuatAndTransl(v, q, t); + R = q; } template void TranslAndRot2Vector(const Eigen::Matrix &R, const Eigen::Matrix &t, std::vector &v) { - const Eigen::Quaternion q(R); + const Eigen::Quaternion q(R); - QuatAndTransl2Vector(q, t, v); + QuatAndTransl2Vector(q, t, v); } inline Eigen::VectorXd OldState2NewState(const std::vector& old_state) { - Eigen::VectorXd new_state(old_state.size() * 2 - 1); // the new state also contains the velocity - new_state(0) = old_state[4]; - new_state(1) = old_state[5]; - new_state(2) = old_state[6]; + Eigen::VectorXd new_state(old_state.size() * 2 - 1); // the new state also contains the velocity + new_state(0) = old_state[4]; + new_state(1) = old_state[5]; + new_state(2) = old_state[6]; - new_state(3) = old_state[1]; - new_state(4) = old_state[2]; - new_state(5) = old_state[3]; - new_state(6) = old_state[0]; + new_state(3) = old_state[1]; + new_state(4) = old_state[2]; + new_state(5) = old_state[3]; + new_state(6) = old_state[0]; // new_state.topRows(3) += Eigen::Quaterniond(new_state.middleRows<4>(3))._transformVector(center); - new_state.middleRows(7, 6) = Eigen::VectorXd::Zero(6); + new_state.middleRows(7, 6) = Eigen::VectorXd::Zero(6); - // we fill in the joint angles - for(size_t i = 7; i < old_state.size(); i++) - new_state(i + 6) = old_state[i]; - // we fill in the joint velocities - for(size_t i = old_state.size() + 6; i < size_t(new_state.rows()); i++) - new_state(i) = 0; + // we fill in the joint angles + for(size_t i = 7; i < old_state.size(); i++) + new_state(i + 6) = old_state[i]; + // we fill in the joint velocities + for(size_t i = old_state.size() + 6; i < size_t(new_state.rows()); i++) + new_state(i) = 0; - return new_state; + return new_state; } inline std::vector NewState2OldState(const Eigen::VectorXd& new_state) { - std::vector old_state((new_state.rows()+1)/2); + std::vector old_state((new_state.rows()+1)/2); - old_state[4] = new_state(0); - old_state[5] = new_state(1); - old_state[6] = new_state(2); + old_state[4] = new_state(0); + old_state[5] = new_state(1); + old_state[6] = new_state(2); - old_state[1] = new_state(3); - old_state[2] = new_state(4); - old_state[3] = new_state(5); - old_state[0] = new_state(6); + old_state[1] = new_state(3); + old_state[2] = new_state(4); + old_state[3] = new_state(5); + old_state[0] = new_state(6); - for(size_t i = 7; i < old_state.size(); i++) - old_state[i] = new_state(i+6); + for(size_t i = 7; i < old_state.size(); i++) + old_state[i] = new_state(i+6); - return old_state; + return old_state; } template void Vector2QuatAndTransl(const std::vector &v, Eigen::Quaternion &q, Eigen::Matrix &t) { - q.w() = v[0]; q.x() = v[1]; q.y() = v[2]; q.z() = v[3]; - t(0) = v[4]; t(1) = v[5]; t(2) = v[6]; + q.w() = v[0]; q.x() = v[1]; q.y() = v[2]; q.z() = v[3]; + t(0) = v[4]; t(1) = v[5]; t(2) = v[6]; } template void QuatAndTransl2Vector(const Eigen::Quaternion &q, const Eigen::Matrix &t, std::vector &v) { - v.resize(7); + v.resize(7); - v[0] = q.w(); v[1] = q.x(); v[2] = q.y(); v[3] = q.z(); - v[4] = t(0); v[5] = t(1); v[6] = t(2); + v[0] = q.w(); v[1] = q.x(); v[2] = q.y(); v[3] = q.z(); + v[4] = t(0); v[5] = t(1); v[6] = t(2); } template void Vector2RotAndTransl(const std::vector &v, Eigen::Matrix &R, Eigen::Matrix &t) { - Eigen::Quaternion q; + Eigen::Quaternion q; - Vector2QuatAndTransl(v, q, t); - R = q; + Vector2QuatAndTransl(v, q, t); + R = q; } template void RotAndTransl2Vector(const Eigen::Matrix &R, const Eigen::Matrix &t, std::vector &v) { - const Eigen::Quaternion q(R); + const Eigen::Quaternion q(R); - QuatAndTransl2Vector(q, t, v); + QuatAndTransl2Vector(q, t, v); } template void Vector2Hom(const std::vector &v, Eigen::Matrix &H) { - Eigen::Matrix R; - Eigen::Matrix t; - Vector2RotAndTransl(v, R, t); + Eigen::Matrix R; + Eigen::Matrix t; + Vector2RotAndTransl(v, R, t); - H.topLeftCorner(3,3) = R; - H.topRightCorner(3,1) = t; - H.row(3) = Eigen::Matrix (0,0,0,1); + H.topLeftCorner(3,3) = R; + H.topRightCorner(3,1) = t; + H.row(3) = Eigen::Matrix (0,0,0,1); } template void Hom2Vector(const Eigen::Matrix &H, std::vector &v) { - const Eigen::Matrix R(H.topLeftCorner(3,3)); - const Eigen::Matrix t(H.topRightCorner(3,1)); + const Eigen::Matrix R(H.topLeftCorner(3,3)); + const Eigen::Matrix t(H.topRightCorner(3,1)); - RotAndTransl2Vector(R, t, v); + RotAndTransl2Vector(R, t, v); } template void Vector2Affine(const std::vector &v, Eigen::Transform &A) { - Eigen::Matrix H; - Vector2Hom(v, H); - A = H; + Eigen::Matrix H; + Vector2Hom(v, H); + A = H; } template void Affine2Vector(const Eigen::Transform &A, std::vector &v) { - Eigen::Matrix H; - H = A.matrix(); - Hom2Vector(H, v); + Eigen::Matrix H; + H = A.matrix(); + Hom2Vector(H, v); } } From c0d98baa4cd157e26b5f1f80347e5446bec6e6a5 Mon Sep 17 00:00:00 2001 From: issac Date: Sun, 13 Sep 2015 16:49:16 +0200 Subject: [PATCH 056/131] publishing pose and image cache --- .../depth_pixel_observation_model.hpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp index 0db143e..de7a2f3 100644 --- a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp +++ b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp @@ -157,7 +157,6 @@ class DepthPixelObservationModel return fg_density_; } - Obsrv depth(const State& current_state) const { if (render_cache_.find(current_state) == render_cache_.end()) @@ -170,6 +169,8 @@ class DepthPixelObservationModel current_state.position() + nominal_pose_.position(); map(current_pose, render_cache_[current_state]); + + render_poses_[current_state] = current_pose; } assert (render_cache_.find(current_state) != render_cache_.end()); @@ -181,7 +182,7 @@ class DepthPixelObservationModel } /** \endcond */ -public: +private: int state_dim_; mutable Gaussian fg_density_; @@ -190,15 +191,22 @@ class DepthPixelObservationModel mutable std::vector depth_rendering_; std::shared_ptr renderer_; +private: + int id_; + mutable State nominal_pose_; + +public: mutable std::unordered_map< State, Eigen::VectorXd, PoseHash > render_cache_; -public: - int id_; - mutable State nominal_pose_; + mutable std::unordered_map< + State, + State, + PoseHash + > render_poses_; }; } From c040ecbc349f111de15618e28af392faa7f9dc4a Mon Sep 17 00:00:00 2001 From: issac Date: Fri, 25 Sep 2015 14:53:15 +0200 Subject: [PATCH 057/131] noexcept default destructors --- .../kinect_image_observation_model_cpu.hpp | 2 +- .../observation_models/kinect_pixel_observation_model.hpp | 2 +- .../observation_models/rao_blackwell_observation_model.hpp | 2 +- .../models/process_models/brownian_object_motion_model.hpp | 2 +- .../models/process_models/damped_wiener_process_model.hpp | 2 +- .../integrated_damped_wiener_process_model.hpp | 2 +- .../dbot/models/process_models/occlusion_process_model.hpp | 6 +++--- include/dbot/rao_blackwell_coordinate_particle_filter.hpp | 2 +- include/dbot/states/free_floating_rigid_bodies_state.hpp | 2 +- include/dbot/states/rigid_bodies_state.hpp | 2 +- 10 files changed, 12 insertions(+), 12 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index d7de0de..13123c5 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -125,7 +125,7 @@ class KinectImageObservationModelCPU: reset(); } - ~KinectImageObservationModelCPU() { } + ~KinectImageObservationModelCPU() noexcept { } RealArray loglikes(const StateArray& deltas, IntArray& indices, diff --git a/include/dbot/models/observation_models/kinect_pixel_observation_model.hpp b/include/dbot/models/observation_models/kinect_pixel_observation_model.hpp index 9bc039f..b737757 100644 --- a/include/dbot/models/observation_models/kinect_pixel_observation_model.hpp +++ b/include/dbot/models/observation_models/kinect_pixel_observation_model.hpp @@ -89,7 +89,7 @@ class KinectPixelObservationModel } - virtual ~KinectPixelObservationModel() {} + virtual ~KinectPixelObservationModel() noexcept {} virtual Scalar Probability(const Observation& observation) const { diff --git a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp index fc689e7..8e46ecf 100644 --- a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp +++ b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp @@ -65,7 +65,7 @@ class RBObservationModel public: /// constructor and destructor ********************************************* RBObservationModel(const fl::Real& delta_time): delta_time_(delta_time) { } - virtual ~RBObservationModel() { } + virtual ~RBObservationModel() noexcept { } /// likelihood computation ************************************************* virtual RealArray loglikes(const StateArray& deviations, diff --git a/include/dbot/models/process_models/brownian_object_motion_model.hpp b/include/dbot/models/process_models/brownian_object_motion_model.hpp index 302db17..1442087 100644 --- a/include/dbot/models/process_models/brownian_object_motion_model.hpp +++ b/include/dbot/models/process_models/brownian_object_motion_model.hpp @@ -143,7 +143,7 @@ class BrownianObjectMotionModel: // angular_process_.resize(count_objects); } - virtual ~BrownianObjectMotionModel() { } + virtual ~BrownianObjectMotionModel() noexcept { } virtual State MapStandardGaussian(const Noise& sample) const { diff --git a/include/dbot/models/process_models/damped_wiener_process_model.hpp b/include/dbot/models/process_models/damped_wiener_process_model.hpp index 3979e08..74af53e 100644 --- a/include/dbot/models/process_models/damped_wiener_process_model.hpp +++ b/include/dbot/models/process_models/damped_wiener_process_model.hpp @@ -119,7 +119,7 @@ class DampedWienerProcessModel static_assert_base(State, Eigen::Matrix); } - virtual ~DampedWienerProcessModel() { } + virtual ~DampedWienerProcessModel() noexcept { } virtual State MapStandardGaussian(const Noise& sample) const { diff --git a/include/dbot/models/process_models/integrated_damped_wiener_process_model.hpp b/include/dbot/models/process_models/integrated_damped_wiener_process_model.hpp index 5d52494..21abc99 100644 --- a/include/dbot/models/process_models/integrated_damped_wiener_process_model.hpp +++ b/include/dbot/models/process_models/integrated_damped_wiener_process_model.hpp @@ -141,7 +141,7 @@ class IntegratedDampedWienerProcessModel "Dimension must be a multitude of 2"); } - virtual ~IntegratedDampedWienerProcessModel() { } + virtual ~IntegratedDampedWienerProcessModel() noexcept { } virtual State MapStandardGaussian(const Noise& sample) const { diff --git a/include/dbot/models/process_models/occlusion_process_model.hpp b/include/dbot/models/process_models/occlusion_process_model.hpp index 8a48818..0002667 100644 --- a/include/dbot/models/process_models/occlusion_process_model.hpp +++ b/include/dbot/models/process_models/occlusion_process_model.hpp @@ -46,8 +46,8 @@ class OcclusionProcessModel // ,public GaussianMap { public: - // the prob of source being object given source was object one sec ago, - // and prob of source being object given one sec ago source was not object + // the prob of source being object given source was object one sec ago, + // and prob of source being object given one sec ago source was not object OcclusionProcessModel(double p_occluded_visible, double p_occluded_occluded): p_occluded_visible_(p_occluded_visible), @@ -55,7 +55,7 @@ class OcclusionProcessModel c_(p_occluded_occluded_ - p_occluded_visible_), log_c_(std::log(c_)) { } - virtual ~OcclusionProcessModel() {} + virtual ~OcclusionProcessModel() noexcept {} virtual void Condition(const double& delta_time, const double& occlusion_probability, diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index 45dbcf2..6d45e3e 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -94,7 +94,7 @@ class RBCoordinateParticleFilter exit(-1); } } - virtual ~RBCoordinateParticleFilter() {} + virtual ~RBCoordinateParticleFilter() noexcept {} /// the filter functions *************************************************** void filter(const Observation& observation, diff --git a/include/dbot/states/free_floating_rigid_bodies_state.hpp b/include/dbot/states/free_floating_rigid_bodies_state.hpp index 78eaea8..2a328ed 100644 --- a/include/dbot/states/free_floating_rigid_bodies_state.hpp +++ b/include/dbot/states/free_floating_rigid_bodies_state.hpp @@ -80,7 +80,7 @@ class FreeFloatingRigidBodiesState: template FreeFloatingRigidBodiesState(const Eigen::MatrixBase& state_vector): Base(state_vector) { } - virtual ~FreeFloatingRigidBodiesState() {} + virtual ~FreeFloatingRigidBodiesState() noexcept {} // accessors *************************************************************** virtual fl::PoseVelocityVector component(int index) const diff --git a/include/dbot/states/rigid_bodies_state.hpp b/include/dbot/states/rigid_bodies_state.hpp index 1bf71d6..dee18fe 100644 --- a/include/dbot/states/rigid_bodies_state.hpp +++ b/include/dbot/states/rigid_bodies_state.hpp @@ -53,7 +53,7 @@ class RigidBodiesState: public Eigen::Matrix *this = state_vector; } - virtual ~RigidBodiesState() {} + virtual ~RigidBodiesState() noexcept {} template void operator = (const Eigen::MatrixBase& state_vector) From 2f00a6636c01f15ece98d2636478cc4781bddcea Mon Sep 17 00:00:00 2001 From: issac Date: Fri, 25 Sep 2015 14:53:58 +0200 Subject: [PATCH 058/131] Put maps on heap --- .../depth_pixel_observation_model.hpp | 60 ++++++++++++++----- 1 file changed, 46 insertions(+), 14 deletions(-) diff --git a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp index de7a2f3..83c6bed 100644 --- a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp +++ b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp @@ -21,6 +21,7 @@ #pragma once +#include #include #include #include @@ -52,6 +53,18 @@ class DepthPixelObservationModel typedef Vector1d Noise; typedef State_ State; + typedef std::unordered_map< + State, + State, + PoseHash + > PoseCacheMap; + + typedef std::unordered_map< + State, + Eigen::VectorXd, + PoseHash + > RenderCacheMap; + public: DepthPixelObservationModel( const std::shared_ptr& renderer, @@ -63,6 +76,11 @@ class DepthPixelObservationModel renderer_(renderer), id_(0) { + mutex = std::make_shared(); + render_cache_ = std::make_shared(); + poses_cache_ = std::make_shared(); + + // setup backgroud density auto bg_mean = Obsrv(1); @@ -75,6 +93,22 @@ class DepthPixelObservationModel fg_density_.square_root(fg_density_.square_root() * fg_sigma); } + DepthPixelObservationModel(const DepthPixelObservationModel& other) + { + state_dim_ = other.state_dim_; + renderer_ = other.renderer_; + id_ = other.id_; + bg_density_ = other.bg_density_; + fg_density_ = other.fg_density_; + mutex = other.mutex; + nominal_pose_ = other.nominal_pose_; + render_cache_ = other.render_cache_; + poses_cache_ = other.poses_cache_; + } + + + virtual ~DepthPixelObservationModel() noexcept { } + Real log_probability(const Obsrv& obsrv, const State& state) const override { return density(state).log_probability(obsrv); @@ -100,7 +134,9 @@ class DepthPixelObservationModel void nominal_pose(const State& p) { - render_cache_.clear(); + std::lock_guard lock(*mutex); + + render_cache_->clear(); nominal_pose_= p; } @@ -144,6 +180,7 @@ class DepthPixelObservationModel } } + const Gaussian& density(const State& state) const { Obsrv y = depth(state); @@ -159,6 +196,9 @@ class DepthPixelObservationModel Obsrv depth(const State& current_state) const { + RenderCacheMap& render_cache_ = *this->render_cache_; + PoseCacheMap& poses_cache_ = *this->poses_cache_; + if (render_cache_.find(current_state) == render_cache_.end()) { State current_pose; @@ -168,9 +208,9 @@ class DepthPixelObservationModel current_pose.position() = current_state.position() + nominal_pose_.position(); + std::lock_guard lock(*mutex); map(current_pose, render_cache_[current_state]); - - render_poses_[current_state] = current_pose; + poses_cache_[current_state] = current_pose; } assert (render_cache_.find(current_state) != render_cache_.end()); @@ -188,6 +228,7 @@ class DepthPixelObservationModel mutable Gaussian fg_density_; mutable Gaussian bg_density_; + mutable std::shared_ptr mutex; mutable std::vector depth_rendering_; std::shared_ptr renderer_; @@ -196,17 +237,8 @@ class DepthPixelObservationModel mutable State nominal_pose_; public: - mutable std::unordered_map< - State, - Eigen::VectorXd, - PoseHash - > render_cache_; - - mutable std::unordered_map< - State, - State, - PoseHash - > render_poses_; + mutable std::shared_ptr render_cache_; + mutable std::shared_ptr poses_cache_; }; } From 28cc593df22bcf6793d2d6923910b93e9766986a Mon Sep 17 00:00:00 2001 From: manuel Date: Fri, 25 Sep 2015 15:26:16 +0200 Subject: [PATCH 059/131] minor --- .../kinect_image_observation_model_gpu.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index c066768..bf841a9 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -95,7 +95,7 @@ class KinectImageObservationModelGPU: visibility_probs_.resize(n_rows_ * n_cols_); } - ~KinectImageObservationModelGPU() { } + ~KinectImageObservationModelGPU() noexcept { } // TODO: DO WE NEED TWO DIFFERENT FUNCTIONS FOR THIS?? From 0ad6b651236a391b246356ce522d164977f71cca Mon Sep 17 00:00:00 2001 From: issac Date: Fri, 2 Oct 2015 16:28:30 +0200 Subject: [PATCH 060/131] Removed profliling header and uniform obsrv model --- .../uniform_depth_pixel_observation_model.hpp | 92 ------------------- include/dbot/utils/profiling.hpp | 59 ------------ 2 files changed, 151 deletions(-) delete mode 100644 include/dbot/models/observation_models/uniform_depth_pixel_observation_model.hpp delete mode 100644 include/dbot/utils/profiling.hpp diff --git a/include/dbot/models/observation_models/uniform_depth_pixel_observation_model.hpp b/include/dbot/models/observation_models/uniform_depth_pixel_observation_model.hpp deleted file mode 100644 index d0ee7d1..0000000 --- a/include/dbot/models/observation_models/uniform_depth_pixel_observation_model.hpp +++ /dev/null @@ -1,92 +0,0 @@ -/* - * This is part of the FL library, a C++ Bayesian filtering library - * (https://github.com/filtering-library) - * - * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com) - * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com) - * - * Max-Planck Institute for Intelligent Systems, AMD Lab - * University of Southern California, CLMC Lab - * - * This Source Code Form is subject to the terms of the MIT License (MIT). - * A copy of the license can be found in the LICENSE file distributed with this - * source code. - */ - -/** - * \file uniform_depth_pixel_observation_model.hpp - * \date September 2015 - * \author Jan Issac (jan.issac@gmail.com) - */ - -#include -#include -#include - -#include - -#include -#include -#include -#include - -namespace fl -{ - -template -class UniformDepthPixelObservationModel - : public ObservationFunction, - public ObservationDensity, - public Descriptor -{ -public: - typedef Vector1d Obsrv; - typedef Vector1d Noise; - typedef State_ State; - -public: - UniformDepthPixelObservationModel( - Real min_depth, - Real max_depth, - int state_dim = DimensionOf::Value) - : state_dim_(state_dim), - density_(min_depth, max_depth) - { } - - Real log_probability(const Obsrv& obsrv, const State& state) const override - { - return density_.log_probability(obsrv); - } - - Real probability(const Obsrv& obsrv, const State& state) const override - { - return density_.probability(obsrv); - } - - Obsrv observation(const State& state, const Noise& noise) const override - { - Obsrv y = density_.map_standard_normal(noise); - return y; - } - - virtual int obsrv_dimension() const { return 1; } - virtual int noise_dimension() const { return 1; } - virtual int state_dimension() const { return state_dim_; } - - virtual std::string name() const - { - return "UniformDepthPixelObservationModel"; - } - - virtual std::string description() const - { - return "UniformDepthPixelObservationModel"; - } - - -private: - int state_dim_; - UniformDistribution density_; -}; - -} diff --git a/include/dbot/utils/profiling.hpp b/include/dbot/utils/profiling.hpp deleted file mode 100644 index f2ba816..0000000 --- a/include/dbot/utils/profiling.hpp +++ /dev/null @@ -1,59 +0,0 @@ -/************************************************************************* -This software allows for filtering in high-dimensional observation and -state spaces, as described in - -M. Wuthrich, P. Pastor, M. Kalakrishnan, J. Bohg, and S. Schaal. -Probabilistic Object Tracking using a Range Camera -IEEE/RSJ Intl Conf on Intelligent Robots and Systems, 2013 - -In a publication based on this software pleace cite the above reference. - - -Copyright (C) 2014 Manuel Wuthrich - -This program is free software: you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation, either version 3 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program. If not, see . -*************************************************************************/ - -#ifndef FAST_FILTERING_UTILS_PROFILING_HPP -#define FAST_FILTERING_UTILS_PROFILING_HPP - -#include -#include - - -// profiling macros -#define GET_TIME(time) {struct timeval profiling_time; gettimeofday(&profiling_time, NULL);\ - time = (profiling_time.tv_sec * 1000000u + profiling_time.tv_usec) /1000000.;} -#ifdef PROFILING_ON - #define PRINT(object) std::cout << object; - - #define INIT_PROFILING struct timeval profiling_start_time, profiling_end_time;\ - gettimeofday(&profiling_start_time, NULL); - #define RESET gettimeofday(&profiling_start_time, NULL); - #define MEASURE(text)\ - gettimeofday(&profiling_end_time, NULL);\ - std::cout << "time for " << text << " " \ - << std::setprecision(9) << std::fixed\ - << ((profiling_end_time.tv_sec - profiling_start_time.tv_sec) * 1000000u\ - + profiling_end_time.tv_usec - profiling_start_time.tv_usec) /1000000. \ - << " s" << std::endl; gettimeofday(&profiling_start_time, NULL); -#else - #define PRINT(object) - #define INIT_PROFILING - #define RESET - #define MEASURE(text) -#endif - - -#endif From 4b32c19b487b0614c253e3fac36d1ebd3d9898d4 Mon Sep 17 00:00:00 2001 From: issac Date: Fri, 2 Oct 2015 16:29:25 +0200 Subject: [PATCH 061/131] Updated package dependencies --- package.xml | 31 +++++-------------------------- 1 file changed, 5 insertions(+), 26 deletions(-) diff --git a/package.xml b/package.xml index 79006d9..1256ab9 100644 --- a/package.xml +++ b/package.xml @@ -1,48 +1,27 @@ dbot - 0.0.0 - The dbot package + 0.1.1 + The depth based object tracking package Jan Issac BSD - http://github.com/jan-issac/dbot + Jan Issac Manuel Wuthrich catkin - roscpp - roslib - sensor_msgs - VTK - OpenCV eigen - pcl - cv_bridge - urdf - orocos_kdl - kdl_parser - message_filters - image_transport fl + osr_catkin - roscpp - roslib - sensor_msgs - VTK - OpenCV eigen - pcl - urdf - orocos_kdl - kdl_parser - message_filters - image_transport fl + osr_catkin From 10392d884c22bffc5825bed569f3fdb7a2acc6f1 Mon Sep 17 00:00:00 2001 From: issac Date: Fri, 2 Oct 2015 16:29:36 +0200 Subject: [PATCH 062/131] namespace update --- include/dbot/utils/rigid_body_renderer.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/dbot/utils/rigid_body_renderer.hpp b/include/dbot/utils/rigid_body_renderer.hpp index ec10b41..7250288 100644 --- a/include/dbot/utils/rigid_body_renderer.hpp +++ b/include/dbot/utils/rigid_body_renderer.hpp @@ -33,7 +33,7 @@ along with this program. If not, see . #include #include -#include +#include namespace dbot @@ -43,7 +43,7 @@ class RigidBodyRenderer { public: typedef boost::shared_ptr Ptr; - typedef dbot::RigidBodiesState State; + typedef osr::RigidBodiesState State; typedef Eigen::Vector3d Vector; typedef Eigen::Matrix3d Matrix; typedef typename Eigen::Transform Affine; From b132320cb5b7753b9409c70dbe5d8af7519a0f55 Mon Sep 17 00:00:00 2001 From: issac Date: Fri, 2 Oct 2015 16:29:48 +0200 Subject: [PATCH 063/131] Moved to osr package --- .../free_floating_rigid_bodies_state.hpp | 131 -- include/dbot/states/rigid_bodies_state.hpp | 72 - include/dbot/utils/helper_functions.hpp | 1289 ----------------- include/dbot/utils/pose_hashing.hpp | 87 -- 4 files changed, 1579 deletions(-) delete mode 100644 include/dbot/states/free_floating_rigid_bodies_state.hpp delete mode 100644 include/dbot/states/rigid_bodies_state.hpp delete mode 100644 include/dbot/utils/helper_functions.hpp delete mode 100644 include/dbot/utils/pose_hashing.hpp diff --git a/include/dbot/states/free_floating_rigid_bodies_state.hpp b/include/dbot/states/free_floating_rigid_bodies_state.hpp deleted file mode 100644 index 2a328ed..0000000 --- a/include/dbot/states/free_floating_rigid_bodies_state.hpp +++ /dev/null @@ -1,131 +0,0 @@ -/************************************************************************* -This software allows for filtering in high-dimensional observation and -state spaces, as described in - -M. Wuthrich, P. Pastor, M. Kalakrishnan, J. Bohg, and S. Schaal. -Probabilistic Object Tracking using a Range Camera -IEEE/RSJ Intl Conf on Intelligent Robots and Systems, 2013 - -In a publication based on this software pleace cite the above reference. - - -Copyright (C) 2014 Manuel Wuthrich - -This program is free software: you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation, either version 3 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program. If not, see . -*************************************************************************/ - - -#ifndef POSE_TRACKING_STATES_FREE_FLOATING_RIGID_BODIES_STATE_HPP -#define POSE_TRACKING_STATES_FREE_FLOATING_RIGID_BODIES_STATE_HPP - -#include -#include - -#include - -#include - - -namespace dbot -{ - -template -struct FreeFloatingRigidBodiesStateTypes -{ - enum - { - BODY_SIZE = 12, - POSE_SIZE = 6, - }; - - typedef RigidBodiesState Base; -}; - - -template -class FreeFloatingRigidBodiesState: - public FreeFloatingRigidBodiesStateTypes::Base -{ -public: - typedef FreeFloatingRigidBodiesStateTypes Types; - typedef typename Types::Base Base; - enum - { - BODY_SIZE = Types::BODY_SIZE, - POSE_SIZE = Types::POSE_SIZE, - }; - - typedef typename Base::State State; - typedef Eigen::Matrix Poses; - typedef typename Base::PoseVelocityBlock PoseVelocityBlock; - - - // constructor and destructor ********************************************** - FreeFloatingRigidBodiesState() { } - FreeFloatingRigidBodiesState(unsigned count_bodies): - Base(State::Zero(count_bodies * BODY_SIZE)) { } - - template - FreeFloatingRigidBodiesState(const Eigen::MatrixBase& state_vector): - Base(state_vector) { } - virtual ~FreeFloatingRigidBodiesState() noexcept {} - - // accessors *************************************************************** - virtual fl::PoseVelocityVector component(int index) const - { - return PoseVelocityBlock(*((State*)(this)), - index * PoseVelocityBlock::SizeAtCompileTime); - } - virtual Poses poses() const - { - Poses poses_(count()*POSE_SIZE); - for(int body_index = 0; body_index < count(); body_index++) - { - poses_.template middleRows(body_index * POSE_SIZE) - = component(body_index).pose(); - } - return poses_; - } - - - - int count() const - { - return this->size() / PoseVelocityBlock::SizeAtCompileTime; - } - - // mutators **************************************************************** - PoseVelocityBlock component(int index) - { - return PoseVelocityBlock(*((State*)(this)), - index * PoseVelocityBlock::SizeAtCompileTime); - } - virtual void poses(const Poses& poses_) - { - for(int body_index = 0; body_index < count(); body_index++) - { - component(body_index).pose() - = poses_.template middleRows(body_index * POSE_SIZE); - } - } - void recount(int new_count) - { - return this->resize(new_count * PoseVelocityBlock::SizeAtCompileTime); - } -}; - -} - -#endif diff --git a/include/dbot/states/rigid_bodies_state.hpp b/include/dbot/states/rigid_bodies_state.hpp deleted file mode 100644 index dee18fe..0000000 --- a/include/dbot/states/rigid_bodies_state.hpp +++ /dev/null @@ -1,72 +0,0 @@ -/************************************************************************* -This software allows for filtering in high-dimensional observation and -state spaces, as described in - -M. Wuthrich, P. Pastor, M. Kalakrishnan, J. Bohg, and S. Schaal. -Probabilistic Object Tracking using a Range Camera -IEEE/RSJ Intl Conf on Intelligent Robots and Systems, 2013 - -In a publication based on this software pleace cite the above reference. - - -Copyright (C) 2014 Manuel Wuthrich - -This program is free software: you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation, either version 3 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program. If not, see . -*************************************************************************/ - -#ifndef POSE_TRACKING_STATES_RIGID_BODIES_STATE_HPP -#define POSE_TRACKING_STATES_RIGID_BODIES_STATE_HPP - -#include -#include -#include - -namespace dbot -{ - -template -class RigidBodiesState: public Eigen::Matrix -{ -public: - typedef double Scalar; - typedef Eigen::Matrix State; - typedef Eigen::Matrix Vector; - - typedef fl::PoseVelocityBlock PoseVelocityBlock; - - // constructor and destructor - RigidBodiesState() { } - template - RigidBodiesState(const Eigen::MatrixBase& state_vector) - { - *this = state_vector; - } - - virtual ~RigidBodiesState() noexcept {} - - template - void operator = (const Eigen::MatrixBase& state_vector) - { - *((State*)(this)) = state_vector; - } - - - virtual fl::PoseVelocityVector component(int index) const = 0; - - virtual int count() const = 0; -}; - -} - -#endif diff --git a/include/dbot/utils/helper_functions.hpp b/include/dbot/utils/helper_functions.hpp deleted file mode 100644 index 691aad8..0000000 --- a/include/dbot/utils/helper_functions.hpp +++ /dev/null @@ -1,1289 +0,0 @@ -/************************************************************************* -This software allows for filtering in high-dimensional observation and -state spaces, as described in - -M. Wuthrich, P. Pastor, M. Kalakrishnan, J. Bohg, and S. Schaal. -Probabilistic Object Tracking using a Range Camera -IEEE/RSJ Intl Conf on Intelligent Robots and Systems, 2013 - -In a publication based on this software pleace cite the above reference. - - -Copyright (C) 2014 Manuel Wuthrich - -This program is free software: you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation, either version 3 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program. If not, see . -*************************************************************************/ - -#ifndef FAST_FILTERING_UTILS_HELPER_FUNCTIONS_HPP -#define FAST_FILTERING_UTILS_HELPER_FUNCTIONS_HPP - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - - -#include -#include - -#include - - - -// TODO: THIS HAS TO BE CLEANED, POSSIBLY SPLIT INTO SEVERAL FILES - -namespace dbot -{ - -namespace hf -{ - - - - -inline double Sigmoid(const double& x) -{ - return 1.0 / (1.0 + std::exp(-x)); -} -inline double Logit(const double& x) -{ - return std::log(x / (1.0 - x)); -} - - -inline std::string DateAndTimeString() -{ - time_t rawtime; - struct tm * timeinfo; - char buffer[80]; - - time (&rawtime); - timeinfo = localtime(&rawtime); - - strftime(buffer,80,"%d.%m.%Y_%I.%M.%S",timeinfo); - std::string current_time(buffer); - - return current_time; -} - - - -inline double get_wall_time(){ - struct timeval time; - if (gettimeofday(&time,NULL)){ - std::cout << "WARNING: gettimeofday() Error" << std::endl; - return 0; - } - return (double)time.tv_sec + (double)time.tv_usec * .000001; -} - - -template void Compare(const std::vector vector_1, - const std::vector vector_2, - char* name_1, - char* name_2) { - if (vector_1.size() == vector_2.size()) { - T difference = 0; - for (uint i = 0; i < vector_1.size(); i++) { - std::cout << name_1 << "[" << i << "]: " << vector_1[i] << ", " - << name_2 << "[" << i << "]: " << vector_2[i] << ", " - << "difference: " << std::abs(vector_1[i] - vector_2[i]) << std::endl; - difference += std::abs(vector_1[i] - vector_2[i]); - } - std::cout << "average difference between " << name_1 << " and " << name_2 << " : " << difference / vector_1.size() << std::endl; - - } else { - std::cout << "WARNING: The two vectors to be compared have a different amount of elements. " << std::endl - << name_1 << ": " << vector_1.size() << ", " << name_2 << ": " << vector_2.size() << std::endl - << "Comparison aborted." << std::endl; - } -} - -template void AverageDifference(const std::vector vector_1, - const std::vector vector_2, - char* name_1, - char* name_2) { - if (vector_1.size() == vector_2.size()) { - T difference = 0; - for (uint i = 0; i < vector_1.size(); i++) { - difference += std::abs(vector_1[i] - vector_2[i]); - } - std::cout << "average difference between " << name_1 << " and " << name_2 << " : " << difference / vector_1.size() << std::endl; - - } else { - std::cout << "WARNING: The two vectors to be compared have a different amount of elements. " << std::endl - << name_1 << ": " << vector_1.size() << ", " << name_2 << ": " << vector_2.size() << std::endl - << "Comparison aborted." << std::endl; - } -} - -template void Output(const T value, - char* name) { - std::cout << name << ": " << value << std::endl; -} - - -template -int IndexNextReal( - const std::vector& data, - const int& current_index = -1, - const int& step_size = 1) -{ - int index_next_real = current_index + step_size; - while( - index_next_real < int(data.size()) && - index_next_real >= 0 && - !std::isfinite(data[index_next_real])) index_next_real+=step_size; - - return index_next_real; -} - -// various useful functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -class Structurer2D -{ -public: - Structurer2D(){} - - ~Structurer2D() {} - - template std::vector Flatten(const std::vector >& deep_structure) - { - sizes_.resize(deep_structure.size()); - size_t global_size = 0; - for(size_t i = 0; i < deep_structure.size(); i++) - { - sizes_[i] = deep_structure[i].size(); - global_size += deep_structure[i].size(); - } - - std::vector flat_structure(global_size); - size_t global_index = 0; - for(size_t i = 0; i < deep_structure.size(); i++) - for(size_t j = 0; j < deep_structure[i].size(); j++) - flat_structure[global_index++] = deep_structure[i][j]; - - return flat_structure; - } - - template std::vector > Deepen(const std::vector& flat_structure) - { - std::vector > deep_structure(sizes_.size()); - - size_t global_index = 0; - for(size_t i = 0; i < deep_structure.size(); i++) - { - deep_structure[i].resize(sizes_[i]); - for(size_t j = 0; j < deep_structure[i].size(); j++) - deep_structure[i][j] = flat_structure[global_index++]; - } - return deep_structure; - } - -private: - std::vector sizes_; -}; - - - -class Structurer3D -{ -public: - Structurer3D(){} - - ~Structurer3D() {} - - template std::vector Flatten(const std::vector > >& deep_structure) - { - size_t global_size = 0; - sizes_.resize(deep_structure.size()); - for(size_t i = 0; i < deep_structure.size(); i++) - { - sizes_[i].resize(deep_structure[i].size()); - for(size_t j = 0; j < deep_structure[i].size(); j++) - { - sizes_[i][j] = deep_structure[i][j].size(); - global_size += deep_structure[i][j].size(); - } - } - - std::vector flat_structure(global_size); - size_t global_index = 0; - for(size_t i = 0; i < deep_structure.size(); i++) - for(size_t j = 0; j < deep_structure[i].size(); j++) - for(size_t k = 0; k < deep_structure[i][j].size(); k++) - flat_structure[global_index++] = deep_structure[i][j][k]; - - return flat_structure; - } - - template std::vector > > Deepen(const std::vector& flat_structure) - { - size_t global_index = 0; - std::vector > > deep_structure(sizes_.size()); - for(size_t i = 0; i < deep_structure.size(); i++) - { - deep_structure[i].resize(sizes_[i].size()); - for(size_t j = 0; j < deep_structure[i].size(); j++) - { - deep_structure[i][j].resize(sizes_[i][j]); - for(size_t k = 0; k < deep_structure[i][j].size(); k++) - deep_structure[i][j][k] = flat_structure[global_index++]; - } - } - return deep_structure; - } - -private: - std::vector > sizes_; -}; - - - - - - - -// this function will interpolat the vector wherever it is NAN or INF -template -void LinearlyInterpolate(std::vector& data) -{ - std::vector limits; - limits.push_back(0); - limits.push_back(data.size()-1); - std::vector step_direction; - step_direction.push_back(1); - step_direction.push_back(-1); - - // extrapolate - for(int i = 0; i < 2; i++) - { - int index_first_real = IndexNextReal(data, limits[i] - step_direction[i], step_direction[i]); - int index_next_real = IndexNextReal(data, index_first_real, step_direction[i]); - if(index_next_real >= int(data.size()) || index_next_real < 0) - return; - - double slope = - double(data[index_next_real] - data[index_first_real]) / - double(index_next_real - index_first_real); - - for(int j = limits[i]; j != index_first_real; j += step_direction[i]) - data[j] = data[index_first_real] + (j - index_first_real) * slope; - } - - // interpolate - int index_current_real = IndexNextReal(data, limits[0] - step_direction[0], step_direction[0]); - int index_next_real = IndexNextReal(data, index_current_real, step_direction[0]); - - while(index_next_real < int(data.size())) - { - double slope = - double(data[index_next_real] - data[index_current_real]) / - double(index_next_real - index_current_real); - - for(int i = index_current_real + 1; i < index_next_real; i++) - data[i] = data[index_current_real] + (i - index_current_real) * slope; - - index_current_real = index_next_real; - index_next_real = IndexNextReal(data, index_next_real, step_direction[0]); - } -} - -template void PrintVector(std::vector v) -{ - for(size_t i = 0; i < v.size(); i++) - std::cout << "(" << i << ": " << v[i] << ") "; - std::cout << std::endl; -} -template void PrintVector(std::vector > v) -{ - for(size_t i = 0; i < v.size(); i++) - { - std::cout << i << " --------------------------------" << std::endl; - PrintVector(v[i]); - } -} - -template void PrintVector(std::vector > > v) -{ - for(size_t i = 0; i < v.size(); i++) - { - std::cout << i << " ================================" << std::endl; - PrintVector(v[i]); - } -} - - -template -void Swap(T& a, T& b) -{ - T temp = a; - a = b; - b = temp; -} - -template -Eigen::Matrix Std2Eigen(const std::vector &std) -{ - Eigen::Matrix eigen(std.size()); - for(size_t i = 0; i < std.size(); i++) - eigen(i) = std[i]; - return eigen; -} - -template -std::string Eigen2Mathematica(const Eigen::MatrixBase& eigen, const std::string &name = "") -{ - std::string mathematica; - if(!name.empty()) - mathematica += name + " = "; - - mathematica += "{"; - for(int row = 0; row < eigen.rows(); row++) - { - mathematica += "{"; - for(int col = 0; col < eigen.cols(); col++) - { - mathematica += boost::lexical_cast(eigen(row, col)); - if(col != eigen.cols() -1) - mathematica += ", "; - } - mathematica += "}"; - if(row != eigen.rows()-1) - mathematica += ", "; - } - mathematica += "}"; - if(!name.empty()) - mathematica += ";"; - - return mathematica; -} - -// returns the first index where the two vectors differ, if there is no difference then -1 is returned. -template int -DifferenceAt( - const std::vector >& a, - const std::vector >& b, - const T& epsilon) -{ - if(a.size() < b.size()) - return a.size(); - else if(b.size() < a.size()) - return b.size(); - - for(size_t i = 0; i < a.size(); i++) - for(size_t row = 0; row < n_rows; row++) - for(size_t col = 0; col < n_cols; col++) - { - if(isnan(a[i](row, col)) && isnan(b[i](row, col))) - continue; - else if(isnan(a[i](row, col)) || isnan(b[i](row, col))) - return i; - else if( (a[i](row, col) - b[i](row, col) > epsilon) || (b[i](row, col) - a[i](row, col) > epsilon) ) - return i; - } - - return -1; -} - -// returns the first index where the two vectors differ, if there is no difference then -1 is returned. -template int -DifferenceAt( - const std::vector& a, - const std::vector& b, - const T& epsilon) -{ - if(a.size() < b.size()) - return a.size(); - else if(b.size() < a.size()) - return b.size(); - - for(size_t i = 0; i < a.size(); i++) - { - if(isnan(a[i]) && isnan(b[i])) - continue; - else if(isnan(a[i]) || isnan(b[i])) - return i; - else if( (a[i] - b[i] > epsilon) || (b[i] - a[i] > epsilon) ) - return i; - } - - return -1; -} - -template std::vector ReverseOrder(std::vector ordered) -{ - std::vector reversed(ordered.size()); - for(size_t i = 0; i < ordered.size(); i++) - reversed[i] = ordered[ordered.size() - 1 - i]; -} - -template std::vector Count(T from, T to, T increment = 1) -{ - std::vector count(size_t((to - from)/increment)); - count[0] = from; - for(size_t i = 1; i < count.size(); i++) - count[i] = count[i-1] + increment; -} - -template struct ValuesIndex -{ - std::vector values; - int index; - - bool operator < (const ValuesIndex& right_side) const - { - for(size_t i = 0; i < values.size(); i++) - { - if(values[i] < right_side.values[i]) - return true; - if(values[i] > right_side.values[i]) - return false; - } - return false; - } - - - bool operator != (const ValuesIndex& right_side) const - { - for(size_t i = 0; i < values.size(); i++) - if(values[i] != right_side.values[i]) - return true; - - return false; - } -}; - -template void -SortAndCollapse( - std::vector >& values, - std::vector& multiplicities) -{ - std::vector > values_indices(values.size()); - for(size_t i = 0; i < values.size(); i++) - { - values_indices[i].index = i; - values_indices[i].values = values[i]; - } - - std::sort(values_indices.begin(), values_indices.end()); - - std::vector > temp_values = values; - multiplicities = std::vector(values.size(), 0); - size_t distinct_index = 0; - values[0] = temp_values[values_indices[0].index]; - for(size_t i = 0; i < values.size(); i++) - { - if(i > 0 && temp_values[values_indices[i-1].index] != temp_values[values_indices[i].index]) - values[++distinct_index] = temp_values[values_indices[i].index]; - multiplicities[distinct_index]++; - } - values.resize(distinct_index+1); - multiplicities.resize(distinct_index+1); -} - - - - - - - - - - -template struct ValueIndex -{ - T value; - int index; - - bool operator < (const ValueIndex& str) const - { - return (value < str.value); - } -}; -template std::vector SortAscend(const std::vector &values) -{ - std::vector indices(values.size()); - - std::vector > values_indices(values.size()); - for(int i = 0; i < int(values.size()); i++) - { - values_indices[i].index = i; - values_indices[i].value = values[i]; - } - - std::sort(values_indices.begin(), values_indices.end()); - - for(int i = 0; i < int(indices.size()); i++) - indices[i] = values_indices[i].index; - - return indices; -} - -template std::vector SortDescend(const std::vector &values) -{ - std::vector ascend_indices = SortAscend(values); - std::vector descend_indices(ascend_indices.size()); - - for(int i = 0; i < int(ascend_indices.size()); i++) - descend_indices[i] = ascend_indices[ascend_indices.size()-1-i]; - - return descend_indices; -} - - -template void Sort(std::vector &ref_values, bool order = 0) -{ - std::vector temp_ref_values = ref_values; - std::vector indices; - if(order == 0) - indices = SortAscend(temp_ref_values); - else - indices = SortDescend(temp_ref_values); - - - for(int i = 0; i < int(indices.size()); i++) - ref_values[i] = temp_ref_values[indices[i]]; -} - - -template void SortAscend(std::vector &ref_values, std::vector &values) -{ - std::vector temp_ref_values = ref_values; - std::vector temp_values = values; - - std::vector indices = SortAscend(temp_ref_values); - - for(int i = 0; i < int(indices.size()); i++) - { - ref_values[i] = temp_ref_values[indices[i]]; - values[i] = temp_values[indices[i]]; - } -} - -template void SortDescend(std::vector &ref_values, std::vector &values) -{ - std::vector temp_ref_values = ref_values; - std::vector temp_values = values; - - std::vector indices = SortDescend(temp_ref_values); - - for(int i = 0; i < int(indices.size()); i++) - { - ref_values[i] = temp_ref_values[indices[i]]; - values[i] = temp_values[indices[i]]; - } -} - -template void SortAscend(std::vector &ref_values, - std::vector &values1, std::vector &values2) -{ - std::vector temp_ref_values = ref_values; - std::vector temp_values1 = values1; - std::vector temp_values2 = values2; - - std::vector indices = SortAscend(temp_ref_values); - - for(int i = 0; i < int(indices.size()); i++) - { - ref_values[i] = temp_ref_values[indices[i]]; - values1[i] = temp_values1[indices[i]]; - values2[i] = temp_values2[indices[i]]; - } -} - -template void SortDescend(std::vector &ref_values, - std::vector &values1, std::vector &values2) -{ - std::vector temp_ref_values = ref_values; - std::vector temp_values1 = values1; - std::vector temp_values2 = values2; - - std::vector indices = SortDescend(temp_ref_values); - - for(int i = 0; i < int(indices.size()); i++) - { - ref_values[i] = temp_ref_values[indices[i]]; - values1[i] = temp_values1[indices[i]]; - values2[i] = temp_values2[indices[i]]; - } -} - -template int BoundIndex(const std::vector &values, bool bound_type) // bound type 1 for max and 0 for min -{ - int BoundIndex = 0; - T bound_value = bound_type ? -std::numeric_limits::max() : std::numeric_limits::max(); - - for(int i = 0; i < int(values.size()); i++) - if(bound_type ? (values[i] > bound_value) : (values[i] < bound_value) ) - { - BoundIndex = i; - bound_value = values[i]; - } - - return BoundIndex; -} - -template T bound_value(const std::vector &values, bool bound_type) // bound type 1 for max and 0 for min -{ - return values[BoundIndex(values, bound_type)]; -} - -template -std::vector Apply(const std::vector &input, Tout(*f)(Tin)) -{ - std::vector output(input.size()); - for(size_t i = 0; i < output.size(); i++) - output[i] = (*f)(input[i]); - - return output; -} - -template std::vector -SetSum(const std::vector &input, T sum) -{ - T old_sum = 0; - for(size_t i = 0; i < input.size(); i++) - old_sum += input[i]; - T factor = sum/old_sum; - - std::vector output(input.size()); - for(size_t i = 0; i < input.size(); i++) - output[i] = factor*input[i]; - - return output; -} - -// geometry functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -template -Eigen::Quaternion Delta2Quaternion(const Eigen::MatrixBase& delta) -{ - typename T::Scalar angle = delta.norm(); - Eigen::Matrix axis = delta.normalized(); - Eigen::Quaternion q; - if(std::isfinite(axis.norm())) - q = Eigen::AngleAxisd(angle, axis); - else - q = Eigen::Quaterniond::Identity(); - - return q; -} - -// this function finds the intersection between two lines. a is a point on line and d_a is the direction -// vector of the line. if lines do not cross, this will return the point which minimizes the squared -// distance between the two lines. if they are parallel, then inf will be returned. -template -Eigen::Matrix Intersection( - const Eigen::MatrixBase& a, - const Eigen::MatrixBase& d_a_in, - const Eigen::MatrixBase& b, - const Eigen::MatrixBase& d_b_in) -{ - const Eigen::Matrix d_a = d_a_in.normalized(); - const Eigen::Matrix d_b = d_b_in.normalized(); - - double c_a = d_a.dot((b-a) - d_b.dot(b-a)*d_b)/ - (1.-pow(d_b.dot(d_a),2)); - double c_b = d_b.dot((a-b) - d_a.dot(a-b)*d_a)/ - (1.-pow(d_a.dot(d_b),2)); - - return((b + c_b*d_b)+(a + c_a*d_a))/2.; -} - -template -typename T1::Scalar Angle( - const Eigen::MatrixBase& a, - const Eigen::MatrixBase& b) -{ - return atan2(a.cross(b).norm(), a.dot(b)); -} - -template -Eigen::Matrix OrthogonalUnitVector(const Eigen::MatrixBase& v) -{ - Eigen::Matrix n = - (v.cross(Eigen::Matrix(1.0, 0, 0))).normalized(); - if(!std::isfinite(n.norm())) - n = (v.cross(Eigen::Matrix(0, 1.0, 0))).normalized(); - - return n; -} - -// returns the angle and axis which rotate a onto b -template -Eigen::AngleAxis AngleAxis( - const Eigen::MatrixBase& a, - const Eigen::MatrixBase& b) -{ - Eigen::Matrix axis = a.cross(b); - typename T1::Scalar axis_norm = axis.norm(); - axis /= axis_norm; - - // if the axis is inf then the angle is either 0 or pi, so the axis has to be just some orthogonal vector - if(!std::isfinite(axis.norm())) - axis = OrthogonalUnitVector(a); - - typename T1::Scalar angle = atan2(axis_norm, a.dot(b)); - - return Eigen::AngleAxis(angle, axis); -} - -// the input quaternion has the order xyzw -inline Eigen::Matrix QuaternionMatrix(const Eigen::Matrix& q_xyzw) -{ - Eigen::Matrix Q; - Q << q_xyzw(3), q_xyzw(2), -q_xyzw(1), - -q_xyzw(2), q_xyzw(3), q_xyzw(0), - q_xyzw(1), -q_xyzw(0), q_xyzw(3), - -q_xyzw(0), -q_xyzw(1), -q_xyzw(2); - return 0.5*Q; -} - -template class TransformationSequence -{ -public: - TransformationSequence( - const Eigen::Matrix &R = Eigen::Matrix::Identity(), - const Eigen::Matrix &t = Eigen::Matrix::Zero()): R_(R), t_(t) {} - - ~TransformationSequence(){} - - void PreRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) - { - t_ = t_ - R_*R*c + R_*c; - R_ = R_*R; - } - void PreTranslate(const Eigen::Matrix &t) - { - t_ = t_ + R_*t; - } - void PostRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) - { - t_ = R*t_ - R*c + c; - R_ = R*R_; - } - void PostTranslate(const Eigen::Matrix &t) - { - t_ = t_ + t; - } - - void get(Eigen::Matrix &R, Eigen::Matrix &t) - { - R = R_; - t = t_; - } - - void NormalizeQuat() - { - Eigen::Quaternion q; q = R_; - q.normalize(); - R_ = q.toRotationMatrix(); - } - -private: - Eigen::Matrix R_; - Eigen::Matrix t_; - -}; - -// depth image functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -// converts from cartesian to image space -template Eigen::Matrix -CartCoord2ImageCoord(const Eigen::Matrix& cart, - const Eigen::Matrix& camera_matrix) -{ - return (camera_matrix * cart/cart(2)).topRows(2); -} - -// converts from cartesian space to image index, (row, col) -template Eigen::Matrix -CartCoord2ImageIndex(const Eigen::Matrix& cart, - const Eigen::Matrix& camera_matrix) -{ - Eigen::Matrix image = CartCoord2ImageCoord(cart, camera_matrix); - Eigen::Matrix image_index; - image_index(0) = floor(image(1)+0.5); - image_index(1) = floor(image(0)+0.5); - - return image_index; -} - -// converts from image coordinates and depth (z value) to cartesian coordinates -template Eigen::Matrix -ImageCoord2CartCoord(const Eigen::Matrix& image, - const T& depth, - const Eigen::Matrix& camera_matrix_inverse) -{ - Eigen::Matrix image_augmented; - image_augmented.topRows(2) = image; - image_augmented(2) = 1; - - return depth * camera_matrix_inverse * image_augmented; -} - -// converts from image index (row, col) and depth (z value) to cartesian coordinates -template Eigen::Matrix -ImageIndex2CartCoord(const Eigen::Matrix& image_index, - const T& depth, - const Eigen::Matrix& camera_matrix_inverse) -{ - Eigen::Matrix image; - image(0) = image_index(1); - image(1) = image_index(0); - return ImageCoord2CartCoord(image, depth, camera_matrix_inverse); -} - - -template Eigen::Matrix , -1, -1> -Image2Points(const Eigen::Matrix& image, - const Eigen::Matrix& camera_matrix) -{ - Eigen::Matrix camera_matrix_inverse = camera_matrix.inverse(); - - Eigen::Matrix , -1, -1> points(image.rows(), image.cols()); - for(size_t row = 0; row < points.rows(); row++) - for(size_t col = 0; col < points.cols(); col++) - points(row, col) = ImageIndex2CartCoord(Eigen::Vector2i(row, col), - image(row, col), - camera_matrix_inverse); - - return points; -} - - - - -template std::vector > -DepthImage2CartVectors(const std::vector& image, - const size_t& n_rows, const size_t& n_cols, - const Eigen::Matrix& camera_matrix) -{ - Eigen::Matrix camera_matrix_inverse = camera_matrix.inverse(); - - std::vector > vectors(n_rows*n_cols); - for(size_t row = 0; row < n_rows; row++) - for(size_t col = 0; col < n_cols; col++) - vectors[row*n_cols + col] = - ImageIndex2CartCoord( - Eigen::Vector2i(row, col), - image[row*n_cols + col], - camera_matrix_inverse); - - return vectors; -} - -template std::vector -CartVectors2DepthImage( - const std::vector >& vectors, - const int& n_rows, const int& n_cols, - const Eigen::Matrix& camera_matrix) -{ - std::vector image(n_rows*n_cols, NAN); - - for(size_t i = 0; i < vectors.size(); i++) - { - if( isnan(vectors[i](0)) || isnan(vectors[i](1)) || isnan(vectors[i](2)) ) - continue; - - Eigen::Matrix index = CartCoord2ImageIndex(vectors[i], camera_matrix); - - if(index(0) < 0 || index(0) >= n_rows || index(1) < 0 || index(1) >= n_cols) - { - std::cout << "ERROR: in CartVectors2DepthImage vector comes to lie outside of image " << std::endl; - exit(-1); - } - - image[index(0)*n_cols + index(1)] = vectors[i](2); - } - - return image; -} - - - -// numerically stable implementation of log(sum(exp(xi))) >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -inline double log_sum_exp(double a, double b) -{ - if(a>b) return a + log(1+exp(b-a)); - else return b + log(1+exp(a-b)); -} -inline double log_sum_exp(double a, double b, double c) -{ - if(a>b && a>c) return a + log(1+exp(b-a)+exp(c-a)); - if(b>c && b>a) return b + log(1+exp(a-b)+exp(c-b)); - else return c + log(1+exp(a-c)+exp(b-c)); -} -inline double log_sum_exp(double a, double b, double c, double d) -{ - if(a>b && a>c && a>d) return a + log(1+exp(b-a)+exp(c-a)+exp(d-a)); - if(b>a && b>c && b>d) return b + log(1+exp(a-b)+exp(c-b)+exp(d-b)); - if(c>a && c>b && c>d) return c + log(1+exp(a-c)+exp(b-c)+exp(d-c)); - else return d + log(1+exp(a-d)+exp(b-d)+exp(c-d)); -} - -template T log_sum_exp(std::vector exponents) -{ - T max_exponent = -std::numeric_limits::max(); - for(size_t i = 0; i < exponents.size(); i++) - if(exponents[i] > max_exponent) max_exponent = exponents[i]; - - for(size_t i = 0; i < exponents.size(); i++) - exponents[i] -= max_exponent; - - T sum = 0; - for(size_t i = 0; i < exponents.size(); i++) - sum += exp(exponents[i]); - - return max_exponent + log(sum); -} - - -template class LogSumExp -{ -public: - LogSumExp(){} - ~LogSumExp(){} - - void add_exponent(T exponent) - { - exponents_.push_back(exponent); - } - - T Compute() - { - T max_exponent = -std::numeric_limits::max(); - for(int i = 0; i < int(exponents_.size()); i++) - if(exponents_[i] > max_exponent) max_exponent = exponents_[i]; - - for(int i = 0; i < int(exponents_.size()); i++) - exponents_[i] -= max_exponent; - - T sum = 0; - for(int i = 0; i < int(exponents_.size()); i++) - sum += exp(exponents_[i]); - - return max_exponent + log(sum); - } -private: - std::vector exponents_; -}; - -// sampling class >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -class DiscreteSampler -{ -public: - template DiscreteSampler(std::vector log_prob) - { - fibo_.seed(RANDOM_SEED); - - // compute the prob and normalize them - sorted_indices_ = dbot::hf::SortDescend(log_prob); - double max = log_prob[sorted_indices_[0]]; - for(int i = 0; i < int(log_prob.size()); i++) - log_prob[i] -= max; - - std::vector prob(log_prob.size()); - double sum = 0; - for(int i = 0; i < int(log_prob.size()); i++) - { - prob[i] = exp(log_prob[i]); - sum += prob[i]; - } - for(int i = 0; i < int(prob.size()); i++) - prob[i] /= sum; - - // compute the cumulative probability - cumulative_prob_.resize(log_prob.size()); - cumulative_prob_[0] = prob[sorted_indices_[0]]; - for(int i = 1; i < int(log_prob.size()); i++) - cumulative_prob_[i] = cumulative_prob_[i-1] + prob[sorted_indices_[i]]; - } - - ~DiscreteSampler() {} - - int Sample() - { - double uniform_sample = fibo_(); - int sample_index = 0; - while(uniform_sample > cumulative_prob_[sample_index]) sample_index++; - - return sorted_indices_[sample_index]; - } - - - - int MapStandardGaussian(double gaussian_sample) const - { - // map from a gaussian to a uniform distribution - double uniform_sample = 0.5 * - (1.0 + std::erf(gaussian_sample / std::sqrt(2.0))); - int sample_index = 0; - while(uniform_sample > cumulative_prob_[sample_index]) sample_index++; //TODO: THIS COULD BE DONE IN LOG TIME - - return sorted_indices_[sample_index]; - } - -private: - boost::lagged_fibonacci607 fibo_; - std::vector sorted_indices_; - std::vector cumulative_prob_; -}; - - - -// the above class should go away -class DiscreteDistribution -{ -public: - template DiscreteDistribution(std::vector log_prob) - { - uniform_sampler_.seed(RANDOM_SEED); - - // substract max to avoid numerical issues - double max = hf::bound_value(log_prob, true); - for(int i = 0; i < int(log_prob.size()); i++) - log_prob[i] -= max; - - // compute probabilities - std::vector prob(log_prob.size()); - double sum = 0; - for(int i = 0; i < int(log_prob.size()); i++) - { - prob[i] = std::exp(log_prob[i]); - sum += prob[i]; - } - for(int i = 0; i < int(prob.size()); i++) - prob[i] /= sum; - - // compute the cumulative probability - cumulative_prob_.resize(prob.size()); - cumulative_prob_[0] = prob[0]; - for(size_t i = 1; i < prob.size(); i++) - cumulative_prob_[i] = cumulative_prob_[i-1] + prob[i]; - } - - ~DiscreteDistribution() {} - - int Sample() - { - return MapStandardUniform(uniform_sampler_()); - } - - int MapStandardGaussian(double gaussian_sample) const - { - double uniform_sample = - 0.5 * (1.0 + std::erf(gaussian_sample / std::sqrt(2.0))); - return MapStandardUniform(uniform_sample); - } - - int MapStandardUniform(double uniform_sample) const - { - std::vector::const_iterator iterator = - std::lower_bound(cumulative_prob_.begin(), - cumulative_prob_.end(), - uniform_sample); - - return iterator - cumulative_prob_.begin(); - } - -private: - boost::lagged_fibonacci607 uniform_sampler_; - std::vector cumulative_prob_; -}; - - - - - - - - - - - - - - - - - - - - - - - - - -// todo this stuff should be removed >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -template void -Vector2TranslAndQuat(const std::vector &v, Eigen::Matrix &t, Eigen::Quaternion &q) -{ - Eigen::Quaternion quat(); - q.w() = v[0]; q.x() = v[1]; q.y() = v[2]; q.z() = v[3]; - t(0) = v[4]; t(1) = v[5]; t(2) = v[6]; -} - -template void -TranslAndQuat2Vector(const Eigen::Quaternion &q, const Eigen::Matrix &t, std::vector &v) -{ - v.resize(7); - - v[0] = q.w(); v[1] = q.x(); v[2] = q.y(); v[3] = q.z(); - v[4] = t(0); v[5] = t(1); v[6] = t(2); -} - -template void -Vector2TranslAndRot(const std::vector &v, Eigen::Matrix &R, Eigen::Matrix &t) -{ - Eigen::Quaternion q; - - Vector2QuatAndTransl(v, q, t); - R = q; -} - -template void -TranslAndRot2Vector(const Eigen::Matrix &R, const Eigen::Matrix &t, std::vector &v) -{ - const Eigen::Quaternion q(R); - - QuatAndTransl2Vector(q, t, v); -} - -inline Eigen::VectorXd OldState2NewState(const std::vector& old_state) -{ - Eigen::VectorXd new_state(old_state.size() * 2 - 1); // the new state also contains the velocity - new_state(0) = old_state[4]; - new_state(1) = old_state[5]; - new_state(2) = old_state[6]; - - new_state(3) = old_state[1]; - new_state(4) = old_state[2]; - new_state(5) = old_state[3]; - new_state(6) = old_state[0]; - -// new_state.topRows(3) += Eigen::Quaterniond(new_state.middleRows<4>(3))._transformVector(center); - new_state.middleRows(7, 6) = Eigen::VectorXd::Zero(6); - - // we fill in the joint angles - for(size_t i = 7; i < old_state.size(); i++) - new_state(i + 6) = old_state[i]; - // we fill in the joint velocities - for(size_t i = old_state.size() + 6; i < size_t(new_state.rows()); i++) - new_state(i) = 0; - - return new_state; -} - -inline std::vector NewState2OldState(const Eigen::VectorXd& new_state) -{ - std::vector old_state((new_state.rows()+1)/2); - - old_state[4] = new_state(0); - old_state[5] = new_state(1); - old_state[6] = new_state(2); - - old_state[1] = new_state(3); - old_state[2] = new_state(4); - old_state[3] = new_state(5); - old_state[0] = new_state(6); - - for(size_t i = 7; i < old_state.size(); i++) - old_state[i] = new_state(i+6); - - return old_state; -} - -template void -Vector2QuatAndTransl(const std::vector &v, Eigen::Quaternion &q, Eigen::Matrix &t) -{ - q.w() = v[0]; q.x() = v[1]; q.y() = v[2]; q.z() = v[3]; - t(0) = v[4]; t(1) = v[5]; t(2) = v[6]; -} - -template void -QuatAndTransl2Vector(const Eigen::Quaternion &q, const Eigen::Matrix &t, std::vector &v) -{ - v.resize(7); - - v[0] = q.w(); v[1] = q.x(); v[2] = q.y(); v[3] = q.z(); - v[4] = t(0); v[5] = t(1); v[6] = t(2); -} - -template void -Vector2RotAndTransl(const std::vector &v, Eigen::Matrix &R, Eigen::Matrix &t) -{ - Eigen::Quaternion q; - - Vector2QuatAndTransl(v, q, t); - R = q; -} - -template void -RotAndTransl2Vector(const Eigen::Matrix &R, const Eigen::Matrix &t, std::vector &v) -{ - const Eigen::Quaternion q(R); - - QuatAndTransl2Vector(q, t, v); -} - -template void -Vector2Hom(const std::vector &v, Eigen::Matrix &H) -{ - Eigen::Matrix R; - Eigen::Matrix t; - Vector2RotAndTransl(v, R, t); - - H.topLeftCorner(3,3) = R; - H.topRightCorner(3,1) = t; - H.row(3) = Eigen::Matrix (0,0,0,1); -} - -template void -Hom2Vector(const Eigen::Matrix &H, std::vector &v) -{ - const Eigen::Matrix R(H.topLeftCorner(3,3)); - const Eigen::Matrix t(H.topRightCorner(3,1)); - - RotAndTransl2Vector(R, t, v); -} - -template void -Vector2Affine(const std::vector &v, Eigen::Transform &A) -{ - Eigen::Matrix H; - Vector2Hom(v, H); - A = H; -} - -template void -Affine2Vector(const Eigen::Transform &A, std::vector &v) -{ - Eigen::Matrix H; - H = A.matrix(); - Hom2Vector(H, v); -} - -} - -} - -#endif diff --git a/include/dbot/utils/pose_hashing.hpp b/include/dbot/utils/pose_hashing.hpp deleted file mode 100644 index 3279f35..0000000 --- a/include/dbot/utils/pose_hashing.hpp +++ /dev/null @@ -1,87 +0,0 @@ -/* - * This is part of the FL library, a C++ Bayesian filtering library - * (https://github.com/filtering-library) - * - * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com) - * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com) - * - * Max-Planck Institute for Intelligent Systems, AMD Lab - * University of Southern California, CLMC Lab - * - * This Source Code Form is subject to the terms of the MIT License (MIT). - * A copy of the license can be found in the LICENSE file distributed with this - * source code. - */ - -/** - * \date January 2015 - * \author Jan Issac (jan.issac@gmail.com) - */ - -#pragma once - - -#include -#include - -#include -#include -#include - -template class PoseHash; - -template <> class PoseHash -{ -public: - std::size_t operator()(const fl::PoseVector& s) const - { - /* primes */ - static constexpr int p1 = 15487457; - static constexpr int p2 = 24092821; - static constexpr int p3 = 73856093; - static constexpr int p4 = 19349663; - static constexpr int p5 = 83492791; - static constexpr int p6 = 17353159; - - /* map size */ - static constexpr int n = 1200; - - /* precision */ - static constexpr int c = 1000000; - - return ((int(s(0, 0) * c) * p1) ^ - (int(s(1, 0) * c) * p2) ^ - (int(s(2, 0) * c) * p3) ^ - (int(s(3, 0) * c) * p4) ^ - (int(s(4, 0) * c) * p5) ^ - (int(s(5, 0) * c) * p6) % n); - } -}; - -template <> class PoseHash -{ -public: - std::size_t operator()(const fl::PoseVelocityVector& s) const - { - /* primes */ - static constexpr int p1 = 15487457; - static constexpr int p2 = 24092821; - static constexpr int p3 = 73856093; - static constexpr int p4 = 19349663; - static constexpr int p5 = 83492791; - static constexpr int p6 = 17353159; - - /* map size */ - static constexpr int n = 1200; - - /* precision */ - static constexpr int c = 1000000; - - return ((int(s(0, 0) * c) * p1) ^ - (int(s(1, 0) * c) * p2) ^ - (int(s(2, 0) * c) * p3) ^ - (int(s(3, 0) * c) * p4) ^ - (int(s(4, 0) * c) * p5) ^ - (int(s(5, 0) * c) * p6) % n); - } -}; From eecf87b47773b27f457d49e355a4146b6f4d8d09 Mon Sep 17 00:00:00 2001 From: issac Date: Fri, 2 Oct 2015 16:30:27 +0200 Subject: [PATCH 064/131] cmake cleanup. removed old and unused stuff --- CMakeLists.txt | 282 +++++++++++++++++++------------------------------ 1 file changed, 109 insertions(+), 173 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2fcc751..5be3c5a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,194 +1,130 @@ cmake_minimum_required(VERSION 2.8.3) project(dbot) -set(CMAKE_BUILD_TYPE Release) -#set(CMAKE_BUILD_TYPE Debug) -add_definitions(-std=c++0x ) +############################ +# Options # +############################ +option(DBOT_ON_CUDA "Compile CUDA enabled trackers" ON) + +############################ +# Flags # +############################ +# Enable c++11 GCC 4.6 or greater required +add_definitions(-std=c++0x) add_definitions(-DPROFILING_ON=1) #print profiling output -set(USE_CUDA True) - -find_package(catkin REQUIRED - roscpp - roslib - sensor_msgs - cv_bridge - urdf - orocos_kdl - kdl_parser - rosbag - message_filters - image_transport - fl -) - - -set(PROJECT_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake") -set(CMAKE_MODULE_PATH ${PROJECT_MODULE_PATH}) -find_package(Boost REQUIRED) +add_definitions(-Wall) +add_definitions(-Wno-unused-local-typedefs) +add_definitions(-Wno-deprecated-declarations) +add_definitions(-Wno-comment) +# for eigen-3.1.2 +add_definitions(-Wno-deprecated-register) + +############################ +# Setup # +############################ +set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) + +############################ +# Dependencies ## +############################ find_package(Eigen REQUIRED) -find_package(OpenCV REQUIRED) -find_package(PCL 1.3 REQUIRED) - +include_directories(${Eigen_INCLUDE_DIRS}) +add_definitions(${Eigen_DEFINITIONS}) +find_package(Boost REQUIRED) +include_directories(${Boost_INCLUDE_DIRS}) -find_package(OpenMP) -if (OPENMP_FOUND) - set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") - set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -endif() +############################ +# GPU # +############################ +if(DBOT_ON_GPU) + set(GLEW_DIR ${CMAKE_MODULE_PATH}) -set(ccache_DIR ${PROJECT_MODULE_PATH}) -find_package(ccache REQUIRED) + find_package(CUDA REQUIRED) + find_package(GLEW REQUIRED) + find_package(OpenGL REQUIRED) -if(CCACHE_FOUND) - message(" ccache found. GCC will be launched via ccache.") - set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) - set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) -else(CCACHE_FOUND) - message(" ccache not found. ") -endif(CCACHE_FOUND) + include_directories(${GLEW_INCLUDE_DIRS}) + include_directories(${OpenGL_INCLUDE_DIRS}) + cuda_include_directories(${CUDA_CUT_INCLUDE_DIRS}) -include_directories(${Boost_INCLUDE_DIRS}) -include_directories(${Eigen_INCLUDE_DIRS}) -include_directories(${PCL_INCLUDE_DIRS}) -include_directories(${OpenCV_INCLUDE_DIRS}) - -######### -## GPU ## -######### -set(GLEW_DIR ${PROJECT_MODULE_PATH}) - -find_package(CUDA) -find_package(GLEW) -find_package(OpenGL) - -include_directories(${GLEW_INCLUDE_DIRS}) -include_directories(${OpenGL_INCLUDE_DIRS}) -cuda_include_directories(${CUDA_CUT_INCLUDE_DIRS}) - -link_directories(${OpenGL_LIBRARY_DIRS}) -link_directories(${GLEW_LIBRARY_DIRS}) - -add_definitions(${OpenGL_DEFINITIONS}) -add_definitions(${GLEW_DEFINITIONS}) - -if(NOT CUDA_FOUND) - message(" CUDA not found!") -else(NOT CUDA_FOUND) - message(" CUDA has been found") -endif(NOT CUDA_FOUND) - -if(NOT OPENGL_FOUND) - message(" OPENGL not found!") -else(NOT OPENGL_FOUND) - message(" OPENGL has been found!") -endif(NOT OPENGL_FOUND) - -if(NOT GLEW_FOUND) - message(" GLEW not found!") -else(NOT GLEW_FOUND) - message(" GLEW has been found!") -endif(NOT GLEW_FOUND) - - - -# enable cuda debug information with -g -G -O0, to use with cuda-dbg -# use --ptxas-options=-v to see number of registers, local, shared and constant memory used in kernels -set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} -O2 -arch=sm_20) - -################################### -## catkin specific configuration ## -################################### - -if(USE_CUDA AND CUDA_FOUND AND OPENGL_FOUND AND GLEW_FOUND) -catkin_package( - INCLUDE_DIRS - include - ${GLEW_INCLUDE_DIRS} - ${OpenGL_INCLUDE_DIRS} - ${CUDA_CUT_INCLUDE_DIRS} - ${CUDA_INCLUDE_DIRS} - LIBRARIES - dbot_cuda - dbot_gpu - dbot - CATKIN_DEPENDS - roscpp - roslib - sensor_msgs - urdf - orocos_kdl - kdl_parser - message_filters - image_transport - fl - DEPENDS - eigen) -else(USE_CUDA AND CUDA_FOUND AND OPENGL_FOUND AND GLEW_FOUND) -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - dbot - CATKIN_DEPENDS - roscpp - roslib - sensor_msgs - urdf - orocos_kdl - kdl_parser - message_filters - image_transport - fl - DEPENDS - eigen) -endif(USE_CUDA AND CUDA_FOUND AND OPENGL_FOUND AND GLEW_FOUND) -########### -## Build ## -########### - -include_directories(include ${catkin_INCLUDE_DIRS} ) - - -## dbot ####################################################################### -set(PROJECT_NAME_DBOT "dbot") - -file(GLOB_RECURSE dbot_headers include/${PROJECT_NAME_DBOT}/*.hpp - include/${PROJECT_NAME_DBOT}/*.h) -set(dbot_sources src/${PROJECT_NAME_DBOT}/utils/rigid_body_renderer.cpp) - - -add_library(${PROJECT_NAME_DBOT} ${dbot_headers} - ${dbot_sources}) - -target_link_libraries(${PROJECT_NAME_DBOT} ${catkin_LIBRARIES}) - - - -if(USE_CUDA AND CUDA_FOUND AND OPENGL_FOUND AND GLEW_FOUND) - add_definitions( -DBUILD_GPU=1 ) - message("building cuda implementation") + link_directories(${OpenGL_LIBRARY_DIRS}) + link_directories(${GLEW_LIBRARY_DIRS}) - set(dbot_gpu_sources - #src/${PROJECT_NAME_DBOT}/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_filter.cpp - #src/${PROJECT_NAME_DBOT}/models/observation_models/kinect_image_observation_model_gpu/cuda_opengl_multiple_filter.cpp - src/${PROJECT_NAME_DBOT}/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp - src/${PROJECT_NAME_DBOT}/models/observation_models/kinect_image_observation_model_gpu/shader.cpp) + add_definitions(${OpenGL_DEFINITIONS}) + add_definitions(${GLEW_DEFINITIONS}) - add_library(${PROJECT_NAME_DBOT}_gpu - ${dbot_gpu_sources}) + # enable cuda debug information with -g -G -O0, to use with cuda-dbg use + # --ptxas-options=-v to see number of registers, local, shared and constant + # memory used in kernels + set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} -O2 -arch=sm_20) - target_link_libraries(${PROJECT_NAME_DBOT}_gpu + add_definitions( -DBUILD_GPU=1 ) +endif(DBOT_ON_GPU) + +############################ +## catkin # +############################ +find_package(catkin REQUIRED fl osr_catkin) + +if(DBOT_ON_GPU) + catkin_package( + INCLUDE_DIRS + include + ${GLEW_INCLUDE_DIRS} + ${OpenGL_INCLUDE_DIRS} + ${CUDA_CUT_INCLUDE_DIRS} + ${CUDA_INCLUDE_DIRS} + LIBRARIES + dbot_gpu + dbot + CATKIN_DEPENDS + fl + osr_catkin + DEPENDS + eigen + ) +else(DBOT_ON_GPU) + catkin_package( + INCLUDE_DIRS + include + LIBRARIES + dbot + CATKIN_DEPENDS + fl + osr_catkin + DEPENDS + eigen + ) +endif(DBOT_ON_GPU) + +include_directories(include ${catkin_INCLUDE_DIRS}) + +############################ +## dbot library # +############################ +file(GLOB_RECURSE dbot_headers include/${PROJECT_NAME}/*.hpp + include/${PROJECT_NAME}/*.h) +set(dbot_sources src/${PROJECT_NAME}/utils/rigid_body_renderer.cpp) +add_library(${PROJECT_NAME} ${dbot_headers} ${dbot_sources}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +if(DBOT_ON_GPU) + set(GPU_IMPL_PATH + src/${PROJECT_NAME}/models/observation_models/kinect_image_observation_model_gpu) + + cuda_add_library(${PROJECT_NAME}_gpu + ${GPU_IMPL_PATH}/cuda_filter.cu + ${GPU_IMPL_PATH}/object_rasterizer.cpp + ${GPU_IMPL_PATH}/shader.cpp) + + target_link_libraries(${PROJECT_NAME}_gpu ${catkin_LIBRARIES} ${OPENGL_LIBRARIES} ${GLFW_LIBRARY} ${GLEW_LIBRARIES}) - - cuda_add_library(${PROJECT_NAME_DBOT}_cuda - src/${PROJECT_NAME_DBOT}/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.cu) - target_link_libraries(${PROJECT_NAME_DBOT} ${PROJECT_NAME_DBOT}_cuda) -endif(USE_CUDA AND CUDA_FOUND AND OPENGL_FOUND AND GLEW_FOUND) +endif(DBOT_ON_GPU) From 45e6bffe956d248db67fdd456ccc30f69b84accf Mon Sep 17 00:00:00 2001 From: issac Date: Fri, 2 Oct 2015 16:30:36 +0200 Subject: [PATCH 065/131] namespace update --- .../dbot/models/process_models/brownian_object_motion_model.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/dbot/models/process_models/brownian_object_motion_model.hpp b/include/dbot/models/process_models/brownian_object_motion_model.hpp index 1442087..c9b4235 100644 --- a/include/dbot/models/process_models/brownian_object_motion_model.hpp +++ b/include/dbot/models/process_models/brownian_object_motion_model.hpp @@ -51,7 +51,7 @@ #include #include -#include +#include #include #include From 3552b4277ad1d6c7b753011962c7b0509762d34d Mon Sep 17 00:00:00 2001 From: issac Date: Fri, 2 Oct 2015 16:31:15 +0200 Subject: [PATCH 066/131] headers update plus clang-format --- .../depth_pixel_observation_model.hpp | 51 +++----- .../cuda_filter.hpp | 118 +++++++++++------- ...o_blackwell_coordinate_particle_filter.hpp | 109 +++++++--------- .../object_rasterizer.cpp | 18 +-- 4 files changed, 147 insertions(+), 149 deletions(-) diff --git a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp index 83c6bed..7526a2b 100644 --- a/include/dbot/models/observation_models/depth_pixel_observation_model.hpp +++ b/include/dbot/models/observation_models/depth_pixel_observation_model.hpp @@ -36,12 +36,12 @@ #include #include -#include #include +#include + namespace fl { - template class DepthPixelObservationModel : public ObservationFunction, @@ -51,19 +51,12 @@ class DepthPixelObservationModel public: typedef Vector1d Obsrv; typedef Vector1d Noise; - typedef State_ State; + typedef State_ State; - typedef std::unordered_map< - State, - State, - PoseHash - > PoseCacheMap; + typedef std::unordered_map> PoseCacheMap; - typedef std::unordered_map< - State, - Eigen::VectorXd, - PoseHash - > RenderCacheMap; + typedef std::unordered_map> + RenderCacheMap; public: DepthPixelObservationModel( @@ -72,15 +65,12 @@ class DepthPixelObservationModel Real fg_sigma, Real bg_sigma, int state_dim = DimensionOf::Value) - : state_dim_(state_dim), - renderer_(renderer), - id_(0) + : state_dim_(state_dim), renderer_(renderer), id_(0) { mutex = std::make_shared(); render_cache_ = std::make_shared(); poses_cache_ = std::make_shared(); - // setup backgroud density auto bg_mean = Obsrv(1); @@ -106,9 +96,7 @@ class DepthPixelObservationModel poses_cache_ = other.poses_cache_; } - - virtual ~DepthPixelObservationModel() noexcept { } - + virtual ~DepthPixelObservationModel() noexcept {} Real log_probability(const Obsrv& obsrv, const State& state) const override { return density(state).log_probability(obsrv); @@ -128,23 +116,17 @@ class DepthPixelObservationModel virtual int obsrv_dimension() const { return 1; } virtual int noise_dimension() const { return 1; } virtual int state_dimension() const { return state_dim_; } - virtual int id() const { return id_; } virtual void id(int new_id) { id_ = new_id; } - void nominal_pose(const State& p) { std::lock_guard lock(*mutex); render_cache_->clear(); - nominal_pose_= p; - } - - virtual std::string name() const - { - return "DepthPixelObservationModel"; + nominal_pose_ = p; } + virtual std::string name() const { return "DepthPixelObservationModel"; } virtual std::string description() const { return "DepthPixelObservationModel"; @@ -160,9 +142,8 @@ class DepthPixelObservationModel convert(depth_rendering_, obsrv_image); } - void convert( - const std::vector& depth, - Eigen::VectorXd& obsrv_image) const + void convert(const std::vector& depth, + Eigen::VectorXd& obsrv_image) const { const int pixel_count = depth.size(); obsrv_image.resize(pixel_count, 1); @@ -180,7 +161,6 @@ class DepthPixelObservationModel } } - const Gaussian& density(const State& state) const { Obsrv y = depth(state); @@ -190,8 +170,8 @@ class DepthPixelObservationModel return bg_density_; } - fg_density_.mean(y); - return fg_density_; + fg_density_.mean(y); + return fg_density_; } Obsrv depth(const State& current_state) const @@ -213,7 +193,7 @@ class DepthPixelObservationModel poses_cache_[current_state] = current_pose; } - assert (render_cache_.find(current_state) != render_cache_.end()); + assert(render_cache_.find(current_state) != render_cache_.end()); Obsrv depth; depth(0) = render_cache_[current_state](id_); @@ -240,5 +220,4 @@ class DepthPixelObservationModel mutable std::shared_ptr render_cache_; mutable std::shared_ptr poses_cache_; }; - } diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp index 7515d8a..49b6568 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/cuda_filter.hpp @@ -11,49 +11,82 @@ #include #include -namespace fil { - +namespace fil +{ class CudaFilter { -public: + public: CudaFilter(); ~CudaFilter(); - void init(std::vector > com_models, float angle_sigma, float trans_sigma, - float p_visible_init, float c, float log_c, float p_visible_occluded, - float tail_weight, float model_sigma, float sigma_factor, float max_depth, float exponential_rate); + void init(std::vector > com_models, + float angle_sigma, + float trans_sigma, + float p_visible_init, + float c, + float log_c, + float p_visible_occluded, + float tail_weight, + float model_sigma, + float sigma_factor, + float max_depth, + float exponential_rate); // filter functions - void propagate(const float ¤t_time, std::vector > &states); // not used - void propagate_multiple(const float ¤t_time, std::vector > > &states); // not used - void compare(float observation_time, bool constant_occlusion, std::vector &log_likelihoods); // not used - void compare_multiple(bool update, std::vector &log_likelihoods); - void resample(std::vector resampling_indices); // not used - void resample_multiple(std::vector resampling_indices); // not used - + void propagate(const float& current_time, + std::vector >& states); // not used + void propagate_multiple( + const float& current_time, + std::vector > >& states); // not used + void compare(float observation_time, + bool constant_occlusion, + std::vector& log_likelihoods); // not used + void compare_multiple(bool update, std::vector& log_likelihoods); + void resample(std::vector resampling_indices); // not used + void resample_multiple(std::vector resampling_indices); // not used // setters - void set_states(std::vector > &states, int seed); // not needed if propagation not on GPU - void set_states_multiple(int n_objects, int n_features, int seed); // not needed if propagation not on GPU - void set_observations(const float* observations, const float observation_time); - void set_observations(const float* observations); // not used outside, can be integrated into above + void set_states(std::vector >& states, + int seed); // not needed if propagation not on GPU + void set_states_multiple(int n_objects, + int n_features, + int seed); // not needed if propagation not on GPU + void set_observations(const float* observations, + const float observation_time); + void set_observations(const float* observations); // not used outside, can + // be integrated into + // above void set_visibility_probabilities(const float* visibility_probabilities); void set_prev_sample_indices(const int* prev_sample_indices); - void set_resolution(const int n_rows, const int n_cols, int& nr_poses, int& nr_poses_per_row, int& nr_poses_per_column); + void set_resolution(const int n_rows, + const int n_cols, + int& nr_poses, + int& nr_poses_per_row, + int& nr_poses_per_column); void allocate_memory_for_max_poses(int& allocated_poses, int& allocated_poses_per_row, int& allocated_poses_per_column); - void set_number_of_poses(int& nr_poses, int& nr_poses_per_row, int& nr_poses_per_column); + void set_number_of_poses(int& nr_poses, + int& nr_poses_per_row, + int& nr_poses_per_column); void set_texture_array(cudaArray_t texture_array); // getters std::vector get_visibility_probabilities(int state_id); - std::vector > get_visibility_probabilities(); // returns all of them. Ask Manuel if they could need that. + std::vector > get_visibility_probabilities(); // returns + // all of + // them. + // Ask + // Manuel + // if they + // could + // need + // that. void map_texture(); void destroy_context(); -private: + private: // resolution values if not specified static const int WINDOW_WIDTH = 80; static const int WINDOW_HEIGHT = 60; @@ -66,16 +99,16 @@ class CudaFilter double copy_likelihoods_time_; // device pointers to arrays stored in global memory on the GPU - float *d_states_; // not needed if propagation not on GPU - float *d_states_copy_; // not needed if propagation not on GPU - float *d_visibility_probs_; - float *d_visibility_probs_copy_; - float *d_observations_; - float *d_log_likelihoods_; - int *d_prev_sample_indices_; - int *d_resampling_indices_; // not needed if resampling not on GPU - curandStateMRG32k3a *d_mrg_states_; - + float* d_states_; // not needed if propagation not on GPU + float* d_states_copy_; // not needed if propagation not on GPU + float* d_visibility_probs_; + float* d_visibility_probs_copy_; + float* d_observations_; + float* d_log_likelihoods_; + int* d_prev_sample_indices_; + int* d_resampling_indices_; // not needed if resampling not on GPU + curandStateMRG32k3a* d_mrg_states_; + // for OpenGL interop cudaArray_t d_texture_array_; @@ -100,7 +133,6 @@ class CudaFilter int nr_threads_, n_blocks_; dim3 grid_dimension_; - // system properties int warp_size_; int n_mps_; @@ -108,29 +140,31 @@ class CudaFilter // visibility prob default float visibility_prob_default_; - // time values to compute the time deltas when calling propagate() or evaluate() + // time values to compute the time deltas when calling propagate() or + // evaluate() float occlusion_time_; float observation_time_; // float delta_time_; - float last_propagation_time_; // not needed if propagation not on GPU + float last_propagation_time_; // not needed if propagation not on GPU - // booleans to describe the state of the cuda filter, to avoid wrong usage of the class + // booleans to describe the state of the cuda filter, to avoid wrong usage + // of the class bool n_poses_set_; // CUDA device properties cudaDeviceProp cuda_device_properties_; - - void set_default_kernel_config(int& nr_poses_, int& nr_poses_per_row, int& nr_poses_per_column, + void set_default_kernel_config(int& nr_poses_, + int& nr_poses_per_row, + int& nr_poses_per_column, bool& nr_poses_changed); // helper functions - template void allocate(T * &pointer, size_t size, std::string name); - void check_cuda_error(const char *msg); - + template + void allocate(T*& pointer, size_t size, std::string name); + void check_cuda_error(const char* msg); }; - } -#endif // CUDAFILTER_HPP +#endif // CUDAFILTER_HPP diff --git a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp index 6d45e3e..ee18923 100644 --- a/include/dbot/rao_blackwell_coordinate_particle_filter.hpp +++ b/include/dbot/rao_blackwell_coordinate_particle_filter.hpp @@ -39,29 +39,24 @@ along with this program. If not, see . #include #include #include +#include -#include #include -#include #include - - namespace dbot { - -template +template class RBCoordinateParticleFilter { public: - typedef typename ProcessModel::State State; - typedef typename ProcessModel::Input Input; - typedef typename ProcessModel::Noise Noise; - - typedef Eigen::Array StateArray; - typedef Eigen::Array RealArray; - typedef Eigen::Array IntArray; + typedef typename ProcessModel::State State; + typedef typename ProcessModel::Input Input; + typedef typename ProcessModel::Noise Noise; + typedef Eigen::Array StateArray; + typedef Eigen::Array RealArray; + typedef Eigen::Array IntArray; typedef typename ObservationModel::Observation Observation; @@ -70,23 +65,23 @@ class RBCoordinateParticleFilter public: /// constructor and destructor ********************************************* RBCoordinateParticleFilter( - const boost::shared_ptr process_model, - const boost::shared_ptr observation_model, - const std::vector >& sampling_blocks, - const fl::Real& max_kl_divergence = 0): - observation_model_(observation_model), - process_model_(process_model), - max_kl_divergence_(max_kl_divergence) + const boost::shared_ptr process_model, + const boost::shared_ptr observation_model, + const std::vector>& sampling_blocks, + const fl::Real& max_kl_divergence = 0) + : observation_model_(observation_model), + process_model_(process_model), + max_kl_divergence_(max_kl_divergence) { sampling_blocks_ = sampling_blocks; // make sure sizes are consistent -------------------------------------- size_t dimension = 0; - for(size_t i = 0; i < sampling_blocks_.size(); i++) + for (size_t i = 0; i < sampling_blocks_.size(); i++) { - dimension += sampling_blocks_[i].size(); + dimension += sampling_blocks_[i].size(); } - if(dimension != process_model_->noise_dimension()) + if (dimension != process_model_->noise_dimension()) { std::cout << "the dimension of the sampling blocks is " << dimension << " while the dimension of the noise is " @@ -95,48 +90,45 @@ class RBCoordinateParticleFilter } } virtual ~RBCoordinateParticleFilter() noexcept {} - /// the filter functions *************************************************** - void filter(const Observation& observation, - const Input& input) + void filter(const Observation& observation, const Input& input) { observation_model_->set_observation(observation); loglikes_ = RealArray::Zero(belief_.size()); - noises_ = std::vector(belief_.size(), - Noise::Zero(process_model_->noise_dimension())); + noises_ = std::vector( + belief_.size(), Noise::Zero(process_model_->noise_dimension())); old_particles_ = belief_.locations(); - for(size_t i_block = 0; i_block < sampling_blocks_.size(); i_block++) + for (size_t i_block = 0; i_block < sampling_blocks_.size(); i_block++) { // add noise of this block ----------------------------------------- - for(size_t i_sampl = 0; i_sampl < belief_.size(); i_sampl++) + for (size_t i_sampl = 0; i_sampl < belief_.size(); i_sampl++) { - for(size_t i = 0; i < sampling_blocks_[i_block].size(); i++) + for (size_t i = 0; i < sampling_blocks_[i_block].size(); i++) { noises_[i_sampl](sampling_blocks_[i_block][i]) = - unit_gaussian_.sample()(0); + unit_gaussian_.sample()(0); } } // propagate using partial noise ----------------------------------- - for(size_t i_sampl = 0; i_sampl < belief_.size(); i_sampl++) + for (size_t i_sampl = 0; i_sampl < belief_.size(); i_sampl++) { - belief_.location(i_sampl) = - process_model_->state(old_particles_[i_sampl], - noises_[i_sampl], input); + belief_.location(i_sampl) = process_model_->state( + old_particles_[i_sampl], noises_[i_sampl], input); } // compute likelihood ---------------------------------------------- - bool update = (i_block == sampling_blocks_.size()-1); - RealArray new_loglikes = - observation_model_->loglikes(belief_.locations(), indices_, update); + bool update = (i_block == sampling_blocks_.size() - 1); + RealArray new_loglikes = observation_model_->loglikes( + belief_.locations(), indices_, update); // update the weights and resample if necessary -------------------- belief_.delta_log_prob_mass(new_loglikes - loglikes_); loglikes_ = new_loglikes; - if(belief_.kl_given_uniform() > max_kl_divergence_) + if (belief_.kl_given_uniform() > max_kl_divergence_) { resample(belief_.size()); } @@ -152,34 +144,29 @@ class RBCoordinateParticleFilter Belief new_belief(sample_count); - for(size_t i = 0; i < sample_count; i++) + for (size_t i = 0; i < sample_count; i++) { int index; new_belief.location(i) = belief_.sample(index); - indices[i] = indices_[index]; - noises[i] = noises_[index]; + indices[i] = indices_[index]; + noises[i] = noises_[index]; next_samples[i] = old_particles_[index]; - loglikes[i] = loglikes_[index]; + loglikes[i] = loglikes_[index]; } - belief_ = new_belief; - indices_ = indices; - noises_ = noises; - old_particles_ = next_samples; - loglikes_ = loglikes; + belief_ = new_belief; + indices_ = indices; + noises_ = noises; + old_particles_ = next_samples; + loglikes_ = loglikes; } - /// mutators *************************************************************** - Belief& belief() - { - return belief_; - } - - void set_particles(const std::vector& samples) + Belief& belief() { return belief_; } + void set_particles(const std::vector& samples) { belief_.set_uniform(samples.size()); - for(int i = 0; i < belief_.size(); i++) + for (int i = 0; i < belief_.size(); i++) belief_.location(i) = samples[i]; indices_ = IntArray::Zero(samples.size()); @@ -196,7 +183,6 @@ class RBCoordinateParticleFilter return process_model_; } - private: /// member variables ******************************************************* Belief belief_; @@ -208,16 +194,15 @@ class RBCoordinateParticleFilter // models boost::shared_ptr observation_model_; - boost::shared_ptr process_model_; + boost::shared_ptr process_model_; // parameters - std::vector > sampling_blocks_; + std::vector> sampling_blocks_; fl::Real max_kl_divergence_; // distribution for sampling - fl::Gaussian > unit_gaussian_; + fl::Gaussian> unit_gaussian_; }; - } #endif diff --git a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp index d00b5df..2015752 100644 --- a/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp +++ b/src/dbot/models/observation_models/kinect_image_observation_model_gpu/object_rasterizer.cpp @@ -4,25 +4,25 @@ * of the different subroutines inside the render call. */ //#define PROFILING_ACTIVE + +#include #include -#include -#include -#include +#include +#include +#include #include -#include -#include -#include +#include +#include -#include +#include -#include +#include using namespace std; using namespace Eigen; - ObjectRasterizer::ObjectRasterizer() { } From 977e2ff7f69ee6f9a15a67f6e95477808265dd56 Mon Sep 17 00:00:00 2001 From: issac Date: Sat, 3 Oct 2015 21:35:01 +0200 Subject: [PATCH 067/131] moving away from state_filtering --- CMakeLists.txt | 2 + .../kinect_image_observation_model_cpu.hpp | 6 +- .../kinect_image_observation_model_gpu.hpp | 16 +- .../brownian_object_motion_model.hpp | 2 +- include/dbot/utils/helper_functions.hpp | 1289 +++++++++++++++++ 5 files changed, 1303 insertions(+), 12 deletions(-) create mode 100644 include/dbot/utils/helper_functions.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5be3c5a..66e718c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,7 @@ add_definitions(-Wno-deprecated-register) # Setup # ############################ set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) +set(CMAKE_EXPORT_COMPILE_COMMANDS 1) ############################ # Dependencies ## @@ -82,6 +83,7 @@ if(DBOT_ON_GPU) CATKIN_DEPENDS fl osr_catkin + dbot_ros_pkg DEPENDS eigen ) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index 13123c5..2e21620 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -35,7 +35,7 @@ along with this program. If not, see . #include #include #include -#include +#include #include #include @@ -117,7 +117,7 @@ class KinectImageObservationModelCPU: observation_time_(0), Base(delta_time) { - static_assert_base(State, RigidBodiesState); + static_assert_base(State, osr::RigidBodiesState); this->default_poses_.recount(object_model_->vertices().size()); this->default_poses_.setZero(); @@ -255,7 +255,7 @@ class KinectImageObservationModelCPU: const size_t n_cols_; const float initial_occlusion_; const size_t max_sample_count_; - const boost::shared_ptr > rigid_bodies_state_; + const boost::shared_ptr > rigid_bodies_state_; // models diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index 56627b1..eb5c314 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -8,7 +8,7 @@ #include #include -#include +#include #include #include @@ -20,8 +20,8 @@ #include -#include -#include +//#include +#include namespace dbot { @@ -220,14 +220,14 @@ class KinectImageObservationModelGPU: count_ = 0; - render_time_ = 0; +// render_time_ = 0; visibility_probs_.resize(n_rows_ * n_cols_); } ~KinectImageObservationModelGPU() noexcept { - std::cout << "time for render: " << render_time_ / count_ << std::endl; +// std::cout << "time for render: " << render_time_ / count_ << std::endl; } @@ -291,13 +291,13 @@ class KinectImageObservationModelGPU: MEASURE("gpu: converting state format"); - double before_render = dbot::hf::get_wall_time(); +// double before_render = dbot::hf::get_wall_time(); opengl_->render(poses); double after_render = dbot::hf::get_wall_time(); - render_time_ += after_render - before_render; +// render_time_ += after_render - before_render; count_++; @@ -477,7 +477,7 @@ class KinectImageObservationModelGPU: std::string fragment_shader_path_; - double render_time_; +// double render_time_; int nr_poses_; int nr_poses_per_row_; diff --git a/include/dbot/models/process_models/brownian_object_motion_model.hpp b/include/dbot/models/process_models/brownian_object_motion_model.hpp index c9b4235..c377609 100644 --- a/include/dbot/models/process_models/brownian_object_motion_model.hpp +++ b/include/dbot/models/process_models/brownian_object_motion_model.hpp @@ -127,7 +127,7 @@ class BrownianObjectMotionModel: state_(count_objects), delta_time_(delta_time) { - static_assert_base(State, FreeFloatingRigidBodiesState); + static_assert_base(State, osr::FreeFloatingRigidBodiesState); quaternion_map_.resize(count_objects); rotation_center_.resize(count_objects); diff --git a/include/dbot/utils/helper_functions.hpp b/include/dbot/utils/helper_functions.hpp new file mode 100644 index 0000000..67d74db --- /dev/null +++ b/include/dbot/utils/helper_functions.hpp @@ -0,0 +1,1289 @@ +///************************************************************************* +//This software allows for filtering in high-dimensional observation and +//state spaces, as described in + +//M. Wuthrich, P. Pastor, M. Kalakrishnan, J. Bohg, and S. Schaal. +//Probabilistic Object Tracking using a Range Camera +//IEEE/RSJ Intl Conf on Intelligent Robots and Systems, 2013 + +//In a publication based on this software pleace cite the above reference. + + +//Copyright (C) 2014 Manuel Wuthrich + +//This program is free software: you can redistribute it and/or modify +//it under the terms of the GNU General Public License as published by +//the Free Software Foundation, either version 3 of the License, or +//(at your option) any later version. + +//This program is distributed in the hope that it will be useful, +//but WITHOUT ANY WARRANTY; without even the implied warranty of +//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +//GNU General Public License for more details. + +//You should have received a copy of the GNU General Public License +//along with this program. If not, see . +//*************************************************************************/ + +//#ifndef FAST_FILTERING_UTILS_HELPER_FUNCTIONS_HPP +//#define FAST_FILTERING_UTILS_HELPER_FUNCTIONS_HPP + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + + +#include +#include + +#include + + + +//// TODO: THIS HAS TO BE CLEANED, POSSIBLY SPLIT INTO SEVERAL FILES + +namespace dbot +{ + +namespace hf +{ + + + + +//inline double Sigmoid(const double& x) +//{ +// return 1.0 / (1.0 + std::exp(-x)); +//} +//inline double Logit(const double& x) +//{ +// return std::log(x / (1.0 - x)); +//} + + +//inline std::string DateAndTimeString() +//{ +// time_t rawtime; +// struct tm * timeinfo; +// char buffer[80]; + +// time (&rawtime); +// timeinfo = localtime(&rawtime); + +// strftime(buffer,80,"%d.%m.%Y_%I.%M.%S",timeinfo); +// std::string current_time(buffer); + +// return current_time; +//} + + + +//inline double get_wall_time(){ +// struct timeval time; +// if (gettimeofday(&time,NULL)){ +// std::cout << "WARNING: gettimeofday() Error" << std::endl; +// return 0; +// } +// return (double)time.tv_sec + (double)time.tv_usec * .000001; +//} + + +//template void Compare(const std::vector vector_1, +// const std::vector vector_2, +// char* name_1, +// char* name_2) { +// if (vector_1.size() == vector_2.size()) { +// T difference = 0; +// for (uint i = 0; i < vector_1.size(); i++) { +// std::cout << name_1 << "[" << i << "]: " << vector_1[i] << ", " +// << name_2 << "[" << i << "]: " << vector_2[i] << ", " +// << "difference: " << std::abs(vector_1[i] - vector_2[i]) << std::endl; +// difference += std::abs(vector_1[i] - vector_2[i]); +// } +// std::cout << "average difference between " << name_1 << " and " << name_2 << " : " << difference / vector_1.size() << std::endl; + +// } else { +// std::cout << "WARNING: The two vectors to be compared have a different amount of elements. " << std::endl +// << name_1 << ": " << vector_1.size() << ", " << name_2 << ": " << vector_2.size() << std::endl +// << "Comparison aborted." << std::endl; +// } +//} + +//template void AverageDifference(const std::vector vector_1, +// const std::vector vector_2, +// char* name_1, +// char* name_2) { +// if (vector_1.size() == vector_2.size()) { +// T difference = 0; +// for (uint i = 0; i < vector_1.size(); i++) { +// difference += std::abs(vector_1[i] - vector_2[i]); +// } +// std::cout << "average difference between " << name_1 << " and " << name_2 << " : " << difference / vector_1.size() << std::endl; + +// } else { +// std::cout << "WARNING: The two vectors to be compared have a different amount of elements. " << std::endl +// << name_1 << ": " << vector_1.size() << ", " << name_2 << ": " << vector_2.size() << std::endl +// << "Comparison aborted." << std::endl; +// } +//} + +//template void Output(const T value, +// char* name) { +// std::cout << name << ": " << value << std::endl; +//} + + +//template +//int IndexNextReal( +// const std::vector& data, +// const int& current_index = -1, +// const int& step_size = 1) +//{ +// int index_next_real = current_index + step_size; +// while( +// index_next_real < int(data.size()) && +// index_next_real >= 0 && +// !std::isfinite(data[index_next_real])) index_next_real+=step_size; + +// return index_next_real; +//} + +//// various useful functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> +//class Structurer2D +//{ +//public: +// Structurer2D(){} + +// ~Structurer2D() {} + +// template std::vector Flatten(const std::vector >& deep_structure) +// { +// sizes_.resize(deep_structure.size()); +// size_t global_size = 0; +// for(size_t i = 0; i < deep_structure.size(); i++) +// { +// sizes_[i] = deep_structure[i].size(); +// global_size += deep_structure[i].size(); +// } + +// std::vector flat_structure(global_size); +// size_t global_index = 0; +// for(size_t i = 0; i < deep_structure.size(); i++) +// for(size_t j = 0; j < deep_structure[i].size(); j++) +// flat_structure[global_index++] = deep_structure[i][j]; + +// return flat_structure; +// } + +// template std::vector > Deepen(const std::vector& flat_structure) +// { +// std::vector > deep_structure(sizes_.size()); + +// size_t global_index = 0; +// for(size_t i = 0; i < deep_structure.size(); i++) +// { +// deep_structure[i].resize(sizes_[i]); +// for(size_t j = 0; j < deep_structure[i].size(); j++) +// deep_structure[i][j] = flat_structure[global_index++]; +// } +// return deep_structure; +// } + +//private: +// std::vector sizes_; +//}; + + + +//class Structurer3D +//{ +//public: +// Structurer3D(){} + +// ~Structurer3D() {} + +// template std::vector Flatten(const std::vector > >& deep_structure) +// { +// size_t global_size = 0; +// sizes_.resize(deep_structure.size()); +// for(size_t i = 0; i < deep_structure.size(); i++) +// { +// sizes_[i].resize(deep_structure[i].size()); +// for(size_t j = 0; j < deep_structure[i].size(); j++) +// { +// sizes_[i][j] = deep_structure[i][j].size(); +// global_size += deep_structure[i][j].size(); +// } +// } + +// std::vector flat_structure(global_size); +// size_t global_index = 0; +// for(size_t i = 0; i < deep_structure.size(); i++) +// for(size_t j = 0; j < deep_structure[i].size(); j++) +// for(size_t k = 0; k < deep_structure[i][j].size(); k++) +// flat_structure[global_index++] = deep_structure[i][j][k]; + +// return flat_structure; +// } + +// template std::vector > > Deepen(const std::vector& flat_structure) +// { +// size_t global_index = 0; +// std::vector > > deep_structure(sizes_.size()); +// for(size_t i = 0; i < deep_structure.size(); i++) +// { +// deep_structure[i].resize(sizes_[i].size()); +// for(size_t j = 0; j < deep_structure[i].size(); j++) +// { +// deep_structure[i][j].resize(sizes_[i][j]); +// for(size_t k = 0; k < deep_structure[i][j].size(); k++) +// deep_structure[i][j][k] = flat_structure[global_index++]; +// } +// } +// return deep_structure; +// } + +//private: +// std::vector > sizes_; +//}; + + + + + + + +//// this function will interpolat the vector wherever it is NAN or INF +//template +//void LinearlyInterpolate(std::vector& data) +//{ +// std::vector limits; +// limits.push_back(0); +// limits.push_back(data.size()-1); +// std::vector step_direction; +// step_direction.push_back(1); +// step_direction.push_back(-1); + +// // extrapolate +// for(int i = 0; i < 2; i++) +// { +// int index_first_real = IndexNextReal(data, limits[i] - step_direction[i], step_direction[i]); +// int index_next_real = IndexNextReal(data, index_first_real, step_direction[i]); +// if(index_next_real >= int(data.size()) || index_next_real < 0) +// return; + +// double slope = +// double(data[index_next_real] - data[index_first_real]) / +// double(index_next_real - index_first_real); + +// for(int j = limits[i]; j != index_first_real; j += step_direction[i]) +// data[j] = data[index_first_real] + (j - index_first_real) * slope; +// } + +// // interpolate +// int index_current_real = IndexNextReal(data, limits[0] - step_direction[0], step_direction[0]); +// int index_next_real = IndexNextReal(data, index_current_real, step_direction[0]); + +// while(index_next_real < int(data.size())) +// { +// double slope = +// double(data[index_next_real] - data[index_current_real]) / +// double(index_next_real - index_current_real); + +// for(int i = index_current_real + 1; i < index_next_real; i++) +// data[i] = data[index_current_real] + (i - index_current_real) * slope; + +// index_current_real = index_next_real; +// index_next_real = IndexNextReal(data, index_next_real, step_direction[0]); +// } +//} + +//template void PrintVector(std::vector v) +//{ +// for(size_t i = 0; i < v.size(); i++) +// std::cout << "(" << i << ": " << v[i] << ") "; +// std::cout << std::endl; +//} +//template void PrintVector(std::vector > v) +//{ +// for(size_t i = 0; i < v.size(); i++) +// { +// std::cout << i << " --------------------------------" << std::endl; +// PrintVector(v[i]); +// } +//} + +//template void PrintVector(std::vector > > v) +//{ +// for(size_t i = 0; i < v.size(); i++) +// { +// std::cout << i << " ================================" << std::endl; +// PrintVector(v[i]); +// } +//} + + +//template +//void Swap(T& a, T& b) +//{ +// T temp = a; +// a = b; +// b = temp; +//} + +//template +//Eigen::Matrix Std2Eigen(const std::vector &std) +//{ +// Eigen::Matrix eigen(std.size()); +// for(size_t i = 0; i < std.size(); i++) +// eigen(i) = std[i]; +// return eigen; +//} + +//template +//std::string Eigen2Mathematica(const Eigen::MatrixBase& eigen, const std::string &name = "") +//{ +// std::string mathematica; +// if(!name.empty()) +// mathematica += name + " = "; + +// mathematica += "{"; +// for(int row = 0; row < eigen.rows(); row++) +// { +// mathematica += "{"; +// for(int col = 0; col < eigen.cols(); col++) +// { +// mathematica += boost::lexical_cast(eigen(row, col)); +// if(col != eigen.cols() -1) +// mathematica += ", "; +// } +// mathematica += "}"; +// if(row != eigen.rows()-1) +// mathematica += ", "; +// } +// mathematica += "}"; +// if(!name.empty()) +// mathematica += ";"; + +// return mathematica; +//} + +//// returns the first index where the two vectors differ, if there is no difference then -1 is returned. +//template int +//DifferenceAt( +// const std::vector >& a, +// const std::vector >& b, +// const T& epsilon) +//{ +// if(a.size() < b.size()) +// return a.size(); +// else if(b.size() < a.size()) +// return b.size(); + +// for(size_t i = 0; i < a.size(); i++) +// for(size_t row = 0; row < n_rows; row++) +// for(size_t col = 0; col < n_cols; col++) +// { +// if(isnan(a[i](row, col)) && isnan(b[i](row, col))) +// continue; +// else if(isnan(a[i](row, col)) || isnan(b[i](row, col))) +// return i; +// else if( (a[i](row, col) - b[i](row, col) > epsilon) || (b[i](row, col) - a[i](row, col) > epsilon) ) +// return i; +// } + +// return -1; +//} + +//// returns the first index where the two vectors differ, if there is no difference then -1 is returned. +//template int +//DifferenceAt( +// const std::vector& a, +// const std::vector& b, +// const T& epsilon) +//{ +// if(a.size() < b.size()) +// return a.size(); +// else if(b.size() < a.size()) +// return b.size(); + +// for(size_t i = 0; i < a.size(); i++) +// { +// if(isnan(a[i]) && isnan(b[i])) +// continue; +// else if(isnan(a[i]) || isnan(b[i])) +// return i; +// else if( (a[i] - b[i] > epsilon) || (b[i] - a[i] > epsilon) ) +// return i; +// } + +// return -1; +//} + +//template std::vector ReverseOrder(std::vector ordered) +//{ +// std::vector reversed(ordered.size()); +// for(size_t i = 0; i < ordered.size(); i++) +// reversed[i] = ordered[ordered.size() - 1 - i]; +//} + +//template std::vector Count(T from, T to, T increment = 1) +//{ +// std::vector count(size_t((to - from)/increment)); +// count[0] = from; +// for(size_t i = 1; i < count.size(); i++) +// count[i] = count[i-1] + increment; +//} + +//template struct ValuesIndex +//{ +// std::vector values; +// int index; + +// bool operator < (const ValuesIndex& right_side) const +// { +// for(size_t i = 0; i < values.size(); i++) +// { +// if(values[i] < right_side.values[i]) +// return true; +// if(values[i] > right_side.values[i]) +// return false; +// } +// return false; +// } + + +// bool operator != (const ValuesIndex& right_side) const +// { +// for(size_t i = 0; i < values.size(); i++) +// if(values[i] != right_side.values[i]) +// return true; + +// return false; +// } +//}; + +//template void +//SortAndCollapse( +// std::vector >& values, +// std::vector& multiplicities) +//{ +// std::vector > values_indices(values.size()); +// for(size_t i = 0; i < values.size(); i++) +// { +// values_indices[i].index = i; +// values_indices[i].values = values[i]; +// } + +// std::sort(values_indices.begin(), values_indices.end()); + +// std::vector > temp_values = values; +// multiplicities = std::vector(values.size(), 0); +// size_t distinct_index = 0; +// values[0] = temp_values[values_indices[0].index]; +// for(size_t i = 0; i < values.size(); i++) +// { +// if(i > 0 && temp_values[values_indices[i-1].index] != temp_values[values_indices[i].index]) +// values[++distinct_index] = temp_values[values_indices[i].index]; +// multiplicities[distinct_index]++; +// } +// values.resize(distinct_index+1); +// multiplicities.resize(distinct_index+1); +//} + + + + + + + + + + +//template struct ValueIndex +//{ +// T value; +// int index; + +// bool operator < (const ValueIndex& str) const +// { +// return (value < str.value); +// } +//}; +//template std::vector SortAscend(const std::vector &values) +//{ +// std::vector indices(values.size()); + +// std::vector > values_indices(values.size()); +// for(int i = 0; i < int(values.size()); i++) +// { +// values_indices[i].index = i; +// values_indices[i].value = values[i]; +// } + +// std::sort(values_indices.begin(), values_indices.end()); + +// for(int i = 0; i < int(indices.size()); i++) +// indices[i] = values_indices[i].index; + +// return indices; +//} + +//template std::vector SortDescend(const std::vector &values) +//{ +// std::vector ascend_indices = SortAscend(values); +// std::vector descend_indices(ascend_indices.size()); + +// for(int i = 0; i < int(ascend_indices.size()); i++) +// descend_indices[i] = ascend_indices[ascend_indices.size()-1-i]; + +// return descend_indices; +//} + + +//template void Sort(std::vector &ref_values, bool order = 0) +//{ +// std::vector temp_ref_values = ref_values; +// std::vector indices; +// if(order == 0) +// indices = SortAscend(temp_ref_values); +// else +// indices = SortDescend(temp_ref_values); + + +// for(int i = 0; i < int(indices.size()); i++) +// ref_values[i] = temp_ref_values[indices[i]]; +//} + + +//template void SortAscend(std::vector &ref_values, std::vector &values) +//{ +// std::vector temp_ref_values = ref_values; +// std::vector temp_values = values; + +// std::vector indices = SortAscend(temp_ref_values); + +// for(int i = 0; i < int(indices.size()); i++) +// { +// ref_values[i] = temp_ref_values[indices[i]]; +// values[i] = temp_values[indices[i]]; +// } +//} + +//template void SortDescend(std::vector &ref_values, std::vector &values) +//{ +// std::vector temp_ref_values = ref_values; +// std::vector temp_values = values; + +// std::vector indices = SortDescend(temp_ref_values); + +// for(int i = 0; i < int(indices.size()); i++) +// { +// ref_values[i] = temp_ref_values[indices[i]]; +// values[i] = temp_values[indices[i]]; +// } +//} + +//template void SortAscend(std::vector &ref_values, +// std::vector &values1, std::vector &values2) +//{ +// std::vector temp_ref_values = ref_values; +// std::vector temp_values1 = values1; +// std::vector temp_values2 = values2; + +// std::vector indices = SortAscend(temp_ref_values); + +// for(int i = 0; i < int(indices.size()); i++) +// { +// ref_values[i] = temp_ref_values[indices[i]]; +// values1[i] = temp_values1[indices[i]]; +// values2[i] = temp_values2[indices[i]]; +// } +//} + +//template void SortDescend(std::vector &ref_values, +// std::vector &values1, std::vector &values2) +//{ +// std::vector temp_ref_values = ref_values; +// std::vector temp_values1 = values1; +// std::vector temp_values2 = values2; + +// std::vector indices = SortDescend(temp_ref_values); + +// for(int i = 0; i < int(indices.size()); i++) +// { +// ref_values[i] = temp_ref_values[indices[i]]; +// values1[i] = temp_values1[indices[i]]; +// values2[i] = temp_values2[indices[i]]; +// } +//} + +//template int BoundIndex(const std::vector &values, bool bound_type) // bound type 1 for max and 0 for min +//{ +// int BoundIndex = 0; +// T bound_value = bound_type ? -std::numeric_limits::max() : std::numeric_limits::max(); + +// for(int i = 0; i < int(values.size()); i++) +// if(bound_type ? (values[i] > bound_value) : (values[i] < bound_value) ) +// { +// BoundIndex = i; +// bound_value = values[i]; +// } + +// return BoundIndex; +//} + +//template T bound_value(const std::vector &values, bool bound_type) // bound type 1 for max and 0 for min +//{ +// return values[BoundIndex(values, bound_type)]; +//} + +//template +//std::vector Apply(const std::vector &input, Tout(*f)(Tin)) +//{ +// std::vector output(input.size()); +// for(size_t i = 0; i < output.size(); i++) +// output[i] = (*f)(input[i]); + +// return output; +//} + +//template std::vector +//SetSum(const std::vector &input, T sum) +//{ +// T old_sum = 0; +// for(size_t i = 0; i < input.size(); i++) +// old_sum += input[i]; +// T factor = sum/old_sum; + +// std::vector output(input.size()); +// for(size_t i = 0; i < input.size(); i++) +// output[i] = factor*input[i]; + +// return output; +//} + +//// geometry functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> +//template +//Eigen::Quaternion Delta2Quaternion(const Eigen::MatrixBase& delta) +//{ +// typename T::Scalar angle = delta.norm(); +// Eigen::Matrix axis = delta.normalized(); +// Eigen::Quaternion q; +// if(std::isfinite(axis.norm())) +// q = Eigen::AngleAxisd(angle, axis); +// else +// q = Eigen::Quaterniond::Identity(); + +// return q; +//} + +//// this function finds the intersection between two lines. a is a point on line and d_a is the direction +//// vector of the line. if lines do not cross, this will return the point which minimizes the squared +//// distance between the two lines. if they are parallel, then inf will be returned. +//template +//Eigen::Matrix Intersection( +// const Eigen::MatrixBase& a, +// const Eigen::MatrixBase& d_a_in, +// const Eigen::MatrixBase& b, +// const Eigen::MatrixBase& d_b_in) +//{ +// const Eigen::Matrix d_a = d_a_in.normalized(); +// const Eigen::Matrix d_b = d_b_in.normalized(); + +// double c_a = d_a.dot((b-a) - d_b.dot(b-a)*d_b)/ +// (1.-pow(d_b.dot(d_a),2)); +// double c_b = d_b.dot((a-b) - d_a.dot(a-b)*d_a)/ +// (1.-pow(d_a.dot(d_b),2)); + +// return((b + c_b*d_b)+(a + c_a*d_a))/2.; +//} + +//template +//typename T1::Scalar Angle( +// const Eigen::MatrixBase& a, +// const Eigen::MatrixBase& b) +//{ +// return atan2(a.cross(b).norm(), a.dot(b)); +//} + +//template +//Eigen::Matrix OrthogonalUnitVector(const Eigen::MatrixBase& v) +//{ +// Eigen::Matrix n = +// (v.cross(Eigen::Matrix(1.0, 0, 0))).normalized(); +// if(!std::isfinite(n.norm())) +// n = (v.cross(Eigen::Matrix(0, 1.0, 0))).normalized(); + +// return n; +//} + +//// returns the angle and axis which rotate a onto b +//template +//Eigen::AngleAxis AngleAxis( +// const Eigen::MatrixBase& a, +// const Eigen::MatrixBase& b) +//{ +// Eigen::Matrix axis = a.cross(b); +// typename T1::Scalar axis_norm = axis.norm(); +// axis /= axis_norm; + +// // if the axis is inf then the angle is either 0 or pi, so the axis has to be just some orthogonal vector +// if(!std::isfinite(axis.norm())) +// axis = OrthogonalUnitVector(a); + +// typename T1::Scalar angle = atan2(axis_norm, a.dot(b)); + +// return Eigen::AngleAxis(angle, axis); +//} + +// the input quaternion has the order xyzw +inline Eigen::Matrix QuaternionMatrix(const Eigen::Matrix& q_xyzw) +{ + Eigen::Matrix Q; + Q << q_xyzw(3), q_xyzw(2), -q_xyzw(1), + -q_xyzw(2), q_xyzw(3), q_xyzw(0), + q_xyzw(1), -q_xyzw(0), q_xyzw(3), + -q_xyzw(0), -q_xyzw(1), -q_xyzw(2); + return 0.5*Q; +} + +//template class TransformationSequence +//{ +//public: +// TransformationSequence( +// const Eigen::Matrix &R = Eigen::Matrix::Identity(), +// const Eigen::Matrix &t = Eigen::Matrix::Zero()): R_(R), t_(t) {} + +// ~TransformationSequence(){} + +// void PreRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) +// { +// t_ = t_ - R_*R*c + R_*c; +// R_ = R_*R; +// } +// void PreTranslate(const Eigen::Matrix &t) +// { +// t_ = t_ + R_*t; +// } +// void PostRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) +// { +// t_ = R*t_ - R*c + c; +// R_ = R*R_; +// } +// void PostTranslate(const Eigen::Matrix &t) +// { +// t_ = t_ + t; +// } + +// void get(Eigen::Matrix &R, Eigen::Matrix &t) +// { +// R = R_; +// t = t_; +// } + +// void NormalizeQuat() +// { +// Eigen::Quaternion q; q = R_; +// q.normalize(); +// R_ = q.toRotationMatrix(); +// } + +//private: +// Eigen::Matrix R_; +// Eigen::Matrix t_; + +//}; + +//// depth image functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> +//// converts from cartesian to image space +//template Eigen::Matrix +//CartCoord2ImageCoord(const Eigen::Matrix& cart, +// const Eigen::Matrix& camera_matrix) +//{ +// return (camera_matrix * cart/cart(2)).topRows(2); +//} + +//// converts from cartesian space to image index, (row, col) +//template Eigen::Matrix +//CartCoord2ImageIndex(const Eigen::Matrix& cart, +// const Eigen::Matrix& camera_matrix) +//{ +// Eigen::Matrix image = CartCoord2ImageCoord(cart, camera_matrix); +// Eigen::Matrix image_index; +// image_index(0) = floor(image(1)+0.5); +// image_index(1) = floor(image(0)+0.5); + +// return image_index; +//} + +//// converts from image coordinates and depth (z value) to cartesian coordinates +//template Eigen::Matrix +//ImageCoord2CartCoord(const Eigen::Matrix& image, +// const T& depth, +// const Eigen::Matrix& camera_matrix_inverse) +//{ +// Eigen::Matrix image_augmented; +// image_augmented.topRows(2) = image; +// image_augmented(2) = 1; + +// return depth * camera_matrix_inverse * image_augmented; +//} + +//// converts from image index (row, col) and depth (z value) to cartesian coordinates +//template Eigen::Matrix +//ImageIndex2CartCoord(const Eigen::Matrix& image_index, +// const T& depth, +// const Eigen::Matrix& camera_matrix_inverse) +//{ +// Eigen::Matrix image; +// image(0) = image_index(1); +// image(1) = image_index(0); +// return ImageCoord2CartCoord(image, depth, camera_matrix_inverse); +//} + + +//template Eigen::Matrix , -1, -1> +//Image2Points(const Eigen::Matrix& image, +// const Eigen::Matrix& camera_matrix) +//{ +// Eigen::Matrix camera_matrix_inverse = camera_matrix.inverse(); + +// Eigen::Matrix , -1, -1> points(image.rows(), image.cols()); +// for(size_t row = 0; row < points.rows(); row++) +// for(size_t col = 0; col < points.cols(); col++) +// points(row, col) = ImageIndex2CartCoord(Eigen::Vector2i(row, col), +// image(row, col), +// camera_matrix_inverse); + +// return points; +//} + + + + +//template std::vector > +//DepthImage2CartVectors(const std::vector& image, +// const size_t& n_rows, const size_t& n_cols, +// const Eigen::Matrix& camera_matrix) +//{ +// Eigen::Matrix camera_matrix_inverse = camera_matrix.inverse(); + +// std::vector > vectors(n_rows*n_cols); +// for(size_t row = 0; row < n_rows; row++) +// for(size_t col = 0; col < n_cols; col++) +// vectors[row*n_cols + col] = +// ImageIndex2CartCoord( +// Eigen::Vector2i(row, col), +// image[row*n_cols + col], +// camera_matrix_inverse); + +// return vectors; +//} + +//template std::vector +//CartVectors2DepthImage( +// const std::vector >& vectors, +// const int& n_rows, const int& n_cols, +// const Eigen::Matrix& camera_matrix) +//{ +// std::vector image(n_rows*n_cols, NAN); + +// for(size_t i = 0; i < vectors.size(); i++) +// { +// if( isnan(vectors[i](0)) || isnan(vectors[i](1)) || isnan(vectors[i](2)) ) +// continue; + +// Eigen::Matrix index = CartCoord2ImageIndex(vectors[i], camera_matrix); + +// if(index(0) < 0 || index(0) >= n_rows || index(1) < 0 || index(1) >= n_cols) +// { +// std::cout << "ERROR: in CartVectors2DepthImage vector comes to lie outside of image " << std::endl; +// exit(-1); +// } + +// image[index(0)*n_cols + index(1)] = vectors[i](2); +// } + +// return image; +//} + + + +//// numerically stable implementation of log(sum(exp(xi))) >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> +//inline double log_sum_exp(double a, double b) +//{ +// if(a>b) return a + log(1+exp(b-a)); +// else return b + log(1+exp(a-b)); +//} +//inline double log_sum_exp(double a, double b, double c) +//{ +// if(a>b && a>c) return a + log(1+exp(b-a)+exp(c-a)); +// if(b>c && b>a) return b + log(1+exp(a-b)+exp(c-b)); +// else return c + log(1+exp(a-c)+exp(b-c)); +//} +//inline double log_sum_exp(double a, double b, double c, double d) +//{ +// if(a>b && a>c && a>d) return a + log(1+exp(b-a)+exp(c-a)+exp(d-a)); +// if(b>a && b>c && b>d) return b + log(1+exp(a-b)+exp(c-b)+exp(d-b)); +// if(c>a && c>b && c>d) return c + log(1+exp(a-c)+exp(b-c)+exp(d-c)); +// else return d + log(1+exp(a-d)+exp(b-d)+exp(c-d)); +//} + +//template T log_sum_exp(std::vector exponents) +//{ +// T max_exponent = -std::numeric_limits::max(); +// for(size_t i = 0; i < exponents.size(); i++) +// if(exponents[i] > max_exponent) max_exponent = exponents[i]; + +// for(size_t i = 0; i < exponents.size(); i++) +// exponents[i] -= max_exponent; + +// T sum = 0; +// for(size_t i = 0; i < exponents.size(); i++) +// sum += exp(exponents[i]); + +// return max_exponent + log(sum); +//} + + +//template class LogSumExp +//{ +//public: +// LogSumExp(){} +// ~LogSumExp(){} + +// void add_exponent(T exponent) +// { +// exponents_.push_back(exponent); +// } + +// T Compute() +// { +// T max_exponent = -std::numeric_limits::max(); +// for(int i = 0; i < int(exponents_.size()); i++) +// if(exponents_[i] > max_exponent) max_exponent = exponents_[i]; + +// for(int i = 0; i < int(exponents_.size()); i++) +// exponents_[i] -= max_exponent; + +// T sum = 0; +// for(int i = 0; i < int(exponents_.size()); i++) +// sum += exp(exponents_[i]); + +// return max_exponent + log(sum); +// } +//private: +// std::vector exponents_; +//}; + +//// sampling class >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> +//class DiscreteSampler +//{ +//public: +// template DiscreteSampler(std::vector log_prob) +// { +// fibo_.seed(RANDOM_SEED); + +// // compute the prob and normalize them +// sorted_indices_ = dbot::hf::SortDescend(log_prob); +// double max = log_prob[sorted_indices_[0]]; +// for(int i = 0; i < int(log_prob.size()); i++) +// log_prob[i] -= max; + +// std::vector prob(log_prob.size()); +// double sum = 0; +// for(int i = 0; i < int(log_prob.size()); i++) +// { +// prob[i] = exp(log_prob[i]); +// sum += prob[i]; +// } +// for(int i = 0; i < int(prob.size()); i++) +// prob[i] /= sum; + +// // compute the cumulative probability +// cumulative_prob_.resize(log_prob.size()); +// cumulative_prob_[0] = prob[sorted_indices_[0]]; +// for(int i = 1; i < int(log_prob.size()); i++) +// cumulative_prob_[i] = cumulative_prob_[i-1] + prob[sorted_indices_[i]]; +// } + +// ~DiscreteSampler() {} + +// int Sample() +// { +// double uniform_sample = fibo_(); +// int sample_index = 0; +// while(uniform_sample > cumulative_prob_[sample_index]) sample_index++; + +// return sorted_indices_[sample_index]; +// } + + + +// int MapStandardGaussian(double gaussian_sample) const +// { +// // map from a gaussian to a uniform distribution +// double uniform_sample = 0.5 * +// (1.0 + std::erf(gaussian_sample / std::sqrt(2.0))); +// int sample_index = 0; +// while(uniform_sample > cumulative_prob_[sample_index]) sample_index++; //TODO: THIS COULD BE DONE IN LOG TIME + +// return sorted_indices_[sample_index]; +// } + +//private: +// boost::lagged_fibonacci607 fibo_; +// std::vector sorted_indices_; +// std::vector cumulative_prob_; +//}; + + + +//// the above class should go away +//class DiscreteDistribution +//{ +//public: +// template DiscreteDistribution(std::vector log_prob) +// { +// uniform_sampler_.seed(RANDOM_SEED); + +// // substract max to avoid numerical issues +// double max = hf::bound_value(log_prob, true); +// for(int i = 0; i < int(log_prob.size()); i++) +// log_prob[i] -= max; + +// // compute probabilities +// std::vector prob(log_prob.size()); +// double sum = 0; +// for(int i = 0; i < int(log_prob.size()); i++) +// { +// prob[i] = std::exp(log_prob[i]); +// sum += prob[i]; +// } +// for(int i = 0; i < int(prob.size()); i++) +// prob[i] /= sum; + +// // compute the cumulative probability +// cumulative_prob_.resize(prob.size()); +// cumulative_prob_[0] = prob[0]; +// for(size_t i = 1; i < prob.size(); i++) +// cumulative_prob_[i] = cumulative_prob_[i-1] + prob[i]; +// } + +// ~DiscreteDistribution() {} + +// int Sample() +// { +// return MapStandardUniform(uniform_sampler_()); +// } + +// int MapStandardGaussian(double gaussian_sample) const +// { +// double uniform_sample = +// 0.5 * (1.0 + std::erf(gaussian_sample / std::sqrt(2.0))); +// return MapStandardUniform(uniform_sample); +// } + +// int MapStandardUniform(double uniform_sample) const +// { +// std::vector::const_iterator iterator = +// std::lower_bound(cumulative_prob_.begin(), +// cumulative_prob_.end(), +// uniform_sample); + +// return iterator - cumulative_prob_.begin(); +// } + +//private: +// boost::lagged_fibonacci607 uniform_sampler_; +// std::vector cumulative_prob_; +//}; + + + + + + + + + + + + + + + + + + + + + + + + + +//// todo this stuff should be removed >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> +//template void +//Vector2TranslAndQuat(const std::vector &v, Eigen::Matrix &t, Eigen::Quaternion &q) +//{ +// Eigen::Quaternion quat(); +// q.w() = v[0]; q.x() = v[1]; q.y() = v[2]; q.z() = v[3]; +// t(0) = v[4]; t(1) = v[5]; t(2) = v[6]; +//} + +//template void +//TranslAndQuat2Vector(const Eigen::Quaternion &q, const Eigen::Matrix &t, std::vector &v) +//{ +// v.resize(7); + +// v[0] = q.w(); v[1] = q.x(); v[2] = q.y(); v[3] = q.z(); +// v[4] = t(0); v[5] = t(1); v[6] = t(2); +//} + +//template void +//Vector2TranslAndRot(const std::vector &v, Eigen::Matrix &R, Eigen::Matrix &t) +//{ +// Eigen::Quaternion q; + +// Vector2QuatAndTransl(v, q, t); +// R = q; +//} + +//template void +//TranslAndRot2Vector(const Eigen::Matrix &R, const Eigen::Matrix &t, std::vector &v) +//{ +// const Eigen::Quaternion q(R); + +// QuatAndTransl2Vector(q, t, v); +//} + +//inline Eigen::VectorXd OldState2NewState(const std::vector& old_state) +//{ +// Eigen::VectorXd new_state(old_state.size() * 2 - 1); // the new state also contains the velocity +// new_state(0) = old_state[4]; +// new_state(1) = old_state[5]; +// new_state(2) = old_state[6]; + +// new_state(3) = old_state[1]; +// new_state(4) = old_state[2]; +// new_state(5) = old_state[3]; +// new_state(6) = old_state[0]; + +//// new_state.topRows(3) += Eigen::Quaterniond(new_state.middleRows<4>(3))._transformVector(center); +// new_state.middleRows(7, 6) = Eigen::VectorXd::Zero(6); + +// // we fill in the joint angles +// for(size_t i = 7; i < old_state.size(); i++) +// new_state(i + 6) = old_state[i]; +// // we fill in the joint velocities +// for(size_t i = old_state.size() + 6; i < size_t(new_state.rows()); i++) +// new_state(i) = 0; + +// return new_state; +//} + +//inline std::vector NewState2OldState(const Eigen::VectorXd& new_state) +//{ +// std::vector old_state((new_state.rows()+1)/2); + +// old_state[4] = new_state(0); +// old_state[5] = new_state(1); +// old_state[6] = new_state(2); + +// old_state[1] = new_state(3); +// old_state[2] = new_state(4); +// old_state[3] = new_state(5); +// old_state[0] = new_state(6); + +// for(size_t i = 7; i < old_state.size(); i++) +// old_state[i] = new_state(i+6); + +// return old_state; +//} + +//template void +//Vector2QuatAndTransl(const std::vector &v, Eigen::Quaternion &q, Eigen::Matrix &t) +//{ +// q.w() = v[0]; q.x() = v[1]; q.y() = v[2]; q.z() = v[3]; +// t(0) = v[4]; t(1) = v[5]; t(2) = v[6]; +//} + +//template void +//QuatAndTransl2Vector(const Eigen::Quaternion &q, const Eigen::Matrix &t, std::vector &v) +//{ +// v.resize(7); + +// v[0] = q.w(); v[1] = q.x(); v[2] = q.y(); v[3] = q.z(); +// v[4] = t(0); v[5] = t(1); v[6] = t(2); +//} + +//template void +//Vector2RotAndTransl(const std::vector &v, Eigen::Matrix &R, Eigen::Matrix &t) +//{ +// Eigen::Quaternion q; + +// Vector2QuatAndTransl(v, q, t); +// R = q; +//} + +//template void +//RotAndTransl2Vector(const Eigen::Matrix &R, const Eigen::Matrix &t, std::vector &v) +//{ +// const Eigen::Quaternion q(R); + +// QuatAndTransl2Vector(q, t, v); +//} + +//template void +//Vector2Hom(const std::vector &v, Eigen::Matrix &H) +//{ +// Eigen::Matrix R; +// Eigen::Matrix t; +// Vector2RotAndTransl(v, R, t); + +// H.topLeftCorner(3,3) = R; +// H.topRightCorner(3,1) = t; +// H.row(3) = Eigen::Matrix (0,0,0,1); +//} + +//template void +//Hom2Vector(const Eigen::Matrix &H, std::vector &v) +//{ +// const Eigen::Matrix R(H.topLeftCorner(3,3)); +// const Eigen::Matrix t(H.topRightCorner(3,1)); + +// RotAndTransl2Vector(R, t, v); +//} + +//template void +//Vector2Affine(const std::vector &v, Eigen::Transform &A) +//{ +// Eigen::Matrix H; +// Vector2Hom(v, H); +// A = H; +//} + +//template void +//Affine2Vector(const Eigen::Transform &A, std::vector &v) +//{ +// Eigen::Matrix H; +// H = A.matrix(); +// Hom2Vector(H, v); +//} + +} + +} From a368cb8b6419dd51a201fb478c846a1cac31250f Mon Sep 17 00:00:00 2001 From: issac Date: Sat, 3 Oct 2015 22:05:05 +0200 Subject: [PATCH 068/131] some updates --- CMakeLists.txt | 3 +-- .../kinect_image_observation_model_gpu.hpp | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 66e718c..9d7111b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(dbot) ############################ # Options # ############################ -option(DBOT_ON_CUDA "Compile CUDA enabled trackers" ON) +option(DBOT_ON_GPU "Compile CUDA enabled trackers" ON) ############################ # Flags # @@ -83,7 +83,6 @@ if(DBOT_ON_GPU) CATKIN_DEPENDS fl osr_catkin - dbot_ros_pkg DEPENDS eigen ) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index eb5c314..b810a0a 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -296,7 +296,7 @@ class KinectImageObservationModelGPU: opengl_->render(poses); - double after_render = dbot::hf::get_wall_time(); +// double after_render = dbot::hf::get_wall_time(); // render_time_ += after_render - before_render; count_++; From f684f7854076cb4a3b03e017807b5ec797a44f57 Mon Sep 17 00:00:00 2001 From: issac Date: Sat, 3 Oct 2015 22:08:19 +0200 Subject: [PATCH 069/131] more updates --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9d7111b..1a2adfe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -125,6 +125,8 @@ if(DBOT_ON_GPU) ${OPENGL_LIBRARIES} ${GLFW_LIBRARY} ${GLEW_LIBRARIES}) + + target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_gpu) endif(DBOT_ON_GPU) From db10fc210f2501f0d85de78dc61312f33cf65600 Mon Sep 17 00:00:00 2001 From: issac Date: Sat, 3 Oct 2015 22:42:57 +0200 Subject: [PATCH 070/131] annoying fixes --- include/dbot/utils/helper_functions.hpp | 2493 +++++++++++--------- include/dbot/utils/rigid_body_renderer.hpp | 7 +- 2 files changed, 1329 insertions(+), 1171 deletions(-) diff --git a/include/dbot/utils/helper_functions.hpp b/include/dbot/utils/helper_functions.hpp index 67d74db..02c70af 100644 --- a/include/dbot/utils/helper_functions.hpp +++ b/include/dbot/utils/helper_functions.hpp @@ -1,32 +1,30 @@ -///************************************************************************* -//This software allows for filtering in high-dimensional observation and -//state spaces, as described in +/************************************************************************* +This software allows for filtering in high-dimensional observation and +state spaces, as described in -//M. Wuthrich, P. Pastor, M. Kalakrishnan, J. Bohg, and S. Schaal. -//Probabilistic Object Tracking using a Range Camera -//IEEE/RSJ Intl Conf on Intelligent Robots and Systems, 2013 +M. Wuthrich, P. Pastor, M. Kalakrishnan, J. Bohg, and S. Schaal. +Probabilistic Object Tracking using a Range Camera +IEEE/RSJ Intl Conf on Intelligent Robots and Systems, 2013 -//In a publication based on this software pleace cite the above reference. +In a publication based on this software pleace cite the above reference. -//Copyright (C) 2014 Manuel Wuthrich +Copyright (C) 2014 Manuel Wuthrich -//This program is free software: you can redistribute it and/or modify -//it under the terms of the GNU General Public License as published by -//the Free Software Foundation, either version 3 of the License, or -//(at your option) any later version. +This program is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. -//This program is distributed in the hope that it will be useful, -//but WITHOUT ANY WARRANTY; without even the implied warranty of -//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -//GNU General Public License for more details. +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. -//You should have received a copy of the GNU General Public License -//along with this program. If not, see . -//*************************************************************************/ +You should have received a copy of the GNU General Public License +along with this program. If not, see . +*************************************************************************/ -//#ifndef FAST_FILTERING_UTILS_HELPER_FUNCTIONS_HPP -//#define FAST_FILTERING_UTILS_HELPER_FUNCTIONS_HPP #pragma once @@ -62,691 +60,691 @@ namespace hf -//inline double Sigmoid(const double& x) -//{ -// return 1.0 / (1.0 + std::exp(-x)); -//} -//inline double Logit(const double& x) -//{ -// return std::log(x / (1.0 - x)); -//} - - -//inline std::string DateAndTimeString() -//{ -// time_t rawtime; -// struct tm * timeinfo; -// char buffer[80]; - -// time (&rawtime); -// timeinfo = localtime(&rawtime); - -// strftime(buffer,80,"%d.%m.%Y_%I.%M.%S",timeinfo); -// std::string current_time(buffer); - -// return current_time; -//} - - - -//inline double get_wall_time(){ -// struct timeval time; -// if (gettimeofday(&time,NULL)){ -// std::cout << "WARNING: gettimeofday() Error" << std::endl; -// return 0; -// } -// return (double)time.tv_sec + (double)time.tv_usec * .000001; -//} - - -//template void Compare(const std::vector vector_1, -// const std::vector vector_2, -// char* name_1, -// char* name_2) { -// if (vector_1.size() == vector_2.size()) { -// T difference = 0; -// for (uint i = 0; i < vector_1.size(); i++) { -// std::cout << name_1 << "[" << i << "]: " << vector_1[i] << ", " -// << name_2 << "[" << i << "]: " << vector_2[i] << ", " -// << "difference: " << std::abs(vector_1[i] - vector_2[i]) << std::endl; -// difference += std::abs(vector_1[i] - vector_2[i]); -// } -// std::cout << "average difference between " << name_1 << " and " << name_2 << " : " << difference / vector_1.size() << std::endl; - -// } else { -// std::cout << "WARNING: The two vectors to be compared have a different amount of elements. " << std::endl -// << name_1 << ": " << vector_1.size() << ", " << name_2 << ": " << vector_2.size() << std::endl -// << "Comparison aborted." << std::endl; -// } -//} - -//template void AverageDifference(const std::vector vector_1, -// const std::vector vector_2, -// char* name_1, -// char* name_2) { -// if (vector_1.size() == vector_2.size()) { -// T difference = 0; -// for (uint i = 0; i < vector_1.size(); i++) { -// difference += std::abs(vector_1[i] - vector_2[i]); -// } -// std::cout << "average difference between " << name_1 << " and " << name_2 << " : " << difference / vector_1.size() << std::endl; - -// } else { -// std::cout << "WARNING: The two vectors to be compared have a different amount of elements. " << std::endl -// << name_1 << ": " << vector_1.size() << ", " << name_2 << ": " << vector_2.size() << std::endl -// << "Comparison aborted." << std::endl; -// } -//} - -//template void Output(const T value, -// char* name) { -// std::cout << name << ": " << value << std::endl; -//} - - -//template -//int IndexNextReal( -// const std::vector& data, -// const int& current_index = -1, -// const int& step_size = 1) -//{ -// int index_next_real = current_index + step_size; -// while( -// index_next_real < int(data.size()) && -// index_next_real >= 0 && -// !std::isfinite(data[index_next_real])) index_next_real+=step_size; - -// return index_next_real; -//} - -//// various useful functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -//class Structurer2D -//{ -//public: -// Structurer2D(){} - -// ~Structurer2D() {} - -// template std::vector Flatten(const std::vector >& deep_structure) -// { -// sizes_.resize(deep_structure.size()); -// size_t global_size = 0; -// for(size_t i = 0; i < deep_structure.size(); i++) -// { -// sizes_[i] = deep_structure[i].size(); -// global_size += deep_structure[i].size(); -// } - -// std::vector flat_structure(global_size); -// size_t global_index = 0; -// for(size_t i = 0; i < deep_structure.size(); i++) -// for(size_t j = 0; j < deep_structure[i].size(); j++) -// flat_structure[global_index++] = deep_structure[i][j]; - -// return flat_structure; -// } - -// template std::vector > Deepen(const std::vector& flat_structure) -// { -// std::vector > deep_structure(sizes_.size()); - -// size_t global_index = 0; -// for(size_t i = 0; i < deep_structure.size(); i++) -// { -// deep_structure[i].resize(sizes_[i]); -// for(size_t j = 0; j < deep_structure[i].size(); j++) -// deep_structure[i][j] = flat_structure[global_index++]; -// } -// return deep_structure; -// } - -//private: -// std::vector sizes_; -//}; - - - -//class Structurer3D -//{ -//public: -// Structurer3D(){} - -// ~Structurer3D() {} - -// template std::vector Flatten(const std::vector > >& deep_structure) -// { -// size_t global_size = 0; -// sizes_.resize(deep_structure.size()); -// for(size_t i = 0; i < deep_structure.size(); i++) -// { -// sizes_[i].resize(deep_structure[i].size()); -// for(size_t j = 0; j < deep_structure[i].size(); j++) -// { -// sizes_[i][j] = deep_structure[i][j].size(); -// global_size += deep_structure[i][j].size(); -// } -// } - -// std::vector flat_structure(global_size); -// size_t global_index = 0; -// for(size_t i = 0; i < deep_structure.size(); i++) -// for(size_t j = 0; j < deep_structure[i].size(); j++) -// for(size_t k = 0; k < deep_structure[i][j].size(); k++) -// flat_structure[global_index++] = deep_structure[i][j][k]; - -// return flat_structure; -// } - -// template std::vector > > Deepen(const std::vector& flat_structure) -// { -// size_t global_index = 0; -// std::vector > > deep_structure(sizes_.size()); -// for(size_t i = 0; i < deep_structure.size(); i++) -// { -// deep_structure[i].resize(sizes_[i].size()); -// for(size_t j = 0; j < deep_structure[i].size(); j++) -// { -// deep_structure[i][j].resize(sizes_[i][j]); -// for(size_t k = 0; k < deep_structure[i][j].size(); k++) -// deep_structure[i][j][k] = flat_structure[global_index++]; -// } -// } -// return deep_structure; -// } - -//private: -// std::vector > sizes_; -//}; - - - - - - - -//// this function will interpolat the vector wherever it is NAN or INF -//template -//void LinearlyInterpolate(std::vector& data) -//{ -// std::vector limits; -// limits.push_back(0); -// limits.push_back(data.size()-1); -// std::vector step_direction; -// step_direction.push_back(1); -// step_direction.push_back(-1); - -// // extrapolate -// for(int i = 0; i < 2; i++) -// { -// int index_first_real = IndexNextReal(data, limits[i] - step_direction[i], step_direction[i]); -// int index_next_real = IndexNextReal(data, index_first_real, step_direction[i]); -// if(index_next_real >= int(data.size()) || index_next_real < 0) -// return; - -// double slope = -// double(data[index_next_real] - data[index_first_real]) / -// double(index_next_real - index_first_real); - -// for(int j = limits[i]; j != index_first_real; j += step_direction[i]) -// data[j] = data[index_first_real] + (j - index_first_real) * slope; -// } - -// // interpolate -// int index_current_real = IndexNextReal(data, limits[0] - step_direction[0], step_direction[0]); -// int index_next_real = IndexNextReal(data, index_current_real, step_direction[0]); - -// while(index_next_real < int(data.size())) -// { -// double slope = -// double(data[index_next_real] - data[index_current_real]) / -// double(index_next_real - index_current_real); - -// for(int i = index_current_real + 1; i < index_next_real; i++) -// data[i] = data[index_current_real] + (i - index_current_real) * slope; - -// index_current_real = index_next_real; -// index_next_real = IndexNextReal(data, index_next_real, step_direction[0]); -// } -//} - -//template void PrintVector(std::vector v) -//{ -// for(size_t i = 0; i < v.size(); i++) -// std::cout << "(" << i << ": " << v[i] << ") "; -// std::cout << std::endl; -//} -//template void PrintVector(std::vector > v) -//{ -// for(size_t i = 0; i < v.size(); i++) -// { -// std::cout << i << " --------------------------------" << std::endl; -// PrintVector(v[i]); -// } -//} - -//template void PrintVector(std::vector > > v) -//{ -// for(size_t i = 0; i < v.size(); i++) -// { -// std::cout << i << " ================================" << std::endl; -// PrintVector(v[i]); -// } -//} - - -//template -//void Swap(T& a, T& b) -//{ -// T temp = a; -// a = b; -// b = temp; -//} - -//template -//Eigen::Matrix Std2Eigen(const std::vector &std) -//{ -// Eigen::Matrix eigen(std.size()); -// for(size_t i = 0; i < std.size(); i++) -// eigen(i) = std[i]; -// return eigen; -//} - -//template -//std::string Eigen2Mathematica(const Eigen::MatrixBase& eigen, const std::string &name = "") -//{ -// std::string mathematica; -// if(!name.empty()) -// mathematica += name + " = "; - -// mathematica += "{"; -// for(int row = 0; row < eigen.rows(); row++) -// { -// mathematica += "{"; -// for(int col = 0; col < eigen.cols(); col++) -// { -// mathematica += boost::lexical_cast(eigen(row, col)); -// if(col != eigen.cols() -1) -// mathematica += ", "; -// } -// mathematica += "}"; -// if(row != eigen.rows()-1) -// mathematica += ", "; -// } -// mathematica += "}"; -// if(!name.empty()) -// mathematica += ";"; - -// return mathematica; -//} - -//// returns the first index where the two vectors differ, if there is no difference then -1 is returned. -//template int -//DifferenceAt( -// const std::vector >& a, -// const std::vector >& b, -// const T& epsilon) -//{ -// if(a.size() < b.size()) -// return a.size(); -// else if(b.size() < a.size()) -// return b.size(); - -// for(size_t i = 0; i < a.size(); i++) -// for(size_t row = 0; row < n_rows; row++) -// for(size_t col = 0; col < n_cols; col++) -// { -// if(isnan(a[i](row, col)) && isnan(b[i](row, col))) -// continue; -// else if(isnan(a[i](row, col)) || isnan(b[i](row, col))) -// return i; -// else if( (a[i](row, col) - b[i](row, col) > epsilon) || (b[i](row, col) - a[i](row, col) > epsilon) ) -// return i; -// } - -// return -1; -//} - -//// returns the first index where the two vectors differ, if there is no difference then -1 is returned. -//template int -//DifferenceAt( -// const std::vector& a, -// const std::vector& b, -// const T& epsilon) -//{ -// if(a.size() < b.size()) -// return a.size(); -// else if(b.size() < a.size()) -// return b.size(); - -// for(size_t i = 0; i < a.size(); i++) -// { -// if(isnan(a[i]) && isnan(b[i])) -// continue; -// else if(isnan(a[i]) || isnan(b[i])) -// return i; -// else if( (a[i] - b[i] > epsilon) || (b[i] - a[i] > epsilon) ) -// return i; -// } - -// return -1; -//} - -//template std::vector ReverseOrder(std::vector ordered) -//{ -// std::vector reversed(ordered.size()); -// for(size_t i = 0; i < ordered.size(); i++) -// reversed[i] = ordered[ordered.size() - 1 - i]; -//} - -//template std::vector Count(T from, T to, T increment = 1) -//{ -// std::vector count(size_t((to - from)/increment)); -// count[0] = from; -// for(size_t i = 1; i < count.size(); i++) -// count[i] = count[i-1] + increment; -//} - -//template struct ValuesIndex -//{ -// std::vector values; -// int index; - -// bool operator < (const ValuesIndex& right_side) const -// { -// for(size_t i = 0; i < values.size(); i++) -// { -// if(values[i] < right_side.values[i]) -// return true; -// if(values[i] > right_side.values[i]) -// return false; -// } -// return false; -// } - - -// bool operator != (const ValuesIndex& right_side) const -// { -// for(size_t i = 0; i < values.size(); i++) -// if(values[i] != right_side.values[i]) -// return true; - -// return false; -// } -//}; - -//template void -//SortAndCollapse( -// std::vector >& values, -// std::vector& multiplicities) -//{ -// std::vector > values_indices(values.size()); -// for(size_t i = 0; i < values.size(); i++) -// { -// values_indices[i].index = i; -// values_indices[i].values = values[i]; -// } - -// std::sort(values_indices.begin(), values_indices.end()); - -// std::vector > temp_values = values; -// multiplicities = std::vector(values.size(), 0); -// size_t distinct_index = 0; -// values[0] = temp_values[values_indices[0].index]; -// for(size_t i = 0; i < values.size(); i++) -// { -// if(i > 0 && temp_values[values_indices[i-1].index] != temp_values[values_indices[i].index]) -// values[++distinct_index] = temp_values[values_indices[i].index]; -// multiplicities[distinct_index]++; -// } -// values.resize(distinct_index+1); -// multiplicities.resize(distinct_index+1); -//} - - - - - - - - - - -//template struct ValueIndex -//{ -// T value; -// int index; - -// bool operator < (const ValueIndex& str) const -// { -// return (value < str.value); -// } -//}; -//template std::vector SortAscend(const std::vector &values) -//{ -// std::vector indices(values.size()); - -// std::vector > values_indices(values.size()); -// for(int i = 0; i < int(values.size()); i++) -// { -// values_indices[i].index = i; -// values_indices[i].value = values[i]; -// } - -// std::sort(values_indices.begin(), values_indices.end()); - -// for(int i = 0; i < int(indices.size()); i++) -// indices[i] = values_indices[i].index; - -// return indices; -//} - -//template std::vector SortDescend(const std::vector &values) -//{ -// std::vector ascend_indices = SortAscend(values); -// std::vector descend_indices(ascend_indices.size()); - -// for(int i = 0; i < int(ascend_indices.size()); i++) -// descend_indices[i] = ascend_indices[ascend_indices.size()-1-i]; - -// return descend_indices; -//} - - -//template void Sort(std::vector &ref_values, bool order = 0) -//{ -// std::vector temp_ref_values = ref_values; -// std::vector indices; -// if(order == 0) -// indices = SortAscend(temp_ref_values); -// else -// indices = SortDescend(temp_ref_values); - - -// for(int i = 0; i < int(indices.size()); i++) -// ref_values[i] = temp_ref_values[indices[i]]; -//} - - -//template void SortAscend(std::vector &ref_values, std::vector &values) -//{ -// std::vector temp_ref_values = ref_values; -// std::vector temp_values = values; - -// std::vector indices = SortAscend(temp_ref_values); - -// for(int i = 0; i < int(indices.size()); i++) -// { -// ref_values[i] = temp_ref_values[indices[i]]; -// values[i] = temp_values[indices[i]]; -// } -//} - -//template void SortDescend(std::vector &ref_values, std::vector &values) -//{ -// std::vector temp_ref_values = ref_values; -// std::vector temp_values = values; - -// std::vector indices = SortDescend(temp_ref_values); - -// for(int i = 0; i < int(indices.size()); i++) -// { -// ref_values[i] = temp_ref_values[indices[i]]; -// values[i] = temp_values[indices[i]]; -// } -//} - -//template void SortAscend(std::vector &ref_values, -// std::vector &values1, std::vector &values2) -//{ -// std::vector temp_ref_values = ref_values; -// std::vector temp_values1 = values1; -// std::vector temp_values2 = values2; - -// std::vector indices = SortAscend(temp_ref_values); - -// for(int i = 0; i < int(indices.size()); i++) -// { -// ref_values[i] = temp_ref_values[indices[i]]; -// values1[i] = temp_values1[indices[i]]; -// values2[i] = temp_values2[indices[i]]; -// } -//} - -//template void SortDescend(std::vector &ref_values, -// std::vector &values1, std::vector &values2) -//{ -// std::vector temp_ref_values = ref_values; -// std::vector temp_values1 = values1; -// std::vector temp_values2 = values2; - -// std::vector indices = SortDescend(temp_ref_values); - -// for(int i = 0; i < int(indices.size()); i++) -// { -// ref_values[i] = temp_ref_values[indices[i]]; -// values1[i] = temp_values1[indices[i]]; -// values2[i] = temp_values2[indices[i]]; -// } -//} - -//template int BoundIndex(const std::vector &values, bool bound_type) // bound type 1 for max and 0 for min -//{ -// int BoundIndex = 0; -// T bound_value = bound_type ? -std::numeric_limits::max() : std::numeric_limits::max(); - -// for(int i = 0; i < int(values.size()); i++) -// if(bound_type ? (values[i] > bound_value) : (values[i] < bound_value) ) -// { -// BoundIndex = i; -// bound_value = values[i]; -// } - -// return BoundIndex; -//} - -//template T bound_value(const std::vector &values, bool bound_type) // bound type 1 for max and 0 for min -//{ -// return values[BoundIndex(values, bound_type)]; -//} - -//template -//std::vector Apply(const std::vector &input, Tout(*f)(Tin)) -//{ -// std::vector output(input.size()); -// for(size_t i = 0; i < output.size(); i++) -// output[i] = (*f)(input[i]); - -// return output; -//} - -//template std::vector -//SetSum(const std::vector &input, T sum) -//{ -// T old_sum = 0; -// for(size_t i = 0; i < input.size(); i++) -// old_sum += input[i]; -// T factor = sum/old_sum; - -// std::vector output(input.size()); -// for(size_t i = 0; i < input.size(); i++) -// output[i] = factor*input[i]; - -// return output; -//} - -//// geometry functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -//template -//Eigen::Quaternion Delta2Quaternion(const Eigen::MatrixBase& delta) -//{ -// typename T::Scalar angle = delta.norm(); -// Eigen::Matrix axis = delta.normalized(); -// Eigen::Quaternion q; -// if(std::isfinite(axis.norm())) -// q = Eigen::AngleAxisd(angle, axis); -// else -// q = Eigen::Quaterniond::Identity(); - -// return q; -//} - -//// this function finds the intersection between two lines. a is a point on line and d_a is the direction -//// vector of the line. if lines do not cross, this will return the point which minimizes the squared -//// distance between the two lines. if they are parallel, then inf will be returned. -//template -//Eigen::Matrix Intersection( -// const Eigen::MatrixBase& a, -// const Eigen::MatrixBase& d_a_in, -// const Eigen::MatrixBase& b, -// const Eigen::MatrixBase& d_b_in) -//{ -// const Eigen::Matrix d_a = d_a_in.normalized(); -// const Eigen::Matrix d_b = d_b_in.normalized(); - -// double c_a = d_a.dot((b-a) - d_b.dot(b-a)*d_b)/ -// (1.-pow(d_b.dot(d_a),2)); -// double c_b = d_b.dot((a-b) - d_a.dot(a-b)*d_a)/ -// (1.-pow(d_a.dot(d_b),2)); - -// return((b + c_b*d_b)+(a + c_a*d_a))/2.; -//} - -//template -//typename T1::Scalar Angle( -// const Eigen::MatrixBase& a, -// const Eigen::MatrixBase& b) -//{ -// return atan2(a.cross(b).norm(), a.dot(b)); -//} - -//template -//Eigen::Matrix OrthogonalUnitVector(const Eigen::MatrixBase& v) -//{ -// Eigen::Matrix n = -// (v.cross(Eigen::Matrix(1.0, 0, 0))).normalized(); -// if(!std::isfinite(n.norm())) -// n = (v.cross(Eigen::Matrix(0, 1.0, 0))).normalized(); - -// return n; -//} - -//// returns the angle and axis which rotate a onto b -//template -//Eigen::AngleAxis AngleAxis( -// const Eigen::MatrixBase& a, -// const Eigen::MatrixBase& b) -//{ -// Eigen::Matrix axis = a.cross(b); -// typename T1::Scalar axis_norm = axis.norm(); -// axis /= axis_norm; - -// // if the axis is inf then the angle is either 0 or pi, so the axis has to be just some orthogonal vector -// if(!std::isfinite(axis.norm())) -// axis = OrthogonalUnitVector(a); - -// typename T1::Scalar angle = atan2(axis_norm, a.dot(b)); - -// return Eigen::AngleAxis(angle, axis); -//} +inline double Sigmoid(const double& x) +{ + return 1.0 / (1.0 + std::exp(-x)); +} +inline double Logit(const double& x) +{ + return std::log(x / (1.0 - x)); +} + + +inline std::string DateAndTimeString() +{ + time_t rawtime; + struct tm * timeinfo; + char buffer[80]; + + time (&rawtime); + timeinfo = localtime(&rawtime); + + strftime(buffer,80,"%d.%m.%Y_%I.%M.%S",timeinfo); + std::string current_time(buffer); + + return current_time; +} + + + +inline double get_wall_time(){ + struct timeval time; + if (gettimeofday(&time,NULL)){ + std::cout << "WARNING: gettimeofday() Error" << std::endl; + return 0; + } + return (double)time.tv_sec + (double)time.tv_usec * .000001; +} + + +template void Compare(const std::vector vector_1, + const std::vector vector_2, + char* name_1, + char* name_2) { + if (vector_1.size() == vector_2.size()) { + T difference = 0; + for (uint i = 0; i < vector_1.size(); i++) { + std::cout << name_1 << "[" << i << "]: " << vector_1[i] << ", " + << name_2 << "[" << i << "]: " << vector_2[i] << ", " + << "difference: " << std::abs(vector_1[i] - vector_2[i]) << std::endl; + difference += std::abs(vector_1[i] - vector_2[i]); + } + std::cout << "average difference between " << name_1 << " and " << name_2 << " : " << difference / vector_1.size() << std::endl; + + } else { + std::cout << "WARNING: The two vectors to be compared have a different amount of elements. " << std::endl + << name_1 << ": " << vector_1.size() << ", " << name_2 << ": " << vector_2.size() << std::endl + << "Comparison aborted." << std::endl; + } +} + +template void AverageDifference(const std::vector vector_1, + const std::vector vector_2, + char* name_1, + char* name_2) { + if (vector_1.size() == vector_2.size()) { + T difference = 0; + for (uint i = 0; i < vector_1.size(); i++) { + difference += std::abs(vector_1[i] - vector_2[i]); + } + std::cout << "average difference between " << name_1 << " and " << name_2 << " : " << difference / vector_1.size() << std::endl; + + } else { + std::cout << "WARNING: The two vectors to be compared have a different amount of elements. " << std::endl + << name_1 << ": " << vector_1.size() << ", " << name_2 << ": " << vector_2.size() << std::endl + << "Comparison aborted." << std::endl; + } +} + +template void Output(const T value, + char* name) { + std::cout << name << ": " << value << std::endl; +} + + +template +int IndexNextReal( + const std::vector& data, + const int& current_index = -1, + const int& step_size = 1) +{ + int index_next_real = current_index + step_size; + while( + index_next_real < int(data.size()) && + index_next_real >= 0 && + !std::isfinite(data[index_next_real])) index_next_real+=step_size; + + return index_next_real; +} + +// various useful functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> +class Structurer2D +{ +public: + Structurer2D(){} + + ~Structurer2D() {} + + template std::vector Flatten(const std::vector >& deep_structure) + { + sizes_.resize(deep_structure.size()); + size_t global_size = 0; + for(size_t i = 0; i < deep_structure.size(); i++) + { + sizes_[i] = deep_structure[i].size(); + global_size += deep_structure[i].size(); + } + + std::vector flat_structure(global_size); + size_t global_index = 0; + for(size_t i = 0; i < deep_structure.size(); i++) + for(size_t j = 0; j < deep_structure[i].size(); j++) + flat_structure[global_index++] = deep_structure[i][j]; + + return flat_structure; + } + + template std::vector > Deepen(const std::vector& flat_structure) + { + std::vector > deep_structure(sizes_.size()); + + size_t global_index = 0; + for(size_t i = 0; i < deep_structure.size(); i++) + { + deep_structure[i].resize(sizes_[i]); + for(size_t j = 0; j < deep_structure[i].size(); j++) + deep_structure[i][j] = flat_structure[global_index++]; + } + return deep_structure; + } + +private: + std::vector sizes_; +}; + + + +class Structurer3D +{ +public: + Structurer3D(){} + + ~Structurer3D() {} + + template std::vector Flatten(const std::vector > >& deep_structure) + { + size_t global_size = 0; + sizes_.resize(deep_structure.size()); + for(size_t i = 0; i < deep_structure.size(); i++) + { + sizes_[i].resize(deep_structure[i].size()); + for(size_t j = 0; j < deep_structure[i].size(); j++) + { + sizes_[i][j] = deep_structure[i][j].size(); + global_size += deep_structure[i][j].size(); + } + } + + std::vector flat_structure(global_size); + size_t global_index = 0; + for(size_t i = 0; i < deep_structure.size(); i++) + for(size_t j = 0; j < deep_structure[i].size(); j++) + for(size_t k = 0; k < deep_structure[i][j].size(); k++) + flat_structure[global_index++] = deep_structure[i][j][k]; + + return flat_structure; + } + + template std::vector > > Deepen(const std::vector& flat_structure) + { + size_t global_index = 0; + std::vector > > deep_structure(sizes_.size()); + for(size_t i = 0; i < deep_structure.size(); i++) + { + deep_structure[i].resize(sizes_[i].size()); + for(size_t j = 0; j < deep_structure[i].size(); j++) + { + deep_structure[i][j].resize(sizes_[i][j]); + for(size_t k = 0; k < deep_structure[i][j].size(); k++) + deep_structure[i][j][k] = flat_structure[global_index++]; + } + } + return deep_structure; + } + +private: + std::vector > sizes_; +}; + + + + + + + +// this function will interpolat the vector wherever it is NAN or INF +template +void LinearlyInterpolate(std::vector& data) +{ + std::vector limits; + limits.push_back(0); + limits.push_back(data.size()-1); + std::vector step_direction; + step_direction.push_back(1); + step_direction.push_back(-1); + + // extrapolate + for(int i = 0; i < 2; i++) + { + int index_first_real = IndexNextReal(data, limits[i] - step_direction[i], step_direction[i]); + int index_next_real = IndexNextReal(data, index_first_real, step_direction[i]); + if(index_next_real >= int(data.size()) || index_next_real < 0) + return; + + double slope = + double(data[index_next_real] - data[index_first_real]) / + double(index_next_real - index_first_real); + + for(int j = limits[i]; j != index_first_real; j += step_direction[i]) + data[j] = data[index_first_real] + (j - index_first_real) * slope; + } + + // interpolate + int index_current_real = IndexNextReal(data, limits[0] - step_direction[0], step_direction[0]); + int index_next_real = IndexNextReal(data, index_current_real, step_direction[0]); + + while(index_next_real < int(data.size())) + { + double slope = + double(data[index_next_real] - data[index_current_real]) / + double(index_next_real - index_current_real); + + for(int i = index_current_real + 1; i < index_next_real; i++) + data[i] = data[index_current_real] + (i - index_current_real) * slope; + + index_current_real = index_next_real; + index_next_real = IndexNextReal(data, index_next_real, step_direction[0]); + } +} + +template void PrintVector(std::vector v) +{ + for(size_t i = 0; i < v.size(); i++) + std::cout << "(" << i << ": " << v[i] << ") "; + std::cout << std::endl; +} +template void PrintVector(std::vector > v) +{ + for(size_t i = 0; i < v.size(); i++) + { + std::cout << i << " --------------------------------" << std::endl; + PrintVector(v[i]); + } +} + +template void PrintVector(std::vector > > v) +{ + for(size_t i = 0; i < v.size(); i++) + { + std::cout << i << " ================================" << std::endl; + PrintVector(v[i]); + } +} + + +template +void Swap(T& a, T& b) +{ + T temp = a; + a = b; + b = temp; +} + +template +Eigen::Matrix Std2Eigen(const std::vector &std) +{ + Eigen::Matrix eigen(std.size()); + for(size_t i = 0; i < std.size(); i++) + eigen(i) = std[i]; + return eigen; +} + +template +std::string Eigen2Mathematica(const Eigen::MatrixBase& eigen, const std::string &name = "") +{ + std::string mathematica; + if(!name.empty()) + mathematica += name + " = "; + + mathematica += "{"; + for(int row = 0; row < eigen.rows(); row++) + { + mathematica += "{"; + for(int col = 0; col < eigen.cols(); col++) + { + mathematica += boost::lexical_cast(eigen(row, col)); + if(col != eigen.cols() -1) + mathematica += ", "; + } + mathematica += "}"; + if(row != eigen.rows()-1) + mathematica += ", "; + } + mathematica += "}"; + if(!name.empty()) + mathematica += ";"; + + return mathematica; +} + +// returns the first index where the two vectors differ, if there is no difference then -1 is returned. +template int +DifferenceAt( + const std::vector >& a, + const std::vector >& b, + const T& epsilon) +{ + if(a.size() < b.size()) + return a.size(); + else if(b.size() < a.size()) + return b.size(); + + for(size_t i = 0; i < a.size(); i++) + for(size_t row = 0; row < n_rows; row++) + for(size_t col = 0; col < n_cols; col++) + { + if(isnan(a[i](row, col)) && isnan(b[i](row, col))) + continue; + else if(isnan(a[i](row, col)) || isnan(b[i](row, col))) + return i; + else if( (a[i](row, col) - b[i](row, col) > epsilon) || (b[i](row, col) - a[i](row, col) > epsilon) ) + return i; + } + + return -1; +} + +// returns the first index where the two vectors differ, if there is no difference then -1 is returned. +template int +DifferenceAt( + const std::vector& a, + const std::vector& b, + const T& epsilon) +{ + if(a.size() < b.size()) + return a.size(); + else if(b.size() < a.size()) + return b.size(); + + for(size_t i = 0; i < a.size(); i++) + { + if(isnan(a[i]) && isnan(b[i])) + continue; + else if(isnan(a[i]) || isnan(b[i])) + return i; + else if( (a[i] - b[i] > epsilon) || (b[i] - a[i] > epsilon) ) + return i; + } + + return -1; +} + +template std::vector ReverseOrder(std::vector ordered) +{ + std::vector reversed(ordered.size()); + for(size_t i = 0; i < ordered.size(); i++) + reversed[i] = ordered[ordered.size() - 1 - i]; +} + +template std::vector Count(T from, T to, T increment = 1) +{ + std::vector count(size_t((to - from)/increment)); + count[0] = from; + for(size_t i = 1; i < count.size(); i++) + count[i] = count[i-1] + increment; +} + +template struct ValuesIndex +{ + std::vector values; + int index; + + bool operator < (const ValuesIndex& right_side) const + { + for(size_t i = 0; i < values.size(); i++) + { + if(values[i] < right_side.values[i]) + return true; + if(values[i] > right_side.values[i]) + return false; + } + return false; + } + + + bool operator != (const ValuesIndex& right_side) const + { + for(size_t i = 0; i < values.size(); i++) + if(values[i] != right_side.values[i]) + return true; + + return false; + } +}; + +template void +SortAndCollapse( + std::vector >& values, + std::vector& multiplicities) +{ + std::vector > values_indices(values.size()); + for(size_t i = 0; i < values.size(); i++) + { + values_indices[i].index = i; + values_indices[i].values = values[i]; + } + + std::sort(values_indices.begin(), values_indices.end()); + + std::vector > temp_values = values; + multiplicities = std::vector(values.size(), 0); + size_t distinct_index = 0; + values[0] = temp_values[values_indices[0].index]; + for(size_t i = 0; i < values.size(); i++) + { + if(i > 0 && temp_values[values_indices[i-1].index] != temp_values[values_indices[i].index]) + values[++distinct_index] = temp_values[values_indices[i].index]; + multiplicities[distinct_index]++; + } + values.resize(distinct_index+1); + multiplicities.resize(distinct_index+1); +} + + + + + + + + + + +template struct ValueIndex +{ + T value; + int index; + + bool operator < (const ValueIndex& str) const + { + return (value < str.value); + } +}; +template std::vector SortAscend(const std::vector &values) +{ + std::vector indices(values.size()); + + std::vector > values_indices(values.size()); + for(int i = 0; i < int(values.size()); i++) + { + values_indices[i].index = i; + values_indices[i].value = values[i]; + } + + std::sort(values_indices.begin(), values_indices.end()); + + for(int i = 0; i < int(indices.size()); i++) + indices[i] = values_indices[i].index; + + return indices; +} + +template std::vector SortDescend(const std::vector &values) +{ + std::vector ascend_indices = SortAscend(values); + std::vector descend_indices(ascend_indices.size()); + + for(int i = 0; i < int(ascend_indices.size()); i++) + descend_indices[i] = ascend_indices[ascend_indices.size()-1-i]; + + return descend_indices; +} + + +template void Sort(std::vector &ref_values, bool order = 0) +{ + std::vector temp_ref_values = ref_values; + std::vector indices; + if(order == 0) + indices = SortAscend(temp_ref_values); + else + indices = SortDescend(temp_ref_values); + + + for(int i = 0; i < int(indices.size()); i++) + ref_values[i] = temp_ref_values[indices[i]]; +} + + +template void SortAscend(std::vector &ref_values, std::vector &values) +{ + std::vector temp_ref_values = ref_values; + std::vector temp_values = values; + + std::vector indices = SortAscend(temp_ref_values); + + for(int i = 0; i < int(indices.size()); i++) + { + ref_values[i] = temp_ref_values[indices[i]]; + values[i] = temp_values[indices[i]]; + } +} + +template void SortDescend(std::vector &ref_values, std::vector &values) +{ + std::vector temp_ref_values = ref_values; + std::vector temp_values = values; + + std::vector indices = SortDescend(temp_ref_values); + + for(int i = 0; i < int(indices.size()); i++) + { + ref_values[i] = temp_ref_values[indices[i]]; + values[i] = temp_values[indices[i]]; + } +} + +template void SortAscend(std::vector &ref_values, + std::vector &values1, std::vector &values2) +{ + std::vector temp_ref_values = ref_values; + std::vector temp_values1 = values1; + std::vector temp_values2 = values2; + + std::vector indices = SortAscend(temp_ref_values); + + for(int i = 0; i < int(indices.size()); i++) + { + ref_values[i] = temp_ref_values[indices[i]]; + values1[i] = temp_values1[indices[i]]; + values2[i] = temp_values2[indices[i]]; + } +} + +template void SortDescend(std::vector &ref_values, + std::vector &values1, std::vector &values2) +{ + std::vector temp_ref_values = ref_values; + std::vector temp_values1 = values1; + std::vector temp_values2 = values2; + + std::vector indices = SortDescend(temp_ref_values); + + for(int i = 0; i < int(indices.size()); i++) + { + ref_values[i] = temp_ref_values[indices[i]]; + values1[i] = temp_values1[indices[i]]; + values2[i] = temp_values2[indices[i]]; + } +} + +template int BoundIndex(const std::vector &values, bool bound_type) // bound type 1 for max and 0 for min +{ + int BoundIndex = 0; + T bound_value = bound_type ? -std::numeric_limits::max() : std::numeric_limits::max(); + + for(int i = 0; i < int(values.size()); i++) + if(bound_type ? (values[i] > bound_value) : (values[i] < bound_value) ) + { + BoundIndex = i; + bound_value = values[i]; + } + + return BoundIndex; +} + +template T bound_value(const std::vector &values, bool bound_type) // bound type 1 for max and 0 for min +{ + return values[BoundIndex(values, bound_type)]; +} + +template +std::vector Apply(const std::vector &input, Tout(*f)(Tin)) +{ + std::vector output(input.size()); + for(size_t i = 0; i < output.size(); i++) + output[i] = (*f)(input[i]); + + return output; +} + +template std::vector +SetSum(const std::vector &input, T sum) +{ + T old_sum = 0; + for(size_t i = 0; i < input.size(); i++) + old_sum += input[i]; + T factor = sum/old_sum; + + std::vector output(input.size()); + for(size_t i = 0; i < input.size(); i++) + output[i] = factor*input[i]; + + return output; +} + +// geometry functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> +template +Eigen::Quaternion Delta2Quaternion(const Eigen::MatrixBase& delta) +{ + typename T::Scalar angle = delta.norm(); + Eigen::Matrix axis = delta.normalized(); + Eigen::Quaternion q; + if(std::isfinite(axis.norm())) + q = Eigen::AngleAxisd(angle, axis); + else + q = Eigen::Quaterniond::Identity(); + + return q; +} + +// this function finds the intersection between two lines. a is a point on line and d_a is the direction +// vector of the line. if lines do not cross, this will return the point which minimizes the squared +// distance between the two lines. if they are parallel, then inf will be returned. +template +Eigen::Matrix Intersection( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& d_a_in, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& d_b_in) +{ + const Eigen::Matrix d_a = d_a_in.normalized(); + const Eigen::Matrix d_b = d_b_in.normalized(); + + double c_a = d_a.dot((b-a) - d_b.dot(b-a)*d_b)/ + (1.-pow(d_b.dot(d_a),2)); + double c_b = d_b.dot((a-b) - d_a.dot(a-b)*d_a)/ + (1.-pow(d_a.dot(d_b),2)); + + return((b + c_b*d_b)+(a + c_a*d_a))/2.; +} + +template +typename T1::Scalar Angle( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b) +{ + return atan2(a.cross(b).norm(), a.dot(b)); +} + +template +Eigen::Matrix OrthogonalUnitVector(const Eigen::MatrixBase& v) +{ + Eigen::Matrix n = + (v.cross(Eigen::Matrix(1.0, 0, 0))).normalized(); + if(!std::isfinite(n.norm())) + n = (v.cross(Eigen::Matrix(0, 1.0, 0))).normalized(); + + return n; +} + +// returns the angle and axis which rotate a onto b +template +Eigen::AngleAxis AngleAxis( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b) +{ + Eigen::Matrix axis = a.cross(b); + typename T1::Scalar axis_norm = axis.norm(); + axis /= axis_norm; + + // if the axis is inf then the angle is either 0 or pi, so the axis has to be just some orthogonal vector + if(!std::isfinite(axis.norm())) + axis = OrthogonalUnitVector(a); + + typename T1::Scalar angle = atan2(axis_norm, a.dot(b)); + + return Eigen::AngleAxis(angle, axis); +} // the input quaternion has the order xyzw inline Eigen::Matrix QuaternionMatrix(const Eigen::Matrix& q_xyzw) @@ -759,530 +757,689 @@ inline Eigen::Matrix QuaternionMatrix(const Eigen::Matrix class TransformationSequence -//{ -//public: -// TransformationSequence( -// const Eigen::Matrix &R = Eigen::Matrix::Identity(), -// const Eigen::Matrix &t = Eigen::Matrix::Zero()): R_(R), t_(t) {} - -// ~TransformationSequence(){} - -// void PreRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) -// { -// t_ = t_ - R_*R*c + R_*c; -// R_ = R_*R; -// } -// void PreTranslate(const Eigen::Matrix &t) -// { -// t_ = t_ + R_*t; -// } -// void PostRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) -// { -// t_ = R*t_ - R*c + c; -// R_ = R*R_; -// } -// void PostTranslate(const Eigen::Matrix &t) -// { -// t_ = t_ + t; -// } - -// void get(Eigen::Matrix &R, Eigen::Matrix &t) -// { -// R = R_; -// t = t_; -// } - -// void NormalizeQuat() -// { -// Eigen::Quaternion q; q = R_; -// q.normalize(); -// R_ = q.toRotationMatrix(); -// } - -//private: -// Eigen::Matrix R_; -// Eigen::Matrix t_; - -//}; - -//// depth image functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -//// converts from cartesian to image space -//template Eigen::Matrix -//CartCoord2ImageCoord(const Eigen::Matrix& cart, -// const Eigen::Matrix& camera_matrix) -//{ -// return (camera_matrix * cart/cart(2)).topRows(2); -//} - -//// converts from cartesian space to image index, (row, col) -//template Eigen::Matrix -//CartCoord2ImageIndex(const Eigen::Matrix& cart, -// const Eigen::Matrix& camera_matrix) -//{ -// Eigen::Matrix image = CartCoord2ImageCoord(cart, camera_matrix); -// Eigen::Matrix image_index; -// image_index(0) = floor(image(1)+0.5); -// image_index(1) = floor(image(0)+0.5); - -// return image_index; -//} - -//// converts from image coordinates and depth (z value) to cartesian coordinates -//template Eigen::Matrix -//ImageCoord2CartCoord(const Eigen::Matrix& image, -// const T& depth, -// const Eigen::Matrix& camera_matrix_inverse) -//{ -// Eigen::Matrix image_augmented; -// image_augmented.topRows(2) = image; -// image_augmented(2) = 1; - -// return depth * camera_matrix_inverse * image_augmented; -//} - -//// converts from image index (row, col) and depth (z value) to cartesian coordinates -//template Eigen::Matrix -//ImageIndex2CartCoord(const Eigen::Matrix& image_index, -// const T& depth, -// const Eigen::Matrix& camera_matrix_inverse) -//{ -// Eigen::Matrix image; -// image(0) = image_index(1); -// image(1) = image_index(0); -// return ImageCoord2CartCoord(image, depth, camera_matrix_inverse); -//} - - -//template Eigen::Matrix , -1, -1> -//Image2Points(const Eigen::Matrix& image, -// const Eigen::Matrix& camera_matrix) -//{ -// Eigen::Matrix camera_matrix_inverse = camera_matrix.inverse(); - -// Eigen::Matrix , -1, -1> points(image.rows(), image.cols()); -// for(size_t row = 0; row < points.rows(); row++) -// for(size_t col = 0; col < points.cols(); col++) -// points(row, col) = ImageIndex2CartCoord(Eigen::Vector2i(row, col), -// image(row, col), -// camera_matrix_inverse); - -// return points; -//} - - - - -//template std::vector > -//DepthImage2CartVectors(const std::vector& image, -// const size_t& n_rows, const size_t& n_cols, -// const Eigen::Matrix& camera_matrix) -//{ -// Eigen::Matrix camera_matrix_inverse = camera_matrix.inverse(); - -// std::vector > vectors(n_rows*n_cols); -// for(size_t row = 0; row < n_rows; row++) -// for(size_t col = 0; col < n_cols; col++) -// vectors[row*n_cols + col] = -// ImageIndex2CartCoord( -// Eigen::Vector2i(row, col), -// image[row*n_cols + col], -// camera_matrix_inverse); - -// return vectors; -//} - -//template std::vector -//CartVectors2DepthImage( -// const std::vector >& vectors, -// const int& n_rows, const int& n_cols, -// const Eigen::Matrix& camera_matrix) -//{ -// std::vector image(n_rows*n_cols, NAN); - -// for(size_t i = 0; i < vectors.size(); i++) -// { -// if( isnan(vectors[i](0)) || isnan(vectors[i](1)) || isnan(vectors[i](2)) ) -// continue; - -// Eigen::Matrix index = CartCoord2ImageIndex(vectors[i], camera_matrix); - -// if(index(0) < 0 || index(0) >= n_rows || index(1) < 0 || index(1) >= n_cols) -// { -// std::cout << "ERROR: in CartVectors2DepthImage vector comes to lie outside of image " << std::endl; -// exit(-1); -// } - -// image[index(0)*n_cols + index(1)] = vectors[i](2); -// } - -// return image; -//} - - - -//// numerically stable implementation of log(sum(exp(xi))) >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -//inline double log_sum_exp(double a, double b) -//{ -// if(a>b) return a + log(1+exp(b-a)); -// else return b + log(1+exp(a-b)); -//} -//inline double log_sum_exp(double a, double b, double c) -//{ -// if(a>b && a>c) return a + log(1+exp(b-a)+exp(c-a)); -// if(b>c && b>a) return b + log(1+exp(a-b)+exp(c-b)); -// else return c + log(1+exp(a-c)+exp(b-c)); -//} -//inline double log_sum_exp(double a, double b, double c, double d) -//{ -// if(a>b && a>c && a>d) return a + log(1+exp(b-a)+exp(c-a)+exp(d-a)); -// if(b>a && b>c && b>d) return b + log(1+exp(a-b)+exp(c-b)+exp(d-b)); -// if(c>a && c>b && c>d) return c + log(1+exp(a-c)+exp(b-c)+exp(d-c)); -// else return d + log(1+exp(a-d)+exp(b-d)+exp(c-d)); -//} - -//template T log_sum_exp(std::vector exponents) -//{ -// T max_exponent = -std::numeric_limits::max(); -// for(size_t i = 0; i < exponents.size(); i++) -// if(exponents[i] > max_exponent) max_exponent = exponents[i]; - -// for(size_t i = 0; i < exponents.size(); i++) -// exponents[i] -= max_exponent; - -// T sum = 0; -// for(size_t i = 0; i < exponents.size(); i++) -// sum += exp(exponents[i]); - -// return max_exponent + log(sum); -//} - - -//template class LogSumExp -//{ -//public: -// LogSumExp(){} -// ~LogSumExp(){} - -// void add_exponent(T exponent) -// { -// exponents_.push_back(exponent); -// } - -// T Compute() -// { -// T max_exponent = -std::numeric_limits::max(); -// for(int i = 0; i < int(exponents_.size()); i++) -// if(exponents_[i] > max_exponent) max_exponent = exponents_[i]; - -// for(int i = 0; i < int(exponents_.size()); i++) -// exponents_[i] -= max_exponent; - -// T sum = 0; -// for(int i = 0; i < int(exponents_.size()); i++) -// sum += exp(exponents_[i]); - -// return max_exponent + log(sum); -// } -//private: -// std::vector exponents_; -//}; - -//// sampling class >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -//class DiscreteSampler -//{ -//public: -// template DiscreteSampler(std::vector log_prob) -// { -// fibo_.seed(RANDOM_SEED); - -// // compute the prob and normalize them -// sorted_indices_ = dbot::hf::SortDescend(log_prob); -// double max = log_prob[sorted_indices_[0]]; -// for(int i = 0; i < int(log_prob.size()); i++) -// log_prob[i] -= max; - -// std::vector prob(log_prob.size()); -// double sum = 0; -// for(int i = 0; i < int(log_prob.size()); i++) -// { -// prob[i] = exp(log_prob[i]); -// sum += prob[i]; -// } -// for(int i = 0; i < int(prob.size()); i++) -// prob[i] /= sum; - -// // compute the cumulative probability -// cumulative_prob_.resize(log_prob.size()); -// cumulative_prob_[0] = prob[sorted_indices_[0]]; -// for(int i = 1; i < int(log_prob.size()); i++) -// cumulative_prob_[i] = cumulative_prob_[i-1] + prob[sorted_indices_[i]]; -// } - -// ~DiscreteSampler() {} - -// int Sample() -// { -// double uniform_sample = fibo_(); -// int sample_index = 0; -// while(uniform_sample > cumulative_prob_[sample_index]) sample_index++; +template class TransformationSequence +{ +public: + TransformationSequence( + const Eigen::Matrix &R = Eigen::Matrix::Identity(), + const Eigen::Matrix &t = Eigen::Matrix::Zero()): R_(R), t_(t) {} + + ~TransformationSequence(){} + + void PreRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) + { + t_ = t_ - R_*R*c + R_*c; + R_ = R_*R; + } + void PreTranslate(const Eigen::Matrix &t) + { + t_ = t_ + R_*t; + } + void PostRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) + { + t_ = R*t_ - R*c + c; + R_ = R*R_; + } + void PostTranslate(const Eigen::Matrix &t) + { + t_ = t_ + t; + } + + void get(Eigen::Matrix &R, Eigen::Matrix &t) + { + R = R_; + t = t_; + } + + void NormalizeQuat() + { + Eigen::Quaternion q; q = R_; + q.normalize(); + R_ = q.toRotationMatrix(); + } + +private: + Eigen::Matrix R_; + Eigen::Matrix t_; + +}; + +// depth image functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> +// converts from cartesian to image space +template Eigen::Matrix +CartCoord2ImageCoord(const Eigen::Matrix& cart, + const Eigen::Matrix& camera_matrix) +{ + return (camera_matrix * cart/cart(2)).topRows(2); +} -// return sorted_indices_[sample_index]; -// } +// converts from cartesian space to image index, (row, col) +template Eigen::Matrix +CartCoord2ImageIndex(const Eigen::Matrix& cart, + const Eigen::Matrix& camera_matrix) +{ + Eigen::Matrix image = CartCoord2ImageCoord(cart, camera_matrix); + Eigen::Matrix image_index; + image_index(0) = floor(image(1)+0.5); + image_index(1) = floor(image(0)+0.5); + return image_index; +} +// converts from image coordinates and depth (z value) to cartesian coordinates +template Eigen::Matrix +ImageCoord2CartCoord(const Eigen::Matrix& image, + const T& depth, + const Eigen::Matrix& camera_matrix_inverse) +{ + Eigen::Matrix image_augmented; + image_augmented.topRows(2) = image; + image_augmented(2) = 1; -// int MapStandardGaussian(double gaussian_sample) const -// { -// // map from a gaussian to a uniform distribution -// double uniform_sample = 0.5 * -// (1.0 + std::erf(gaussian_sample / std::sqrt(2.0))); -// int sample_index = 0; -// while(uniform_sample > cumulative_prob_[sample_index]) sample_index++; //TODO: THIS COULD BE DONE IN LOG TIME + return depth * camera_matrix_inverse * image_augmented; +} -// return sorted_indices_[sample_index]; -// } +// converts from image index (row, col) and depth (z value) to cartesian coordinates +template Eigen::Matrix +ImageIndex2CartCoord(const Eigen::Matrix& image_index, + const T& depth, + const Eigen::Matrix& camera_matrix_inverse) +{ + Eigen::Matrix image; + image(0) = image_index(1); + image(1) = image_index(0); + return ImageCoord2CartCoord(image, depth, camera_matrix_inverse); +} + + +template Eigen::Matrix , -1, -1> +Image2Points(const Eigen::Matrix& image, + const Eigen::Matrix& camera_matrix) +{ + Eigen::Matrix camera_matrix_inverse = camera_matrix.inverse(); + + Eigen::Matrix , -1, -1> points(image.rows(), image.cols()); + for(size_t row = 0; row < points.rows(); row++) + for(size_t col = 0; col < points.cols(); col++) + points(row, col) = ImageIndex2CartCoord(Eigen::Vector2i(row, col), + image(row, col), + camera_matrix_inverse); + + return points; +} -//private: -// boost::lagged_fibonacci607 fibo_; -// std::vector sorted_indices_; -// std::vector cumulative_prob_; -//}; -//// the above class should go away -//class DiscreteDistribution -//{ -//public: -// template DiscreteDistribution(std::vector log_prob) -// { -// uniform_sampler_.seed(RANDOM_SEED); +template std::vector > +DepthImage2CartVectors(const std::vector& image, + const size_t& n_rows, const size_t& n_cols, + const Eigen::Matrix& camera_matrix) +{ + Eigen::Matrix camera_matrix_inverse = camera_matrix.inverse(); + + std::vector > vectors(n_rows*n_cols); + for(size_t row = 0; row < n_rows; row++) + for(size_t col = 0; col < n_cols; col++) + vectors[row*n_cols + col] = + ImageIndex2CartCoord( + Eigen::Vector2i(row, col), + image[row*n_cols + col], + camera_matrix_inverse); + + return vectors; +} -// // substract max to avoid numerical issues -// double max = hf::bound_value(log_prob, true); -// for(int i = 0; i < int(log_prob.size()); i++) -// log_prob[i] -= max; +template std::vector +CartVectors2DepthImage( + const std::vector >& vectors, + const int& n_rows, const int& n_cols, + const Eigen::Matrix& camera_matrix) +{ + std::vector image(n_rows*n_cols, NAN); -// // compute probabilities -// std::vector prob(log_prob.size()); -// double sum = 0; -// for(int i = 0; i < int(log_prob.size()); i++) -// { -// prob[i] = std::exp(log_prob[i]); -// sum += prob[i]; -// } -// for(int i = 0; i < int(prob.size()); i++) -// prob[i] /= sum; + for(size_t i = 0; i < vectors.size(); i++) + { + if( isnan(vectors[i](0)) || isnan(vectors[i](1)) || isnan(vectors[i](2)) ) + continue; -// // compute the cumulative probability -// cumulative_prob_.resize(prob.size()); -// cumulative_prob_[0] = prob[0]; -// for(size_t i = 1; i < prob.size(); i++) -// cumulative_prob_[i] = cumulative_prob_[i-1] + prob[i]; -// } + Eigen::Matrix index = CartCoord2ImageIndex(vectors[i], camera_matrix); -// ~DiscreteDistribution() {} + if(index(0) < 0 || index(0) >= n_rows || index(1) < 0 || index(1) >= n_cols) + { + std::cout << "ERROR: in CartVectors2DepthImage vector comes to lie outside of image " << std::endl; + exit(-1); + } -// int Sample() -// { -// return MapStandardUniform(uniform_sampler_()); -// } + image[index(0)*n_cols + index(1)] = vectors[i](2); + } -// int MapStandardGaussian(double gaussian_sample) const -// { -// double uniform_sample = -// 0.5 * (1.0 + std::erf(gaussian_sample / std::sqrt(2.0))); -// return MapStandardUniform(uniform_sample); -// } + return image; +} -// int MapStandardUniform(double uniform_sample) const -// { -// std::vector::const_iterator iterator = -// std::lower_bound(cumulative_prob_.begin(), -// cumulative_prob_.end(), -// uniform_sample); -// return iterator - cumulative_prob_.begin(); -// } -//private: -// boost::lagged_fibonacci607 uniform_sampler_; -// std::vector cumulative_prob_; -//}; +// numerically stable implementation of log(sum(exp(xi))) >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> +inline double log_sum_exp(double a, double b) +{ + if(a>b) return a + log(1+exp(b-a)); + else return b + log(1+exp(a-b)); +} +inline double log_sum_exp(double a, double b, double c) +{ + if(a>b && a>c) return a + log(1+exp(b-a)+exp(c-a)); + if(b>c && b>a) return b + log(1+exp(a-b)+exp(c-b)); + else return c + log(1+exp(a-c)+exp(b-c)); +} +inline double log_sum_exp(double a, double b, double c, double d) +{ + if(a>b && a>c && a>d) return a + log(1+exp(b-a)+exp(c-a)+exp(d-a)); + if(b>a && b>c && b>d) return b + log(1+exp(a-b)+exp(c-b)+exp(d-b)); + if(c>a && c>b && c>d) return c + log(1+exp(a-c)+exp(b-c)+exp(d-c)); + else return d + log(1+exp(a-d)+exp(b-d)+exp(c-d)); +} +template T log_sum_exp(std::vector exponents) +{ + T max_exponent = -std::numeric_limits::max(); + for(size_t i = 0; i < exponents.size(); i++) + if(exponents[i] > max_exponent) max_exponent = exponents[i]; + for(size_t i = 0; i < exponents.size(); i++) + exponents[i] -= max_exponent; + T sum = 0; + for(size_t i = 0; i < exponents.size(); i++) + sum += exp(exponents[i]); + return max_exponent + log(sum); +} +template class LogSumExp +{ +public: + LogSumExp(){} + ~LogSumExp(){} + + void add_exponent(T exponent) + { + exponents_.push_back(exponent); + } + + T Compute() + { + T max_exponent = -std::numeric_limits::max(); + for(int i = 0; i < int(exponents_.size()); i++) + if(exponents_[i] > max_exponent) max_exponent = exponents_[i]; + + for(int i = 0; i < int(exponents_.size()); i++) + exponents_[i] -= max_exponent; + + T sum = 0; + for(int i = 0; i < int(exponents_.size()); i++) + sum += exp(exponents_[i]); + + return max_exponent + log(sum); + } +private: + std::vector exponents_; +}; + +// sampling class >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> +class DiscreteSampler +{ +public: + template DiscreteSampler(std::vector log_prob) + { + fibo_.seed(RANDOM_SEED); + + // compute the prob and normalize them + sorted_indices_ = dbot::hf::SortDescend(log_prob); + double max = log_prob[sorted_indices_[0]]; + for(int i = 0; i < int(log_prob.size()); i++) + log_prob[i] -= max; + + std::vector prob(log_prob.size()); + double sum = 0; + for(int i = 0; i < int(log_prob.size()); i++) + { + prob[i] = exp(log_prob[i]); + sum += prob[i]; + } + for(int i = 0; i < int(prob.size()); i++) + prob[i] /= sum; + + // compute the cumulative probability + cumulative_prob_.resize(log_prob.size()); + cumulative_prob_[0] = prob[sorted_indices_[0]]; + for(int i = 1; i < int(log_prob.size()); i++) + cumulative_prob_[i] = cumulative_prob_[i-1] + prob[sorted_indices_[i]]; + } + + ~DiscreteSampler() {} + + int Sample() + { + double uniform_sample = fibo_(); + int sample_index = 0; + while(uniform_sample > cumulative_prob_[sample_index]) sample_index++; + + return sorted_indices_[sample_index]; + } + + + + int MapStandardGaussian(double gaussian_sample) const + { + // map from a gaussian to a uniform distribution + double uniform_sample = 0.5 * + (1.0 + std::erf(gaussian_sample / std::sqrt(2.0))); + int sample_index = 0; + while(uniform_sample > cumulative_prob_[sample_index]) sample_index++; //TODO: THIS COULD BE DONE IN LOG TIME + + return sorted_indices_[sample_index]; + } + +private: + boost::lagged_fibonacci607 fibo_; + std::vector sorted_indices_; + std::vector cumulative_prob_; +}; + + + +// the above class should go away +class DiscreteDistribution +{ +public: + template DiscreteDistribution(std::vector log_prob) + { + uniform_sampler_.seed(RANDOM_SEED); + + // substract max to avoid numerical issues + double max = hf::bound_value(log_prob, true); + for(int i = 0; i < int(log_prob.size()); i++) + log_prob[i] -= max; + + // compute probabilities + std::vector prob(log_prob.size()); + double sum = 0; + for(int i = 0; i < int(log_prob.size()); i++) + { + prob[i] = std::exp(log_prob[i]); + sum += prob[i]; + } + for(int i = 0; i < int(prob.size()); i++) + prob[i] /= sum; + + // compute the cumulative probability + cumulative_prob_.resize(prob.size()); + cumulative_prob_[0] = prob[0]; + for(size_t i = 1; i < prob.size(); i++) + cumulative_prob_[i] = cumulative_prob_[i-1] + prob[i]; + } + template class TransformationSequence + { + public: + TransformationSequence( + const Eigen::Matrix &R = Eigen::Matrix::Identity(), + const Eigen::Matrix &t = Eigen::Matrix::Zero()): R_(R), t_(t) {} + + ~TransformationSequence(){} + + void PreRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) + { + t_ = t_ - R_*R*c + R_*c; + R_ = R_*R; + } + void PreTranslate(const Eigen::Matrix &t) + { + t_ = t_ + R_*t; + } + void PostRotate(const Eigen::Matrix &R, const Eigen::Matrix &c = Eigen::Matrix::Zero()) + { + t_ = R*t_ - R*c + c; + R_ = R*R_; + } + void PostTranslate(const Eigen::Matrix &t) + { + t_ = t_ + t; + } + + void get(Eigen::Matrix &R, Eigen::Matrix &t) + { + R = R_; + t = t_; + } + + void NormalizeQuat() + { + Eigen::Quaternion q; q = R_; + q.normalize(); + R_ = q.toRotationMatrix(); + } + + private: + Eigen::Matrix R_; + Eigen::Matrix t_; + + }; + + // depth image functions >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> + // converts from cartesian to image space + template Eigen::Matrix + CartCoord2ImageCoord(const Eigen::Matrix& cart, + const Eigen::Matrix& camera_matrix) + { + return (camera_matrix * cart/cart(2)).topRows(2); + } + + // converts from cartesian space to image index, (row, col) + template Eigen::Matrix + CartCoord2ImageIndex(const Eigen::Matrix& cart, + const Eigen::Matrix& camera_matrix) + { + Eigen::Matrix image = CartCoord2ImageCoord(cart, camera_matrix); + Eigen::Matrix image_index; + image_index(0) = floor(image(1)+0.5); + image_index(1) = floor(image(0)+0.5); + + return image_index; + } + + // converts from image coordinates and depth (z value) to cartesian coordinates + template Eigen::Matrix + ImageCoord2CartCoord(const Eigen::Matrix& image, + const T& depth, + const Eigen::Matrix& camera_matrix_inverse) + { + Eigen::Matrix image_augmented; + image_augmented.topRows(2) = image; + image_augmented(2) = 1; + + return depth * camera_matrix_inverse * image_augmented; + } + + // converts from image index (row, col) and depth (z value) to cartesian coordinates + template Eigen::Matrix + ImageIndex2CartCoord(const Eigen::Matrix& image_index, + const T& depth, + const Eigen::Matrix& camera_matrix_inverse) + { + Eigen::Matrix image; + image(0) = image_index(1); + image(1) = image_index(0); + return ImageCoord2CartCoord(image, depth, camera_matrix_inverse); + } + + + template Eigen::Matrix , -1, -1> + Image2Points(const Eigen::Matrix& image, + const Eigen::Matrix& camera_matrix) + { + Eigen::Matrix camera_matrix_inverse = camera_matrix.inverse(); + + Eigen::Matrix , -1, -1> points(image.rows(), image.cols()); + for(size_t row = 0; row < points.rows(); row++) + for(size_t col = 0; col < points.cols(); col++) + points(row, col) = ImageIndex2CartCoord(Eigen::Vector2i(row, col), + image(row, col), + camera_matrix_inverse); + return points; + } + template std::vector > + DepthImage2CartVectors(const std::vector& image, + const size_t& n_rows, const size_t& n_cols, + const Eigen::Matrix& camera_matrix) + { + Eigen::Matrix camera_matrix_inverse = camera_matrix.inverse(); + std::vector > vectors(n_rows*n_cols); + for(size_t row = 0; row < n_rows; row++) + for(size_t col = 0; col < n_cols; col++) + vectors[row*n_cols + col] = + ImageIndex2CartCoord( + Eigen::Vector2i(row, col), + image[row*n_cols + col], + camera_matrix_inverse); + return vectors; + } + template std::vector + CartVectors2DepthImage( + const std::vector >& vectors, + const int& n_rows, const int& n_cols, + const Eigen::Matrix& camera_matrix) + { + std::vector image(n_rows*n_cols, NAN); + for(size_t i = 0; i < vectors.size(); i++) + { + if( isnan(vectors[i](0)) || isnan(vectors[i](1)) || isnan(vectors[i](2)) ) + continue; + Eigen::Matrix index = CartCoord2ImageIndex(vectors[i], camera_matrix); + if(index(0) < 0 || index(0) >= n_rows || index(1) < 0 || index(1) >= n_cols) + { + std::cout << "ERROR: in CartVectors2DepthImage vector comes to lie outside of image " << std::endl; + exit(-1); + } + image[index(0)*n_cols + index(1)] = vectors[i](2); + } + return image; + } + ~DiscreteDistribution() {} + int Sample() + { + return MapStandardUniform(uniform_sampler_()); + } + int MapStandardGaussian(double gaussian_sample) const + { + double uniform_sample = + 0.5 * (1.0 + std::erf(gaussian_sample / std::sqrt(2.0))); + return MapStandardUniform(uniform_sample); + } + int MapStandardUniform(double uniform_sample) const + { + std::vector::const_iterator iterator = + std::lower_bound(cumulative_prob_.begin(), + cumulative_prob_.end(), + uniform_sample); + return iterator - cumulative_prob_.begin(); + } -//// todo this stuff should be removed >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> -//template void -//Vector2TranslAndQuat(const std::vector &v, Eigen::Matrix &t, Eigen::Quaternion &q) -//{ -// Eigen::Quaternion quat(); -// q.w() = v[0]; q.x() = v[1]; q.y() = v[2]; q.z() = v[3]; -// t(0) = v[4]; t(1) = v[5]; t(2) = v[6]; -//} +private: + boost::lagged_fibonacci607 uniform_sampler_; + std::vector cumulative_prob_; +}; -//template void -//TranslAndQuat2Vector(const Eigen::Quaternion &q, const Eigen::Matrix &t, std::vector &v) -//{ -// v.resize(7); -// v[0] = q.w(); v[1] = q.x(); v[2] = q.y(); v[3] = q.z(); -// v[4] = t(0); v[5] = t(1); v[6] = t(2); -//} -//template void -//Vector2TranslAndRot(const std::vector &v, Eigen::Matrix &R, Eigen::Matrix &t) -//{ -// Eigen::Quaternion q; -// Vector2QuatAndTransl(v, q, t); -// R = q; -//} -//template void -//TranslAndRot2Vector(const Eigen::Matrix &R, const Eigen::Matrix &t, std::vector &v) -//{ -// const Eigen::Quaternion q(R); -// QuatAndTransl2Vector(q, t, v); -//} -//inline Eigen::VectorXd OldState2NewState(const std::vector& old_state) -//{ -// Eigen::VectorXd new_state(old_state.size() * 2 - 1); // the new state also contains the velocity -// new_state(0) = old_state[4]; -// new_state(1) = old_state[5]; -// new_state(2) = old_state[6]; -// new_state(3) = old_state[1]; -// new_state(4) = old_state[2]; -// new_state(5) = old_state[3]; -// new_state(6) = old_state[0]; -//// new_state.topRows(3) += Eigen::Quaterniond(new_state.middleRows<4>(3))._transformVector(center); -// new_state.middleRows(7, 6) = Eigen::VectorXd::Zero(6); -// // we fill in the joint angles -// for(size_t i = 7; i < old_state.size(); i++) -// new_state(i + 6) = old_state[i]; -// // we fill in the joint velocities -// for(size_t i = old_state.size() + 6; i < size_t(new_state.rows()); i++) -// new_state(i) = 0; -// return new_state; -//} -//inline std::vector NewState2OldState(const Eigen::VectorXd& new_state) -//{ -// std::vector old_state((new_state.rows()+1)/2); -// old_state[4] = new_state(0); -// old_state[5] = new_state(1); -// old_state[6] = new_state(2); -// old_state[1] = new_state(3); -// old_state[2] = new_state(4); -// old_state[3] = new_state(5); -// old_state[0] = new_state(6); -// for(size_t i = 7; i < old_state.size(); i++) -// old_state[i] = new_state(i+6); -// return old_state; -//} -//template void -//Vector2QuatAndTransl(const std::vector &v, Eigen::Quaternion &q, Eigen::Matrix &t) -//{ -// q.w() = v[0]; q.x() = v[1]; q.y() = v[2]; q.z() = v[3]; -// t(0) = v[4]; t(1) = v[5]; t(2) = v[6]; -//} -//template void -//QuatAndTransl2Vector(const Eigen::Quaternion &q, const Eigen::Matrix &t, std::vector &v) -//{ -// v.resize(7); -// v[0] = q.w(); v[1] = q.x(); v[2] = q.y(); v[3] = q.z(); -// v[4] = t(0); v[5] = t(1); v[6] = t(2); -//} - -//template void -//Vector2RotAndTransl(const std::vector &v, Eigen::Matrix &R, Eigen::Matrix &t) -//{ -// Eigen::Quaternion q; - -// Vector2QuatAndTransl(v, q, t); -// R = q; -//} - -//template void -//RotAndTransl2Vector(const Eigen::Matrix &R, const Eigen::Matrix &t, std::vector &v) -//{ -// const Eigen::Quaternion q(R); - -// QuatAndTransl2Vector(q, t, v); -//} - -//template void -//Vector2Hom(const std::vector &v, Eigen::Matrix &H) -//{ -// Eigen::Matrix R; -// Eigen::Matrix t; -// Vector2RotAndTransl(v, R, t); - -// H.topLeftCorner(3,3) = R; -// H.topRightCorner(3,1) = t; -// H.row(3) = Eigen::Matrix (0,0,0,1); -//} - -//template void -//Hom2Vector(const Eigen::Matrix &H, std::vector &v) -//{ -// const Eigen::Matrix R(H.topLeftCorner(3,3)); -// const Eigen::Matrix t(H.topRightCorner(3,1)); - -// RotAndTransl2Vector(R, t, v); -//} - -//template void -//Vector2Affine(const std::vector &v, Eigen::Transform &A) -//{ -// Eigen::Matrix H; -// Vector2Hom(v, H); -// A = H; -//} - -//template void -//Affine2Vector(const Eigen::Transform &A, std::vector &v) -//{ -// Eigen::Matrix H; -// H = A.matrix(); -// Hom2Vector(H, v); -//} + + + + + + +// todo this stuff should be removed >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> +template void +Vector2TranslAndQuat(const std::vector &v, Eigen::Matrix &t, Eigen::Quaternion &q) +{ + Eigen::Quaternion quat(); + q.w() = v[0]; q.x() = v[1]; q.y() = v[2]; q.z() = v[3]; + t(0) = v[4]; t(1) = v[5]; t(2) = v[6]; +} + +template void +TranslAndQuat2Vector(const Eigen::Quaternion &q, const Eigen::Matrix &t, std::vector &v) +{ + v.resize(7); + + v[0] = q.w(); v[1] = q.x(); v[2] = q.y(); v[3] = q.z(); + v[4] = t(0); v[5] = t(1); v[6] = t(2); +} + +template void +Vector2TranslAndRot(const std::vector &v, Eigen::Matrix &R, Eigen::Matrix &t) +{ + Eigen::Quaternion q; + + Vector2QuatAndTransl(v, q, t); + R = q; +} + +template void +TranslAndRot2Vector(const Eigen::Matrix &R, const Eigen::Matrix &t, std::vector &v) +{ + const Eigen::Quaternion q(R); + + QuatAndTransl2Vector(q, t, v); +} + +inline Eigen::VectorXd OldState2NewState(const std::vector& old_state) +{ + Eigen::VectorXd new_state(old_state.size() * 2 - 1); // the new state also contains the velocity + new_state(0) = old_state[4]; + new_state(1) = old_state[5]; + new_state(2) = old_state[6]; + + new_state(3) = old_state[1]; + new_state(4) = old_state[2]; + new_state(5) = old_state[3]; + new_state(6) = old_state[0]; + +// new_state.topRows(3) += Eigen::Quaterniond(new_state.middleRows<4>(3))._transformVector(center); + new_state.middleRows(7, 6) = Eigen::VectorXd::Zero(6); + + // we fill in the joint angles + for(size_t i = 7; i < old_state.size(); i++) + new_state(i + 6) = old_state[i]; + // we fill in the joint velocities + for(size_t i = old_state.size() + 6; i < size_t(new_state.rows()); i++) + new_state(i) = 0; + + return new_state; +} + +inline std::vector NewState2OldState(const Eigen::VectorXd& new_state) +{ + std::vector old_state((new_state.rows()+1)/2); + + old_state[4] = new_state(0); + old_state[5] = new_state(1); + old_state[6] = new_state(2); + + old_state[1] = new_state(3); + old_state[2] = new_state(4); + old_state[3] = new_state(5); + old_state[0] = new_state(6); + + for(size_t i = 7; i < old_state.size(); i++) + old_state[i] = new_state(i+6); + + return old_state; +} + +template void +Vector2QuatAndTransl(const std::vector &v, Eigen::Quaternion &q, Eigen::Matrix &t) +{ + q.w() = v[0]; q.x() = v[1]; q.y() = v[2]; q.z() = v[3]; + t(0) = v[4]; t(1) = v[5]; t(2) = v[6]; +} + +template void +QuatAndTransl2Vector(const Eigen::Quaternion &q, const Eigen::Matrix &t, std::vector &v) +{ + v.resize(7); + + v[0] = q.w(); v[1] = q.x(); v[2] = q.y(); v[3] = q.z(); + v[4] = t(0); v[5] = t(1); v[6] = t(2); +} + +template void +Vector2RotAndTransl(const std::vector &v, Eigen::Matrix &R, Eigen::Matrix &t) +{ + Eigen::Quaternion q; + + Vector2QuatAndTransl(v, q, t); + R = q; +} + +template void +RotAndTransl2Vector(const Eigen::Matrix &R, const Eigen::Matrix &t, std::vector &v) +{ + const Eigen::Quaternion q(R); + + QuatAndTransl2Vector(q, t, v); +} + +template void +Vector2Hom(const std::vector &v, Eigen::Matrix &H) +{ + Eigen::Matrix R; + Eigen::Matrix t; + Vector2RotAndTransl(v, R, t); + + H.topLeftCorner(3,3) = R; + H.topRightCorner(3,1) = t; + H.row(3) = Eigen::Matrix (0,0,0,1); +} + +template void +Hom2Vector(const Eigen::Matrix &H, std::vector &v) +{ + const Eigen::Matrix R(H.topLeftCorner(3,3)); + const Eigen::Matrix t(H.topRightCorner(3,1)); + + RotAndTransl2Vector(R, t, v); +} + +template void +Vector2Affine(const std::vector &v, Eigen::Transform &A) +{ + Eigen::Matrix H; + Vector2Hom(v, H); + A = H; +} + +template void +Affine2Vector(const Eigen::Transform &A, std::vector &v) +{ + Eigen::Matrix H; + H = A.matrix(); + Hom2Vector(H, v); +} } diff --git a/include/dbot/utils/rigid_body_renderer.hpp b/include/dbot/utils/rigid_body_renderer.hpp index 7250288..16ad2ca 100644 --- a/include/dbot/utils/rigid_body_renderer.hpp +++ b/include/dbot/utils/rigid_body_renderer.hpp @@ -91,6 +91,10 @@ class RigidBodyRenderer void init(); protected: + Matrix camera_matrix_; + int n_rows_; + int n_cols_; + // triangles std::vector > vertices_; std::vector > normals_; @@ -104,9 +108,6 @@ class RigidBodyRenderer std::vector coms_; std::vector com_weights_; - Matrix camera_matrix_; - int n_rows_; - int n_cols_; }; } From bb4d1debb4df2ba6e4d84a94c880640882238128 Mon Sep 17 00:00:00 2001 From: issac Date: Sat, 3 Oct 2015 23:43:14 +0200 Subject: [PATCH 071/131] updated osr related content --- .../kinect_image_observation_model_cpu.hpp | 4 ++-- .../kinect_image_observation_model_gpu.hpp | 4 ++-- .../observation_models/rao_blackwell_observation_model.hpp | 6 +++--- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp index 2e21620..f2f3520 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_cpu.hpp @@ -32,7 +32,7 @@ along with this program. If not, see . #include #include -#include +#include #include #include #include @@ -151,7 +151,7 @@ class KinectImageObservationModelCPU: auto pose_0 = this->default_poses_.component(i_obj); auto delta = deltas[i_state].component(i_obj); - fl::PoseVector pose; + osr::PoseVector pose; pose.orientation() = delta.orientation() * pose_0.orientation(); pose.position() = delta.position() + pose_0.position(); diff --git a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp index b810a0a..5dd3a15 100644 --- a/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp +++ b/include/dbot/models/observation_models/kinect_image_observation_model_gpu/kinect_image_observation_model_gpu.hpp @@ -6,7 +6,7 @@ #include "boost/filesystem.hpp" #include "Eigen/Core" -#include +#include #include #include @@ -274,7 +274,7 @@ class KinectImageObservationModelGPU: auto pose_0 = this->default_poses_.component(i_obj); auto delta = deltas[i_state].component(i_obj); - fl::PoseVector pose; + osr::PoseVector pose; pose.orientation() = delta.orientation() * pose_0.orientation(); pose.position() = delta.position() + pose_0.position(); diff --git a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp index 8e46ecf..bdf211d 100644 --- a/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp +++ b/include/dbot/models/observation_models/rao_blackwell_observation_model.hpp @@ -33,8 +33,8 @@ along with this program. If not, see . #include #include -#include -#include +#include +#include //#include @@ -58,7 +58,7 @@ class RBObservationModel typedef Eigen::Array IntArray; typedef Eigen::Matrix RealVector; - typedef fl::ComposedVector, RealVector> PoseArray; + typedef osr::ComposedVector, RealVector> PoseArray; From a0ad5555c6be3c29b5fdaba9a329e56c023f8cd8 Mon Sep 17 00:00:00 2001 From: issac Date: Mon, 5 Oct 2015 11:46:30 +0200 Subject: [PATCH 072/131] completed catkin form of dbot cmake file --- CMakeLists.txt | 133 +++++++++++++++++++++++-------------------------- 1 file changed, 61 insertions(+), 72 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a2adfe..7f912ae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(dbot) ############################ # Options # ############################ -option(DBOT_ON_GPU "Compile CUDA enabled trackers" ON) +option(BUILD_DBOT_ON_GPU "Compile CUDA enabled trackers" ON) ############################ # Flags # @@ -23,23 +23,26 @@ add_definitions(-Wno-deprecated-register) ############################ # Setup # ############################ -set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) set(CMAKE_EXPORT_COMPILE_COMMANDS 1) +set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) + +set(dbot_INCLUDE_DIRS "include") +set(dbot_LIBRARY ${PROJECT_NAME}) +set(dbot_LIBRARY_GPU ${dbot_LIBRARY}_gpu) +set(dbot_LIBRARIES ${dbot_LIBRARY}) ############################ # Dependencies ## ############################ find_package(Eigen REQUIRED) -include_directories(${Eigen_INCLUDE_DIRS}) -add_definitions(${Eigen_DEFINITIONS}) - find_package(Boost REQUIRED) + +include_directories(${Eigen_INCLUDE_DIRS}) include_directories(${Boost_INCLUDE_DIRS}) -############################ -# GPU # -############################ -if(DBOT_ON_GPU) +add_definitions(${Eigen_DEFINITIONS}) + +if(BUILD_DBOT_ON_GPU) set(GLEW_DIR ${CMAKE_MODULE_PATH}) find_package(CUDA REQUIRED) @@ -62,84 +65,70 @@ if(DBOT_ON_GPU) set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} -O2 -arch=sm_20) add_definitions( -DBUILD_GPU=1 ) -endif(DBOT_ON_GPU) + + # add GLEW, OpenGL and CUDA paths + list(APPEND dbot_INCLUDE_DIRS ${GLEW_INCLUDE_DIRS}) + list(APPEND dbot_INCLUDE_DIRS ${OpenGL_INCLUDE_DIRS}) + list(APPEND dbot_INCLUDE_DIRS ${CUDA_CUT_INCLUDE_DIRS}) + list(APPEND dbot_INCLUDE_DIRS ${CUDA_INCLUDE_DIRS}) + + # add gpu sub library + list(APPEND dbot_LIBRARIES ${PROJECT_NAME}_gpu) +endif(BUILD_DBOT_ON_GPU) ############################ ## catkin # ############################ find_package(catkin REQUIRED fl osr_catkin) -if(DBOT_ON_GPU) - catkin_package( - INCLUDE_DIRS - include - ${GLEW_INCLUDE_DIRS} - ${OpenGL_INCLUDE_DIRS} - ${CUDA_CUT_INCLUDE_DIRS} - ${CUDA_INCLUDE_DIRS} - LIBRARIES - dbot_gpu - dbot - CATKIN_DEPENDS - fl - osr_catkin - DEPENDS - eigen - ) -else(DBOT_ON_GPU) - catkin_package( - INCLUDE_DIRS - include - LIBRARIES - dbot - CATKIN_DEPENDS - fl - osr_catkin - DEPENDS - eigen - ) -endif(DBOT_ON_GPU) - -include_directories(include ${catkin_INCLUDE_DIRS}) +catkin_package( + INCLUDE_DIRS + ${dbot_INCLUDE_DIRS} + LIBRARIES + ${dbot_LIBRARIES} + CATKIN_DEPENDS + fl + osr_catkin + DEPENDS + Eigen +) ############################ ## dbot library # ############################ -file(GLOB_RECURSE dbot_headers include/${PROJECT_NAME}/*.hpp - include/${PROJECT_NAME}/*.h) -set(dbot_sources src/${PROJECT_NAME}/utils/rigid_body_renderer.cpp) -add_library(${PROJECT_NAME} ${dbot_headers} ${dbot_sources}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - -if(DBOT_ON_GPU) - set(GPU_IMPL_PATH - src/${PROJECT_NAME}/models/observation_models/kinect_image_observation_model_gpu) - - cuda_add_library(${PROJECT_NAME}_gpu - ${GPU_IMPL_PATH}/cuda_filter.cu - ${GPU_IMPL_PATH}/object_rasterizer.cpp - ${GPU_IMPL_PATH}/shader.cpp) - - target_link_libraries(${PROJECT_NAME}_gpu - ${catkin_LIBRARIES} - ${OPENGL_LIBRARIES} - ${GLFW_LIBRARY} - ${GLEW_LIBRARIES}) - - target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_gpu) -endif(DBOT_ON_GPU) - - - - - - - +include_directories(${dbot_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +set(dbot_SOURCE_DIR src/${PROJECT_NAME}) +set(dbot_HEADER_DIR include/${PROJECT_NAME}) +file(GLOB_RECURSE dbot_HEADERS + ${dbot_HEADER_DIR}/*.hpp + ${dbot_HEADER_DIR}/*.h) +# Build dbot library +set(dbot_SOURCES + ${dbot_SOURCE_DIR}/utils/rigid_body_renderer.cpp) +add_library(${dbot_LIBRARY} + ${dbot_HEADERS} + ${dbot_SOURCES}) +target_link_libraries(${dbot_LIBRARY} + ${catkin_LIBRARIES}) +# Build dbot GPU library +if(BUILD_DBOT_ON_GPU) + set(dbot_SOURCE_DIR_GPU + ${dbot_SOURCE_DIR}/models/observation_models/kinect_image_observation_model_gpu) + cuda_add_library(${dbot_LIBRARY_GPU} + ${dbot_SOURCE_DIR_GPU}/cuda_filter.cu + ${dbot_SOURCE_DIR_GPU}/object_rasterizer.cpp + ${dbot_SOURCE_DIR_GPU}/shader.cpp) + target_link_libraries(${dbot_LIBRARY_GPU} + ${catkin_LIBRARIES} + ${OPENGL_LIBRARIES} + ${GLFW_LIBRARY} + ${GLEW_LIBRARIES}) +endif(BUILD_DBOT_ON_GPU) From 16d15ecfdb5badbf0c7ce64adbf45f23bbd340d1 Mon Sep 17 00:00:00 2001 From: cassinaj Date: Tue, 6 Oct 2015 16:05:54 +0200 Subject: [PATCH 073/131] Cmake updates --- CMakeLists.txt | 41 ++++++++++++++++++++++++++--------------- 1 file changed, 26 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7f912ae..8ebbb48 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,10 +26,11 @@ add_definitions(-Wno-deprecated-register) set(CMAKE_EXPORT_COMPILE_COMMANDS 1) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) -set(dbot_INCLUDE_DIRS "include") set(dbot_LIBRARY ${PROJECT_NAME}) set(dbot_LIBRARY_GPU ${dbot_LIBRARY}_gpu) -set(dbot_LIBRARIES ${dbot_LIBRARY}) + +set(dbot_INCLUDE_DIRS ../dbot/include PARENT_SCOPE) +set(dbot_LIBRARIES ${dbot_LIBRARY} PARENT_SCOPE) ############################ # Dependencies ## @@ -79,19 +80,29 @@ endif(BUILD_DBOT_ON_GPU) ############################ ## catkin # ############################ -find_package(catkin REQUIRED fl osr_catkin) - -catkin_package( - INCLUDE_DIRS - ${dbot_INCLUDE_DIRS} - LIBRARIES - ${dbot_LIBRARIES} - CATKIN_DEPENDS - fl - osr_catkin - DEPENDS - Eigen -) + +find_package(fl REQUIRED) +find_package(osr_catkin REQUIRED) + +include_directories(${fl_INCLUDE_DIRS}) +include_directories(${osr_catkin_INCLUDE_DIRS}) + +list(APPEND dbot_INCLUDE_DIRS ${fl_INCLUDE_DIRS}) +list(APPEND dbot_INCLUDE_DIRS ${osr_catkin_INCLUDE_DIRS}) + +#find_package(catkin REQUIRED fl osr_catkin) + +#catkin_package( +# INCLUDE_DIRS +# ${dbot_INCLUDE_DIRS} +# LIBRARIES +# ${dbot_LIBRARIES} +# CATKIN_DEPENDS +# fl +# osr_catkin +# DEPENDS +# Eigen +#) ############################ ## dbot library # From aa46a7e50715bacf4372531f156d491dacd74647 Mon Sep 17 00:00:00 2001 From: issac Date: Tue, 6 Oct 2015 16:29:02 +0200 Subject: [PATCH 074/131] Added orientation_transition_function --- .../orientation_transition_function.hpp | 172 ++++++++++++++++++ 1 file changed, 172 insertions(+) create mode 100644 include/dbot/models/process_models/orientation_transition_function.hpp diff --git a/include/dbot/models/process_models/orientation_transition_function.hpp b/include/dbot/models/process_models/orientation_transition_function.hpp new file mode 100644 index 0000000..a9ef520 --- /dev/null +++ b/include/dbot/models/process_models/orientation_transition_function.hpp @@ -0,0 +1,172 @@ +/* + * This is part of the FL library, a C++ Bayesian filtering library + * (https://github.com/filtering-library) + * + * Copyright (c) 2014 Jan Issac (jan.issac@gmail.com) + * Copyright (c) 2014 Manuel Wuthrich (manuel.wuthrich@gmail.com) + * + * Max-Planck Institute for Intelligent Systems, AMD Lab + * University of Southern California, CLMC Lab + * + * This Source Code Form is subject to the terms of the MIT License (MIT). + * A copy of the license can be found in the LICENSE file distributed with this + * source code. + */ + +/** + * \file orientation_state_transition_model.hpp + * \date July 2015 + * \author Manuel Wuthrich (manuel.wuthrich@gmail.com) + */ + +#pragma once + + +#include + +#include +#include + +#include +#include + +namespace osr +{ + + +class OSTFTypes +{ +public: + typedef Eigen::Matrix State; + typedef Eigen::Matrix Noise; + typedef Eigen::Matrix Input; + + typedef Eigen::Matrix Orientation; + typedef Eigen::Matrix Delta; +}; + + + +class OrientationStateTransitionFunction: + public fl::StateTransitionFunction +{ +public: + typedef OSTFTypes::State State; + typedef OSTFTypes::Noise Noise; + typedef OSTFTypes::Input Input; + typedef OSTFTypes::Orientation Orientation; + typedef OSTFTypes::Delta Delta; + + typedef fl::LinearStateTransitionModel DeltaModel; + + typedef typename DeltaModel::DynamicsMatrix DynamicsMatrix; + typedef typename DeltaModel::DynamicsMatrix NoiseMatrix; + typedef typename DeltaModel::InputMatrix InputMatrix; + +public: + OrientationStateTransitionFunction() { } + + virtual ~OrientationStateTransitionFunction() noexcept { } + +public: + // state format: (euler_vector, angular_velocity) + virtual State state(const State& prev_state, + const Noise& noise, + const Input& input) const + { + EulerVector prev_orientation = prev_state.topRows(3); + Delta prev_delta = prev_state.bottomRows(3); + + EulerVector orientation = EulerVector(prev_delta) * prev_orientation; + Delta delta = delta_model_.state(prev_delta, noise, input); + + State next_state; next_state << orientation, delta; + return next_state; + } + + + + + /// factory functions ****************************************************** + virtual InputMatrix create_input_matrix() const + { + return delta_model_.create_input_matrix(); + } + + virtual DynamicsMatrix create_dynamics_matrix() const + { + return delta_model_.create_dynamics_matrix(); + } + + virtual NoiseMatrix create_noise_matrix() const + { + return delta_model_.create_noise_matrix(); + } + + /// accessors ************************************************************** + virtual const DynamicsMatrix& dynamics_matrix() const + { + return delta_model_.dynamics_matrix(); + } + + virtual const InputMatrix& input_matrix() const + { + return delta_model_.input_matrix(); + } + + virtual const NoiseMatrix& noise_matrix() const + { + return delta_model_.noise_matrix(); + } + + virtual const NoiseMatrix& noise_covariance() const + { + return delta_model_.noise_covariance(); + } + + virtual int state_dimension() const + { + return fl::DimensionOf::Value; + } + + virtual int noise_dimension() const + { + return fl::DimensionOf::Value; + } + + virtual int input_dimension() const + { + return fl::DimensionOf::Value; + } + + /// mutators *************************************************************** + virtual void dynamics_matrix(const DynamicsMatrix& dynamics_mat) + { + delta_model_.dynamics_matrix(dynamics_mat); + } + + virtual void input_matrix(const InputMatrix& input_mat) + { + delta_model_.input_matrix(input_mat); + } + + virtual void noise_matrix(const NoiseMatrix& noise_mat) + { + delta_model_.noise_matrix(noise_mat); + } + + virtual void noise_covariance(const NoiseMatrix& noise_mat_squared) + { + delta_model_.noise_covariance(noise_mat_squared); + } + + +protected: + DeltaModel delta_model_; +}; + +} + + From a1c93ebe105a8649ca15c6393e7e4ebf6a524313 Mon Sep 17 00:00:00 2001 From: issac Date: Tue, 6 Oct 2015 17:36:19 +0200 Subject: [PATCH 075/131] re-added catkin for now --- CMakeLists.txt | 41 +++++++++++++++-------------------------- 1 file changed, 15 insertions(+), 26 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ebbb48..7f912ae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,11 +26,10 @@ add_definitions(-Wno-deprecated-register) set(CMAKE_EXPORT_COMPILE_COMMANDS 1) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) +set(dbot_INCLUDE_DIRS "include") set(dbot_LIBRARY ${PROJECT_NAME}) set(dbot_LIBRARY_GPU ${dbot_LIBRARY}_gpu) - -set(dbot_INCLUDE_DIRS ../dbot/include PARENT_SCOPE) -set(dbot_LIBRARIES ${dbot_LIBRARY} PARENT_SCOPE) +set(dbot_LIBRARIES ${dbot_LIBRARY}) ############################ # Dependencies ## @@ -80,29 +79,19 @@ endif(BUILD_DBOT_ON_GPU) ############################ ## catkin # ############################ - -find_package(fl REQUIRED) -find_package(osr_catkin REQUIRED) - -include_directories(${fl_INCLUDE_DIRS}) -include_directories(${osr_catkin_INCLUDE_DIRS}) - -list(APPEND dbot_INCLUDE_DIRS ${fl_INCLUDE_DIRS}) -list(APPEND dbot_INCLUDE_DIRS ${osr_catkin_INCLUDE_DIRS}) - -#find_package(catkin REQUIRED fl osr_catkin) - -#catkin_package( -# INCLUDE_DIRS -# ${dbot_INCLUDE_DIRS} -# LIBRARIES -# ${dbot_LIBRARIES} -# CATKIN_DEPENDS -# fl -# osr_catkin -# DEPENDS -# Eigen -#) +find_package(catkin REQUIRED fl osr_catkin) + +catkin_package( + INCLUDE_DIRS + ${dbot_INCLUDE_DIRS} + LIBRARIES + ${dbot_LIBRARIES} + CATKIN_DEPENDS + fl + osr_catkin + DEPENDS + Eigen +) ############################ ## dbot library # From 8f55701d7f18f4a8290ded602fd9528c46ebd28c Mon Sep 17 00:00:00 2001 From: issac Date: Tue, 6 Oct 2015 19:35:18 +0200 Subject: [PATCH 076/131] fixed fl::Real --- .../process_models/orientation_transition_function.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/include/dbot/models/process_models/orientation_transition_function.hpp b/include/dbot/models/process_models/orientation_transition_function.hpp index a9ef520..2fa4118 100644 --- a/include/dbot/models/process_models/orientation_transition_function.hpp +++ b/include/dbot/models/process_models/orientation_transition_function.hpp @@ -37,12 +37,12 @@ namespace osr class OSTFTypes { public: - typedef Eigen::Matrix State; - typedef Eigen::Matrix Noise; - typedef Eigen::Matrix Input; + typedef Eigen::Matrix State; + typedef Eigen::Matrix Noise; + typedef Eigen::Matrix Input; - typedef Eigen::Matrix Orientation; - typedef Eigen::Matrix Delta; + typedef Eigen::Matrix Orientation; + typedef Eigen::Matrix Delta; }; From e779855d2e556cfb20db48afb46ac8a7ffac9ac7 Mon Sep 17 00:00:00 2001 From: issac Date: Wed, 7 Oct 2015 15:23:50 +0200 Subject: [PATCH 077/131] removed osr_catkin; added find osr as a cmake package --- CMakeLists.txt | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7f912ae..9f780ed 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,11 +32,16 @@ set(dbot_LIBRARY_GPU ${dbot_LIBRARY}_gpu) set(dbot_LIBRARIES ${dbot_LIBRARY}) ############################ -# Dependencies ## +# Dependencies # ############################ +if(NOT osr_FOUND) + find_package(osr REQUIRED) +endif(NOT osr_FOUND) + find_package(Eigen REQUIRED) find_package(Boost REQUIRED) +include_directories(${osr_INCLUDE_DIRS}) include_directories(${Eigen_INCLUDE_DIRS}) include_directories(${Boost_INCLUDE_DIRS}) @@ -79,7 +84,7 @@ endif(BUILD_DBOT_ON_GPU) ############################ ## catkin # ############################ -find_package(catkin REQUIRED fl osr_catkin) +find_package(catkin REQUIRED fl) catkin_package( INCLUDE_DIRS @@ -88,7 +93,6 @@ catkin_package( ${dbot_LIBRARIES} CATKIN_DEPENDS fl - osr_catkin DEPENDS Eigen ) From 377d6d6113dca705d785168b701cfc1f842318f0 Mon Sep 17 00:00:00 2001 From: issac Date: Wed, 7 Oct 2015 20:27:50 +0200 Subject: [PATCH 078/131] dbot is no longer a catkin package. just a plain cmake package --- CMakeLists.txt | 31 ++++++++++++++----------------- package.xml | 5 ----- 2 files changed, 14 insertions(+), 22 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9f780ed..1150c04 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,11 +29,23 @@ set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) set(dbot_INCLUDE_DIRS "include") set(dbot_LIBRARY ${PROJECT_NAME}) set(dbot_LIBRARY_GPU ${dbot_LIBRARY}_gpu) -set(dbot_LIBRARIES ${dbot_LIBRARY}) + +############################ +# Exports # +############################ +set(dbot_FOUND 1 PARENT_SCOPE) +set(dbot_INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/include PARENT_SCOPE) +set(dbot_LIBRARIES ${dbot_LIBRARY} PARENT_SCOPE) +set(dbot_DEFINITIONS ${Eigen_DEFINITIONS} PARENT_SCOPE) +set(dbot_DEPENDS Eigen PARENT_SCOPE) ############################ # Dependencies # ############################ +if(NOT fl_FOUND) + find_package(fl REQUIRED) +endif(NOT fl_FOUND) + if(NOT osr_FOUND) find_package(osr REQUIRED) endif(NOT osr_FOUND) @@ -41,6 +53,7 @@ endif(NOT osr_FOUND) find_package(Eigen REQUIRED) find_package(Boost REQUIRED) +include_directories(${fl_INCLUDE_DIRS}) include_directories(${osr_INCLUDE_DIRS}) include_directories(${Eigen_INCLUDE_DIRS}) include_directories(${Boost_INCLUDE_DIRS}) @@ -81,22 +94,6 @@ if(BUILD_DBOT_ON_GPU) list(APPEND dbot_LIBRARIES ${PROJECT_NAME}_gpu) endif(BUILD_DBOT_ON_GPU) -############################ -## catkin # -############################ -find_package(catkin REQUIRED fl) - -catkin_package( - INCLUDE_DIRS - ${dbot_INCLUDE_DIRS} - LIBRARIES - ${dbot_LIBRARIES} - CATKIN_DEPENDS - fl - DEPENDS - Eigen -) - ############################ ## dbot library # ############################ diff --git a/package.xml b/package.xml index 1256ab9..be551f1 100644 --- a/package.xml +++ b/package.xml @@ -15,13 +15,8 @@ catkin - eigen fl - osr_catkin - - eigen fl - osr_catkin From f43ea377b7ea69235629c235987722a2c0e84ab9 Mon Sep 17 00:00:00 2001 From: issac Date: Fri, 9 Oct 2015 19:30:02 +0200 Subject: [PATCH 079/131] Removed opencv2/highgui/highgui.hpp --- include/dbot/utils/rigid_body_renderer.hpp | 1 - src/dbot/utils/rigid_body_renderer.cpp | 2 -- 2 files changed, 3 deletions(-) diff --git a/include/dbot/utils/rigid_body_renderer.hpp b/include/dbot/utils/rigid_body_renderer.hpp index 16ad2ca..18bcc29 100644 --- a/include/dbot/utils/rigid_body_renderer.hpp +++ b/include/dbot/utils/rigid_body_renderer.hpp @@ -35,7 +35,6 @@ along with this program. If not, see . #include #include - namespace dbot { diff --git a/src/dbot/utils/rigid_body_renderer.cpp b/src/dbot/utils/rigid_body_renderer.cpp index b16806a..9aa341c 100644 --- a/src/dbot/utils/rigid_body_renderer.cpp +++ b/src/dbot/utils/rigid_body_renderer.cpp @@ -31,8 +31,6 @@ along with this program. If not, see . #include -#include - using namespace std; using namespace Eigen; From 11cdc38e8b4850fac5a2f38ea7ed536debf18d97 Mon Sep 17 00:00:00 2001 From: issac Date: Fri, 9 Oct 2015 19:30:29 +0200 Subject: [PATCH 080/131] Added doc skeleton --- doc/Doxyfile.in | 2353 ++++++++++++++++++++++++++ doc/bibliography.bib | 62 + doc/mainpage.dox | 64 + doc/modules.dox | 40 + doc/theme/customization.css | 133 ++ doc/theme/header.html | 235 +++ doc/theme/layout.xml | 185 ++ doc/theme/navtree_expand_selected.js | 83 + 8 files changed, 3155 insertions(+) create mode 100644 doc/Doxyfile.in create mode 100644 doc/bibliography.bib create mode 100644 doc/mainpage.dox create mode 100644 doc/modules.dox create mode 100644 doc/theme/customization.css create mode 100644 doc/theme/header.html create mode 100644 doc/theme/layout.xml create mode 100644 doc/theme/navtree_expand_selected.js diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in new file mode 100644 index 0000000..66d7a9b --- /dev/null +++ b/doc/Doxyfile.in @@ -0,0 +1,2353 @@ +# Doxyfile 1.8.8 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a double hash (##) is considered a comment and is placed in +# front of the TAG it is preceding. +# +# All text after a single hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists, items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (\" \"). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all text +# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv +# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv +# for the list of possible encodings. +# The default value is: UTF-8. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# double-quotes, unless you are using Doxywizard) that should identify the +# project for which the documentation is generated. This name is used in the +# title of most generated pages and in a few other places. +# The default value is: My Project. + +PROJECT_NAME = "Object State Representation (osr)" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. This +# could be handy for archiving the generated documentation or if some version +# control system is used. + +PROJECT_NUMBER = "@PROJECT_VERSION@" + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer a +# quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = "Tracking object state representation in 3D space" + +# With the PROJECT_LOGO tag one can specify an logo or icon that is included in +# the documentation. The maximum height of the logo should not exceed 55 pixels +# and the maximum width should not exceed 200 pixels. Doxygen will copy the logo +# to the output directory. + +PROJECT_LOGO = @PROJECT_SOURCE_DIR@/doc/images/bayes.jpg + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# into which the generated documentation will be written. If a relative path is +# entered, it will be relative to the location where doxygen was started. If +# left blank the current directory will be used. + +OUTPUT_DIRECTORY = doc/ + +# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 4096 sub- +# directories (in 2 levels) under the output directory of each output format and +# will distribute the generated files over these directories. Enabling this +# option can be useful when feeding doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise causes +# performance problems for the file system. +# The default value is: NO. + +CREATE_SUBDIRS = NO + +# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII +# characters to appear in the names of generated files. If set to NO, non-ASCII +# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode +# U+3044. +# The default value is: NO. + +ALLOW_UNICODE_NAMES = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, +# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), +# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, +# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), +# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, +# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, +# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, +# Ukrainian and Vietnamese. +# The default value is: English. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES doxygen will include brief member +# descriptions after the members that are listed in the file and class +# documentation (similar to Javadoc). Set to NO to disable this. +# The default value is: YES. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES doxygen will prepend the brief +# description of a member or function before the detailed description +# +# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. +# The default value is: YES. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator that is +# used to form the text in various listings. Each string in this list, if found +# as the leading text of the brief description, will be stripped from the text +# and the result, after processing the whole list, is used as the annotated +# text. Otherwise, the brief description is used as-is. If left blank, the +# following values are used ($name is automatically replaced with the name of +# the entity):The $name class, The $name widget, The $name file, is, provides, +# specifies, contains, represents, a, an and the. + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# doxygen will generate a detailed section even if there is only a brief +# description. +# The default value is: NO. + +ALWAYS_DETAILED_SEC = YES + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. +# The default value is: NO. + +INLINE_INHERITED_MEMB = YES + +# If the FULL_PATH_NAMES tag is set to YES doxygen will prepend the full path +# before files name in the file list and in the header files. If set to NO the +# shortest path that makes the file name unique will be used +# The default value is: YES. + +FULL_PATH_NAMES = NO + +# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. +# Stripping is only done if one of the specified strings matches the left-hand +# part of the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the path to +# strip. +# +# Note that you can specify absolute paths here, but also relative paths, which +# will be relative from the directory where doxygen is started. +# This tag requires that the tag FULL_PATH_NAMES is set to YES. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the +# path mentioned in the documentation of a class, which tells the reader which +# header file to include in order to use a class. If left blank only the name of +# the header file containing the class definition is used. Otherwise one should +# specify the list of include paths that are normally passed to the compiler +# using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# less readable) file names. This can be useful is your file systems doesn't +# support long names like on DOS, Mac, or CD-ROM. +# The default value is: NO. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the +# first line (until the first dot) of a Javadoc-style comment as the brief +# description. If set to NO, the Javadoc-style will behave just like regular Qt- +# style comments (thus requiring an explicit @brief command for a brief +# description.) +# The default value is: NO. + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first +# line (until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# requiring an explicit \brief command for a brief description.) +# The default value is: NO. + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# a brief description. This used to be the default behavior. The new default is +# to treat a multi-line C++ comment block as a detailed description. Set this +# tag to YES if you prefer the old behavior instead. +# +# Note that setting this tag to YES also means that rational rose comments are +# not recognized any more. +# The default value is: NO. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the +# documentation from any documented member that it re-implements. +# The default value is: YES. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce a +# new page for each member. If set to NO, the documentation of a member will be +# part of the file/class/namespace that contains it. +# The default value is: NO. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen +# uses this value to replace tabs by spaces in code fragments. +# Minimum value: 1, maximum value: 16, default value: 4. + +TAB_SIZE = 4 + +# This tag can be used to specify a number of aliases that act as commands in +# the documentation. An alias has the form: +# name=value +# For example adding +# "sideeffect=@par Side Effects:\n" +# will allow you to put the command \sideeffect (or @sideeffect) in the +# documentation, which will result in a user-defined paragraph with heading +# "Side Effects:". You can put \n's in the value part of an alias to insert +# newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding "class=itcl::class" +# will allow you to use the command class in the itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources +# only. Doxygen will then generate output that is more tailored for C. For +# instance, some of the names that are used will be different. The list of all +# members will be omitted, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or +# Python sources only. Doxygen will then generate output that is more tailored +# for that language. For instance, namespaces will be presented as packages, +# qualified scopes will look different, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources. Doxygen will then generate output that is tailored for Fortran. +# The default value is: NO. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for VHDL. +# The default value is: NO. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, and +# language is one of the parsers supported by doxygen: IDL, Java, Javascript, +# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran: +# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran: +# Fortran. In the later case the parser tries to guess whether the code is fixed +# or free formatted code, this is the default for Fortran type files), VHDL. For +# instance to make doxygen treat .inc files as Fortran files (default is PHP), +# and .f files as C (default is Fortran), use: inc=Fortran f=C. +# +# Note For files without extension you can use no_extension as a placeholder. +# +# Note that for custom extensions you also need to set FILE_PATTERNS otherwise +# the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# according to the Markdown format, which allows for more readable +# documentation. See http://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you can +# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# case of backward compatibilities issues. +# The default value is: YES. + +MARKDOWN_SUPPORT = YES + +# When enabled doxygen tries to link words that correspond to documented +# classes, or namespaces to their corresponding documentation. Such a link can +# be prevented in individual cases by by putting a % sign in front of the word +# or globally by setting AUTOLINK_SUPPORT to NO. +# The default value is: YES. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should set this +# tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); +# versus func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. +# The default value is: NO. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. +# The default value is: NO. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen +# will parse them like normal C++ but will assume all classes use public instead +# of private inheritance when no explicit protection keyword is present. +# The default value is: NO. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES will make +# doxygen to replace the get and set methods by a property in the documentation. +# This will only work if the methods are indeed getting or setting a simple +# type. If this is not the case, or you want to show the methods anyway, you +# should set this option to NO. +# The default value is: YES. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES, then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. +# The default value is: NO. + +DISTRIBUTE_GROUP_DOC = NO + +# Set the SUBGROUPING tag to YES to allow class member groups of the same type +# (for instance a group of public functions) to be put as a subgroup of that +# type (e.g. under the Public Functions section). Set it to NO to prevent +# subgrouping. Alternatively, this can be done per class using the +# \nosubgrouping command. +# The default value is: YES. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions +# are shown inside the group in which they are included (e.g. using \ingroup) +# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# and RTF). +# +# Note that this feature does not work in combination with +# SEPARATE_MEMBER_PAGES. +# The default value is: NO. + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions +# with only public data fields or simple typedef fields will be shown inline in +# the documentation of the scope in which they are defined (i.e. file, +# namespace, or group documentation), provided this scope is documented. If set +# to NO, structs, classes, and unions are shown on a separate page (for HTML and +# Man pages) or section (for LaTeX and RTF). +# The default value is: NO. + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or +# enum is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically be +# useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. +# The default value is: NO. + +TYPEDEF_HIDES_STRUCT = YES + +# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This +# cache is used to resolve symbols given their name and scope. Since this can be +# an expensive process and often the same symbol appears multiple times in the +# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# doxygen will become slower. If the cache is too large, memory is wasted. The +# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range +# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 +# symbols. At the end of a run doxygen will report the cache usage and suggest +# the optimal cache size from a speed point of view. +# Minimum value: 0, maximum value: 9, default value: 0. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in +# documentation are documented, even if no documentation was available. Private +# class members and static file members will be hidden unless the +# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. +# Note: This will also disable the warnings about undocumented members that are +# normally produced when WARNINGS is set to YES. +# The default value is: NO. + +EXTRACT_ALL = NO + +# If the EXTRACT_PRIVATE tag is set to YES all private members of a class will +# be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIVATE = NO + +# If the EXTRACT_PACKAGE tag is set to YES all members with package or internal +# scope will be included in the documentation. +# The default value is: NO. + +EXTRACT_PACKAGE = NO + +# If the EXTRACT_STATIC tag is set to YES all static members of a file will be +# included in the documentation. +# The default value is: NO. + +EXTRACT_STATIC = NO + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) defined +# locally in source files will be included in the documentation. If set to NO +# only classes defined in header files are included. Does not have any effect +# for Java sources. +# The default value is: YES. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. When set to YES local methods, +# which are defined in the implementation section but not in the interface are +# included in the documentation. If set to NO only methods in the interface are +# included. +# The default value is: NO. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base name of +# the file that contains the anonymous namespace. By default anonymous namespace +# are hidden. +# The default value is: NO. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# undocumented members inside documented classes or files. If set to NO these +# members will be included in the various overviews, but no documentation +# section is generated. This option has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. If set +# to NO these classes will be included in the various overviews. This option has +# no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# (class|struct|union) declarations. If set to NO these declarations will be +# included in the documentation. +# The default value is: NO. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# documentation blocks found inside the body of a function. If set to NO these +# blocks will be appended to the function's detailed documentation block. +# The default value is: NO. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation that is typed after a +# \internal command is included. If the tag is set to NO then the documentation +# will be excluded. Set it to YES to include the internal documentation. +# The default value is: NO. + +INTERNAL_DOCS = NO + +# The ENABLED_SECTIONS tag can be used to enable conditional documentation +# sections, marked by \if ... \endif and \cond +# ... \endcond blocks. + +ENABLED_SECTIONS = # internal + +# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file +# names in lower-case letters. If set to YES upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. +# The default value is: system dependent. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# their full class and namespace scopes in the documentation. If set to YES the +# scope will be hidden. +# The default value is: NO. + +HIDE_SCOPE_NAMES = NO + +# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# the files that are included by a file in the documentation of that file. +# The default value is: YES. + +SHOW_INCLUDE_FILES = YES + +# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each +# grouped member an include statement to the documentation, telling the reader +# which file to include in order to use the member. +# The default value is: NO. + +SHOW_GROUPED_MEMB_INC = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# files with double quotes in the documentation rather than with sharp brackets. +# The default value is: NO. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the +# documentation for inline members. +# The default value is: YES. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# (detailed) documentation of file and class members alphabetically by member +# name. If set to NO the members will appear in declaration order. +# The default value is: YES. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# descriptions of file, namespace and class members alphabetically by member +# name. If set to NO the members will appear in declaration order. Note that +# this will also influence the order of the classes in the class list. +# The default value is: NO. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# (brief and detailed) documentation of class members so that constructors and +# destructors are listed first. If set to NO the constructors will appear in the +# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. +# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief +# member documentation. +# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting +# detailed member documentation. +# The default value is: NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# of group names into alphabetical order. If set to NO the group names will +# appear in their defined order. +# The default value is: NO. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by +# fully-qualified names, including namespaces. If set to NO, the class list will +# be sorted only by class name, not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the alphabetical +# list. +# The default value is: NO. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# type resolution of all parameters of a function it will reject a match between +# the prototype and the implementation of a member function even if there is +# only one candidate or it is obvious which candidate to choose by doing a +# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# accept a match between prototype and implementation in such cases. +# The default value is: NO. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable ( YES) or disable ( NO) the +# todo list. This list is created by putting \todo commands in the +# documentation. +# The default value is: YES. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable ( YES) or disable ( NO) the +# test list. This list is created by putting \test commands in the +# documentation. +# The default value is: YES. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable ( YES) or disable ( NO) the bug +# list. This list is created by putting \bug commands in the documentation. +# The default value is: YES. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable ( YES) or disable ( NO) +# the deprecated list. This list is created by putting \deprecated commands in +# the documentation. +# The default value is: YES. + +GENERATE_DEPRECATEDLIST= YES + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the +# initial value of a variable or macro / define can have for it to appear in the +# documentation. If the initializer consists of more lines than specified here +# it will be hidden. Use a value of 0 to hide initializers completely. The +# appearance of the value of individual variables and macros / defines can be +# controlled using \showinitializer or \hideinitializer command in the +# documentation regardless of this setting. +# Minimum value: 0, maximum value: 10000, default value: 30. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at +# the bottom of the documentation of classes and structs. If set to YES the list +# will mention the files that were used to generate the documentation. +# The default value is: YES. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This +# will remove the Files entry from the Quick Index and from the Folder Tree View +# (if specified). +# The default value is: YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces +# page. This will remove the Namespaces entry from the Quick Index and from the +# Folder Tree View (if specified). +# The default value is: YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command command input-file, where command is the value of the +# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided +# by doxygen. Whatever the program writes to standard output is used as the file +# version. For an example see the documentation. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. You can +# optionally specify a file name after the option, if omitted DoxygenLayout.xml +# will be used as the name of the layout file. +# +# Note that if you run doxygen from a directory containing a file called +# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# tag is left empty. + +LAYOUT_FILE = @PROJECT_SOURCE_DIR@/doc/theme/layout.xml + +# The CITE_BIB_FILES tag can be used to specify one or more bib files containing +# the reference definitions. This must be a list of .bib files. The .bib +# extension is automatically appended if omitted. This requires the bibtex tool +# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info. +# For LaTeX the style of the bibliography can be controlled using +# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the +# search path. See also \cite for info how to create references. + +CITE_BIB_FILES = @PROJECT_SOURCE_DIR@/doc/bibliography.bib + +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated to +# standard output by doxygen. If QUIET is set to YES this implies that the +# messages are off. +# The default value is: NO. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated to standard error ( stderr) by doxygen. If WARNINGS is set to YES +# this implies that the warnings are on. +# +# Tip: Turn warnings on while writing the documentation. +# The default value is: YES. + +WARNINGS = YES + +# If the WARN_IF_UNDOCUMENTED tag is set to YES, then doxygen will generate +# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: YES. + +WARN_IF_UNDOCUMENTED = NO + +# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some parameters +# in a documented function, or documenting parameters that don't exist or using +# markup commands wrongly. +# The default value is: YES. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that +# are documented, but have no documentation for their parameters or return +# value. If set to NO doxygen will only warn about wrong or incomplete parameter +# documentation, but not about the absence of documentation. +# The default value is: NO. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# can produce. The string should contain the $file, $line, and $text tags, which +# will be replaced by the file and line number from which the warning originated +# and the warning text. Optionally the format may contain $version, which will +# be replaced by the version of the file (if it could be obtained via +# FILE_VERSION_FILTER) +# The default value is: $file:$line: $text. + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning and error +# messages should be written. If left blank the output is written to standard +# error (stderr). + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag is used to specify the files and/or directories that contain +# documented source files. You may enter file names like myfile.cpp or +# directories like /usr/src/myproject. Separate the files or directories with +# spaces. +# Note: If this tag is empty the current directory is searched. + +INPUT = @PROJECT_SOURCE_DIR@/doc \ + @PROJECT_SOURCE_DIR@/include/fl \ + @PROJECT_SOURCE_DIR@/include/ff + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# libiconv (or the iconv built into libc) for the transcoding. See the libiconv +# documentation (see: http://www.gnu.org/software/libiconv) for the list of +# possible encodings. +# The default value is: UTF-8. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank the +# following patterns are tested:*.c, *.cc, *.cxx, *.cpp, *.c++, *.java, *.ii, +# *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp, +# *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown, +# *.md, *.mm, *.dox, *.py, *.f90, *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf, +# *.qsf, *.as and *.js. + +FILE_PATTERNS = *.hpp *.h *.cpp *.dox + +# The RECURSIVE tag can be used to specify whether or not subdirectories should +# be searched for input files as well. +# The default value is: NO. + +RECURSIVE = YES + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = @PROJECT_SOURCE_DIR@/test + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. +# The default value is: NO. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories use the pattern */test/* + +EXCLUDE_SYMBOLS = FilterInterface + +# The EXAMPLE_PATH tag can be used to specify one or more files or directories +# that contain example code fragments that are included (see the \include +# command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank all +# files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude commands +# irrespective of the value of the RECURSIVE tag. +# The default value is: NO. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or directories +# that contain images that are to be included in the documentation (see the +# \image command). + +IMAGE_PATH = @PROJECT_SOURCE_DIR@/doc/images + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command: +# +# +# +# where is the value of the INPUT_FILTER tag, and is the +# name of an input file. Doxygen will then use the output that the filter +# program writes to standard output. If FILTER_PATTERNS is specified, this tag +# will be ignored. +# +# Note that the filter must not add or remove lines; it is applied before the +# code is scanned, but not when the output code is generated. If lines are added +# or removed, the anchors will not be placed correctly. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: pattern=filter +# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +# filters are used. If the FILTER_PATTERNS tag is empty or if none of the +# patterns match the file name, INPUT_FILTER is applied. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER ) will also be used to filter the input files that are used for +# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# The default value is: NO. + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# it is also possible to disable source filtering for a specific pattern using +# *.ext= (so without naming a filter). +# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page +# (index.html). This can be useful if you have a project on for instance GitHub +# and want to reuse the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = + +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will be +# generated. Documented entities will be cross-referenced with these sources. +# +# Note: To get rid of all source code in the generated output, make sure that +# also VERBATIM_HEADERS is set to NO. +# The default value is: NO. + +SOURCE_BROWSER = YES + +# Setting the INLINE_SOURCES tag to YES will include the body of functions, +# classes and enums directly into the documentation. +# The default value is: NO. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# special comment blocks from generated source code fragments. Normal C, C++ and +# Fortran comments will always remain visible. +# The default value is: YES. + +STRIP_CODE_COMMENTS = NO + +# If the REFERENCED_BY_RELATION tag is set to YES then for each documented +# function all documented functions referencing it will be listed. +# The default value is: NO. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES then for each documented function +# all documented entities called/used by that function will be listed. +# The default value is: NO. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set +# to YES, then the hyperlinks from functions in REFERENCES_RELATION and +# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will +# link to the documentation. +# The default value is: YES. + +REFERENCES_LINK_SOURCE = YES + +# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# source code will show a tooltip with additional information such as prototype, +# brief description and links to the definition and documentation. Since this +# will make the HTML file larger and loading of large files a bit slower, you +# can opt to disable this feature. +# The default value is: YES. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +SOURCE_TOOLTIPS = YES + +# If the USE_HTAGS tag is set to YES then the references to source code will +# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# source browser. The htags tool is part of GNU's global source tagging system +# (see http://www.gnu.org/software/global/global.html). You will need version +# 4.8.6 or higher. +# +# To use it do the following: +# - Install the latest version of global +# - Enable SOURCE_BROWSER and USE_HTAGS in the config file +# - Make sure the INPUT points to the root of the source tree +# - Run doxygen as normal +# +# Doxygen will invoke htags (and that will in turn invoke gtags), so these +# tools must be available from the command line (i.e. in the search path). +# +# The result: instead of the source browser generated by doxygen, the links to +# source code will now point to the output of htags. +# The default value is: NO. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# verbatim copy of the header file for each class for which an include is +# specified. Set to NO to disable this. +# See also: Section \class. +# The default value is: YES. + +VERBATIM_HEADERS = YES + +# If the CLANG_ASSISTED_PARSING tag is set to YES, then doxygen will use the +# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the +# cost of reduced performance. This can be particularly helpful with template +# rich C++ code for which doxygen's built-in parser lacks the necessary type +# information. +# Note: The availability of this option depends on whether or not doxygen was +# compiled with the --with-libclang option. +# The default value is: NO. + +CLANG_ASSISTED_PARSING = NO + +# If clang assisted parsing is enabled you can provide the compiler with command +# line options that you would normally use when invoking the compiler. Note that +# the include paths will already be set by doxygen for the files and directories +# specified with INPUT and INCLUDE_PATH. +# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES. + +CLANG_OPTIONS = + +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all +# compounds will be generated. Enable this if the project contains a lot of +# classes, structs, unions or interfaces. +# The default value is: YES. + +ALPHABETICAL_INDEX = YES + +# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in +# which the alphabetical index list will be split. +# Minimum value: 1, maximum value: 20, default value: 5. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all classes will +# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag +# can be used to specify a prefix (or a list of prefixes) that should be ignored +# while generating the index headers. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES doxygen will generate HTML output +# The default value is: YES. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each +# generated HTML page (for example: .htm, .php, .asp). +# The default value is: .html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a user-defined HTML header file for +# each generated HTML page. If the tag is left blank doxygen will generate a +# standard header. +# +# To get valid HTML the header file that includes any scripts and style sheets +# that doxygen needs, which is dependent on the configuration options used (e.g. +# the setting GENERATE_TREEVIEW). It is highly recommended to start with a +# default header using +# doxygen -w html new_header.html new_footer.html new_stylesheet.css +# YourConfigFile +# and then modify the file new_header.html. See also section "Doxygen usage" +# for information on how to generate the default header that doxygen normally +# uses. +# Note: The header is subject to change so you typically have to regenerate the +# default header when upgrading to a newer version of doxygen. For a description +# of the possible markers and block names see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_HEADER = "@PROJECT_SOURCE_DIR@/doc/theme/header.html" + +# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each +# generated HTML page. If the tag is left blank doxygen will generate a standard +# footer. See HTML_HEADER for more information on how to generate a default +# footer and what special commands can be used inside the footer. See also +# section "Doxygen usage" for information on how to generate the default footer +# that doxygen normally uses. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style +# sheet that is used by each HTML page. It can be used to fine-tune the look of +# the HTML output. If left blank doxygen will generate a default style sheet. +# See also section "Doxygen usage" for information on how to generate the style +# sheet that doxygen normally uses. +# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as +# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# obsolete. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined +# cascading style sheets that are included after the standard style sheets +# created by doxygen. Using this option one can overrule certain style aspects. +# This is preferred over using HTML_STYLESHEET since it does not replace the +# standard style sheet and is therefor more robust against future updates. +# Doxygen will copy the style sheet files to the output directory. +# Note: The order of the extra stylesheet files is of importance (e.g. the last +# stylesheet in the list overrules the setting of the previous ones in the +# list). For an example see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that the +# files will be copied as-is; there are no commands or markers available. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_FILES = "@PROJECT_SOURCE_DIR@/doc/theme/customization.css" \ + "@PROJECT_SOURCE_DIR@/doc/theme/navtree_expand_selected.js" + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen +# will adjust the colors in the stylesheet and background images according to +# this color. Hue is specified as an angle on a colorwheel, see +# http://en.wikipedia.org/wiki/Hue for more information. For instance the value +# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 +# purple, and 360 is red again. +# Minimum value: 0, maximum value: 359, default value: 220. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# in the HTML output. For a value of 0 the output will use grayscales only. A +# value of 255 will produce the most vivid colors. +# Minimum value: 0, maximum value: 255, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the +# luminance component of the colors in the HTML output. Values below 100 +# gradually make the output lighter, whereas values above 100 make the output +# darker. The value divided by 100 is the actual gamma applied, so 80 represents +# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not +# change the gamma. +# Minimum value: 40, maximum value: 240, default value: 80. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting this +# to NO can help when comparing the output of multiple runs. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_TIMESTAMP = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_SECTIONS = YES + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries +# shown in the various tree structured indices initially; the user can expand +# and collapse entries dynamically later on. Doxygen will expand the tree to +# such a level that at most the specified number of entries are visible (unless +# a fully collapsed tree already exceeds this amount). So setting the number of +# entries 1 will produce a full collapsed tree by default. 0 is a special value +# representing an infinite number of entries and will result in a full expanded +# tree by default. +# Minimum value: 0, maximum value: 9999, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files will be +# generated that can be used as input for Apple's Xcode 3 integrated development +# environment (see: http://developer.apple.com/tools/xcode/), introduced with +# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a +# Makefile in the HTML output directory. Running make will produce the docset in +# that directory and running make install will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at +# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_DOCSET = NO + +# This tag determines the name of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# The default value is: Doxygen generated docs. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# This tag specifies a string that should uniquely identify the documentation +# set bundle. This should be a reverse domain-name style string, e.g. +# com.mycompany.MyDocSet. Doxygen will append .docset to the name. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. +# The default value is: org.doxygen.Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. +# The default value is: Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# additional HTML index files: index.hhp, index.hhc, and index.hhk. The +# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop +# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on +# Windows. +# +# The HTML Help Workshop contains a compiler that can convert all HTML output +# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# files are now used as the Windows 98 help format, and will replace the old +# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# HTML files also contain an index, a table of contents, and you can search for +# words in the documentation. The HTML workshop also contains a viewer for +# compressed HTML files. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_HTMLHELP = NO + +# The CHM_FILE tag can be used to specify the file name of the resulting .chm +# file. You can add a path in front of the file if the result should not be +# written to the html output directory. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_FILE = + +# The HHC_LOCATION tag can be used to specify the location (absolute path +# including file name) of the HTML help compiler ( hhc.exe). If non-empty +# doxygen will try to run the HTML help compiler on the generated index.hhp. +# The file has to be specified with full path. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +HHC_LOCATION = + +# The GENERATE_CHI flag controls if a separate .chi index file is generated ( +# YES) or that it should be included in the master .chm file ( NO). +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +GENERATE_CHI = NO + +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index ( hhk), content ( hhc) +# and project file content. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_INDEX_ENCODING = + +# The BINARY_TOC flag controls whether a binary table of contents is generated ( +# YES) or a normal table of contents ( NO) in the .chm file. Furthermore it +# enables the Previous and Next buttons. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members to +# the table of contents of the HTML help documentation and to the tree view. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that +# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help +# (.qch) of the generated HTML documentation. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify +# the file name of the resulting .qch file. The path specified is relative to +# the HTML output folder. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help +# Project output. For more information please see Qt Help Project / Namespace +# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace). +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt +# Help Project output. For more information please see Qt Help Project / Virtual +# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual- +# folders). +# The default value is: doc. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_VIRTUAL_FOLDER = doc + +# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom +# filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom- +# filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's filter section matches. Qt Help Project / Filter Attributes (see: +# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_SECT_FILTER_ATTRS = + +# The QHG_LOCATION tag can be used to specify the location of Qt's +# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the +# generated .qhp file. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be +# generated, together with the HTML files, they form an Eclipse help plugin. To +# install this plugin and make it available under the help contents menu in +# Eclipse, the contents of the directory containing the HTML and XML files needs +# to be copied into the plugins directory of eclipse. The name of the directory +# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. +# After copying Eclipse needs to be restarted before the help appears. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the Eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have this +# name. Each documentation set should have its own identifier. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# If you want full control over the layout of the generated HTML pages it might +# be necessary to disable the index and replace it with your own. The +# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# of each HTML page. A value of NO enables the index and the value YES disables +# it. Since the tabs in the index contain the same information as the navigation +# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +DISABLE_INDEX = YES + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. If the tag +# value is set to YES, a side panel will be generated containing a tree-like +# index structure (just like the one that is generated for HTML Help). For this +# to work a browser that supports JavaScript, DHTML, CSS and frames is required +# (i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom stylesheets (see HTML_EXTRA_STYLESHEET) one can +# further fine-tune the look of the index. As an example, the default style +# sheet generated by doxygen has an example that shows how to put an image at +# the root of the tree instead of the PROJECT_NAME. Since the tree basically has +# the same information as the tab index, you could consider setting +# DISABLE_INDEX to YES when enabling this option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_TREEVIEW = YES + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that +# doxygen will group on one line in the generated HTML documentation. +# +# Note that a value of 0 will completely suppress the enum values from appearing +# in the overview section. +# Minimum value: 0, maximum value: 20, default value: 4. +# This tag requires that the tag GENERATE_HTML is set to YES. + +ENUM_VALUES_PER_LINE = 1 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width (in pixels) of the frame in which the tree is shown. +# Minimum value: 0, maximum value: 1500, default value: 250. +# This tag requires that the tag GENERATE_HTML is set to YES. + +TREEVIEW_WIDTH = 250 + +# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open links to +# external symbols imported via tag files in a separate window. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of LaTeX formulas included as images in +# the HTML documentation. When you change the font size after a successful +# doxygen run you need to manually remove any form_*.png images from the HTML +# output directory to force them to be regenerated. +# Minimum value: 8, maximum value: 50, default value: 10. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are not +# supported properly for IE 6.0, but are supported on all modern browsers. +# +# Note that when changing this option you need to delete any form_*.png files in +# the HTML output directory before the changes have effect. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# http://www.mathjax.org) which uses client side Javascript for the rendering +# instead of using prerendered bitmaps. Use this if you do not have LaTeX +# installed or if you want to formulas look prettier in the HTML output. When +# enabled you may also need to install MathJax separately and configure the path +# to it using the MATHJAX_RELPATH option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +USE_MATHJAX = YES + +# When MathJax is enabled you can set the default output format to be used for +# the MathJax output. See the MathJax site (see: +# http://docs.mathjax.org/en/latest/output.html) for more details. +# Possible values are: HTML-CSS (which is slower, but has the best +# compatibility), NativeMML (i.e. MathML) and SVG. +# The default value is: HTML-CSS. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_FORMAT = HTML-CSS + +# When MathJax is enabled you need to specify the location relative to the HTML +# output directory using the MATHJAX_RELPATH option. The destination directory +# should contain the MathJax.js script. For instance, if the mathjax directory +# is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax +# Content Delivery Network so you can quickly see the result without installing +# MathJax. However, it is strongly recommended to install a local copy of +# MathJax from http://www.mathjax.org before deployment. +# The default value is: http://cdn.mathjax.org/mathjax/latest. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest + +# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax +# extension names that should be enabled during MathJax rendering. For example +# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_EXTENSIONS = + +# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# of code that will be used on startup of the MathJax code. See the MathJax site +# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an +# example see the documentation. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_CODEFILE = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box for +# the HTML output. The underlying search engine uses javascript and DHTML and +# should work on any modern browser. Note that when using HTML help +# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +# there is already a search function so this one should typically be disabled. +# For large projects the javascript based search engine can be slow, then +# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to +# search using the keyboard; to jump to the search box use + S +# (what the is depends on the OS and browser, but it is typically +# , /