Merging osr with dbot

Author: cassinaj

CMakeLists.txt

@@ -108,7 +108,7 @@ endif(DBOT_BUILD_GPU AND CUDA_FOUND AND GLEW_FOUND AND OPENGL_FOUND)
 # Dependencies             #
 ############################
 # Use catkin of available otherwise fall back to native cmake
-find_package(catkin QUIET COMPONENTS fl osr)
+find_package(catkin QUIET COMPONENTS fl)
 
 if(DBOT_BUILD_GPU)
   list(APPEND dbot_INCLUDE_DIRS ${CUDA_INCLUDE_DIRS})
@@ -125,7 +125,6 @@ if(catkin_FOUND)
     LIBRARIES
       ${dbot_LIBRARIES}
     CATKIN_DEPENDS
-      osr
       fl
     DEPENDS
       Eigen)

package.xml

@@ -8,17 +8,15 @@
 
   <license>GNU GPL</license>
 
-  <url type="website">https://github.com/bayesian-object-tracking</url>
+  <url type="website">https://github.com/bayesian-object-tracking/dbot</url>
 
   <author email="manuel.wuthrich@gmail.com">Manuel Wuethrich</author>
   <author email="jan.issac@gmail.com">Jan Issac</author>
 
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>fl</build_depend>
-  <build_depend>osr</build_depend>
   <run_depend>fl</run_depend>
-  <run_depend>osr</run_depend>
 
   <export>
     <!-- <metapackage/> -->

source/dbot/builder/rb_sensor_builder.cpp

@@ -27,7 +27,7 @@
  * \author Jan Issac (jan.issac@gmail.com)
  */
 
-#include <osr/free_floating_rigid_bodies_state.hpp>
+#include <dbot/pose/free_floating_rigid_bodies_state.hpp>
 
 #include <dbot/builder/rb_sensor_builder.h>
 #include <dbot/builder/rb_sensor_builder.hpp>

source/dbot/gpu/kinect_image_model_gpu.hpp

@@ -37,9 +37,9 @@
 #include "boost/filesystem.hpp"
 #include "Eigen/Core"
 
-#include <osr/pose_vector.hpp>
+#include <dbot/pose/pose_vector.hpp>
 #include <dbot/model/rao_blackwell_sensor.hpp>
-#include <osr/free_floating_rigid_bodies_state.hpp>
+#include <dbot/pose/free_floating_rigid_bodies_state.hpp>
 
 #include <dbot/traits.hpp>
 #include <dbot/gpu/object_rasterizer.hpp>

source/dbot/model/depth_pixel_model.hpp

@@ -46,7 +46,7 @@
 
 #include <dbot/rigid_body_renderer.hpp>
 
