changed order of rotation

Author: wumanu

source/dbot/gpu/kinect_image_model_gpu.hpp

@@ -378,8 +378,11 @@ class KinectImageModelGPU
 
                 osr::PoseVector pose;
 
-                pose.orientation() = delta.orientation() * pose_0.orientation();
-                pose.position() = delta.position() + pose_0.position();
+                /// \todo: this should be done through the the apply_delta
+                /// function
+                pose.position() = pose_0.orientation().rotation_matrix()
+                        * delta.position() + pose_0.position();
+                pose.orientation() = pose_0.orientation() * delta.orientation();
 
                 poses[i_state][i_obj] = pose.homogeneous().cast<float>();
             }

source/dbot/model/depth_pixel_model.hpp

@@ -189,14 +189,21 @@ class DepthPixelModel
         if (render_cache_.find(current_state) == render_cache_.end())
         {
             State current_pose = current_state;
-            current_pose.component(0).orientation() =
-                current_state.component(0).orientation() *
-                nominal_pose_.component(0).orientation();
+
+
+            /// \todo: this transformation should not be done in here
 
             current_pose.component(0).position() =
-                current_state.component(0).position() +
+                nominal_pose_.component(0).orientation().rotation_matrix() *
+                    current_state.component(0).position() +
                 nominal_pose_.component(0).position();
 
+            current_pose.component(0).orientation() =
+                nominal_pose_.component(0).orientation() *
+                current_state.component(0).orientation();
+
+
+
             std::lock_guard<std::mutex> lock(*mutex);
             map(current_pose, render_cache_[current_state]);
             poses_cache_[current_state] = current_pose;

source/dbot/model/kinect_image_model.hpp

@@ -45,7 +45,6 @@
 
 namespace dbot
 {
-/// \todo get rid of traits
 // Forward declarations
 template <typename Scalar, typename State, int OBJECTS>
 class KinectImageModel;
@@ -152,8 +151,12 @@ class KinectImageModel
                 auto delta = deltas[i_state].component(i_obj);
 
                 osr::PoseVector pose;
-                pose.orientation() = delta.orientation() * pose_0.orientation();
-                pose.position() = delta.position() + pose_0.position();
+
+                /// \todo: this should be done through the the apply_delta
+                /// function
+                pose.position() = pose_0.orientation().rotation_matrix()
+                        * delta.position() + pose_0.position();
+                pose.orientation() = pose_0.orientation() * delta.orientation();
 
                 poses[i_obj] = pose.affine();
             }

source/dbot/model/rao_blackwell_sensor.hpp

@@ -52,6 +52,8 @@ class RbSensor
     typedef Eigen::Array<int, -1, 1> IntArray;
     typedef Eigen::Matrix<fl::Real, -1, 1> RealVector;
 //    typedef State PoseArray;
+
+    /// \todo: this should be a different type, only containing poses
     typedef osr::FreeFloatingRigidBodiesState<> PoseArray;
 
 public:

source/dbot/tracker/gaussian_tracker.cpp

@@ -54,6 +54,9 @@ auto GaussianTracker::on_track(const Obsrv& obsrv) -> State
     // geometry sense, the covariance matrix would also have to change due
     // to the parallel transport
 
+
+    /// \todo: this transformation has to be checked and shoudl not be done here
+
     State old_pose = belief_.mean();
     filter_->sensor().local_sensor().body_model().nominal_pose(old_pose);