-#include <osr/pose_hashing.hpp>
+#include <dbot/pose/pose_hashing.hpp>
 
 namespace fl
 {

source/dbot/model/kinect_image_model.hpp

@@ -33,10 +33,10 @@
 
 #include <Eigen/Core>
 
-#include <osr/pose_vector.hpp>
+#include <dbot/pose/pose_vector.hpp>
 #include <fl/util/assertions.hpp>
 #include <dbot/traits.hpp>
-#include <osr/free_floating_rigid_bodies_state.hpp>
+#include <dbot/pose/free_floating_rigid_bodies_state.hpp>
 #include <dbot/model/rao_blackwell_sensor.hpp>
 
 #include <dbot/rigid_body_renderer.hpp>

source/dbot/model/rao_blackwell_sensor.hpp

@@ -31,10 +31,10 @@
 #include <Eigen/Core>
 
 #include <fl/util/types.hpp>
-#include <osr/pose_vector.hpp>
-#include <osr/pose_velocity_vector.hpp>
-#include <osr/composed_vector.hpp>
-#include <osr/free_floating_rigid_bodies_state.hpp>
+#include <dbot/pose/pose_vector.hpp>
+#include <dbot/pose/pose_velocity_vector.hpp>
+#include <dbot/pose/composed_vector.hpp>
+#include <dbot/pose/free_floating_rigid_bodies_state.hpp>
 
 namespace dbot
 {

source/dbot/pose/composed_vector.hpp

@@ -0,0 +1,62 @@
+/*
+ * This is part of the Bayesian Object Tracking (bot),
+ * (https://github.com/bayesian-object-tracking)
+ *
+ * Copyright (c) 2015 Max Planck Society,
+ * 				 Autonomous Motion Department,
+ * 			     Institute for Intelligent Systems
+ *
+ * This Source Code Form is subject to the terms of the GNU General Public
+ * License License (GNU GPL). A copy of the license can be found in the LICENSE
+ * file distributed with this source code.
+ */
+
+/**
+ * \file composed_vector.hpp
+ * \date August 2015
+ * \author Manuel Wuthrich (manuel.wuthrich@gmail.com)
+ */
+
+#pragma once
+
+#include <Eigen/Dense>
+
+namespace osr
+{
+template <typename Block, typename Vector>
+class ComposedVector : public Vector
+{
+public:
+    // constructor and destructor **********************************************
+    ComposedVector() {}
+    template <typename T>
+    ComposedVector(const Eigen::MatrixBase<T>& vector)
+        : Vector(vector)
+    {
+    }
+
+    virtual ~ComposedVector() noexcept {}
+    // operators ***************************************************************
+    template <typename T>
+    void operator=(const Eigen::MatrixBase<T>& vector)
+    {
+        *((Vector*)(this)) = vector;
+    }
+
+    // accessors ***************************************************************
+    const Block component(int index) const
+    {
+        return Block(*this, index * Block::SizeAtCompileTime);
+    }
+    int count() const { return this->size() / Block::SizeAtCompileTime; }
+    // mutators ****************************************************************
+    Block component(int index)
+    {
+        return Block(*this, index * Block::SizeAtCompileTime);
+    }
+    void recount(int new_count)
+    {
+        return this->resize(new_count * Block::SizeAtCompileTime);
+    }
+};
+}

source/dbot/pose/euler_vector.hpp

@@ -0,0 +1,153 @@
+/*
+ * This is part of the Bayesian Object Tracking (bot),
+ * (https://github.com/bayesian-object-tracking)
+ *
+ * Copyright (c) 2015 Max Planck Society,
+ * 				 Autonomous Motion Department,
+ * 			     Institute for Intelligent Systems
+ *
+ * This Source Code Form is subject to the terms of the GNU General Public
+ * License License (GNU GPL). A copy of the license can be found in the LICENSE
+ * file distributed with this source code.
+ */
+
+/**
+ * \file orientation_transition.hpp
+ * \date July 2015
+ * \author Manuel Wuthrich (manuel.wuthrich@gmail.com)
+ */
+
+#pragma once
+
+#include <Eigen/Dense>
+
+namespace osr
+{
+typedef double Real;
+
+/// basic functionality for both vectors and blocks ****************************
+template <typename Base>
+class EulerBase : public Base
+{
+public:
+    // types *******************************************************************
+    typedef typename Base::Scalar Scalar;
+    typedef Eigen::Matrix<Scalar, 3, 1> Axis;
+    typedef Eigen::Matrix<Scalar, 3, 1> Vector;
+    typedef Eigen::AngleAxis<Scalar> AngleAxis;
+    typedef Eigen::Quaternion<Scalar> Quaternion;
+    typedef Eigen::Matrix<Scalar, 3, 3> RotationMatrix;
+
+    typedef EulerBase<Eigen::Matrix<Real, 3, 1>> EulerVector;
+
+    // constructor and destructor **********************************************
+    EulerBase(const Base& vector) : Base(vector) {}
+    virtual ~EulerBase() noexcept {}
+    // operators ***************************************************************
+    template <typename T>
+    void operator=(const Eigen::MatrixBase<T>& vector)
+    {
+        *((Base*)(this)) = vector;
+    }
+
+    // accessor ****************************************************************
+    virtual Scalar angle() const { return this->norm(); }
+    virtual Axis axis() const
+    {
+        Axis ax = this->normalized();
+
+        if (!std::isfinite(ax.sum()))
+        {
+            return Axis(1, 0, 0);
+        }
+        return ax;
+    }
+    virtual AngleAxis angle_axis() const { return AngleAxis(angle(), axis()); }
+    virtual Quaternion quaternion() const { return Quaternion(angle_axis()); }
+    virtual RotationMatrix rotation_matrix() const
+    {
+        return RotationMatrix(quaternion());
+    }
+    virtual EulerVector inverse() const { return EulerVector(-*this); }
+    // mutators ****************************************************************
+    virtual void angle_axis(const Scalar& angle, const Axis& axis)
+    {
+        *this = angle * axis;
+
+        rescale();
+    }
+    virtual void angle_axis(const AngleAxis& ang_axis)
+    {
+        angle_axis(ang_axis.angle(), ang_axis.axis());
+    }
+    virtual void quaternion(const Quaternion& quat)
+    {
+        angle_axis(AngleAxis(quat));
+    }
+    virtual void rotation_matrix(const RotationMatrix& rot_mat)
+    {
+        angle_axis(AngleAxis(rot_mat));
+    }
+    virtual void rescale()  // make sure the norm is in [0,Pi]
+    {
+        Scalar alpha = angle();
+
+        if (alpha <= M_PI)
+        {
+            return;
+        }
+        if (alpha > 2 * M_PI)
+        {
+            alpha -= std::floor(alpha / (2 * M_PI)) * 2 * M_PI;
+        }
+        if (alpha > M_PI)
+        {
+            alpha -= 2 * M_PI;
+        }
+
+        (*this) = axis() * alpha;
+    }
+
+    // operators ***************************************************************
+    template <typename T>
+    EulerVector operator*(const EulerBase<T>& factor)
+    {
+        EulerVector product(EulerVector::Zero());
+        product.quaternion(this->quaternion() * factor.quaternion());
+        return product;
+    }
+};
+
+/// implementation for vectors *************************************************
+class EulerVector : public EulerBase<Eigen::Matrix<Real, 3, 1>>
+{
+public:
+    typedef EulerBase<Eigen::Matrix<Real, 3, 1>> Base;
+
+    // constructor and destructor **********************************************
+    EulerVector() : Base(Base::Zero()) {}
+    template <typename T>
+    EulerVector(const Eigen::MatrixBase<T>& vector)
+        : Base(vector)
+    {
+    }
+
+    virtual ~EulerVector() noexcept {}
+};
+
+/// implementation for blocks **************************************************
+template <typename Vector>
+class EulerBlock : public EulerBase<Eigen::VectorBlock<Vector, 3>>
+{
+public:
+    typedef Eigen::VectorBlock<Vector, 3> Block;
+    typedef EulerBase<Block> Base;
+
+    using Base::operator=;
+
+    // constructor and destructor **********************************************
+    EulerBlock(const Block& block) : Base(block) {}
+    EulerBlock(Vector& vector, int start) : Base(Block(vector, start)) {}
+    virtual ~EulerBlock() noexcept {}
+};
+}