diff --git a/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp b/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp
index cf20fbe1..9dd4a0c1 100644
--- a/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp
+++ b/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp
@@ -126,18 +126,13 @@ inline visualization_msgs::Marker MarkerShape<T>::getMarker()
 template <typename T>
 FCL_CollisionObject MarkerShape<T>::getCollisionObject() const
 {
-    // FIXME
-    // fcl::Transform3f x(FCL_Quaternion(this->marker_.pose.orientation.w,
-    //                                   this->marker_.pose.orientation.x,
-    //                                   this->marker_.pose.orientation.y,
-    //                                   this->marker_.pose.orientation.z),
-    //                     FCL_Vec3(this->marker_.pose.position.x,
-    //                              this->marker_.pose.position.y,
-    //                              this->marker_.pose.position.z));
     fcl::Transform3f x(FCL_Quaternion(this->marker_.pose.orientation.w,
                                       this->marker_.pose.orientation.x,
                                       this->marker_.pose.orientation.y,
-                                      this->marker_.pose.orientation.z));
+                                      this->marker_.pose.orientation.z),
+                        FCL_Vec3(this->marker_.pose.position.x,
+                                 this->marker_.pose.position.y,
+                                 this->marker_.pose.position.z));
 
     FCL_CollisionObject cobj(this->ptr_fcl_bvh_, x);
     return cobj;
diff --git a/cob_obstacle_distance/src/distance_manager.cpp b/cob_obstacle_distance/src/distance_manager.cpp
index ea91f423..9a238131 100644
--- a/cob_obstacle_distance/src/distance_manager.cpp
+++ b/cob_obstacle_distance/src/distance_manager.cpp
@@ -273,9 +273,7 @@ void DistanceManager::calculate()
                 FCL_CollisionObject collision_obj = obstacle->getCollisionObject();
                 FCL_DistanceResult dist_result;
                 FCL_DistanceRequest dist_request(true, 5.0, 0.01);
-                //FIXME
-                // double dist = fcl::distance(&ooi_co, &collision_obj, dist_request, dist_result);
-                double dist = 0;
+                fcl::FCL_REAL dist = fcl::distance(&ooi_co, &collision_obj, dist_request, dist_result);
 
 
                 Eigen::Vector3d abs_obst_vector(dist_result.nearest_points[1][VEC_X],
diff --git a/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp b/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp
index 1503ef1c..5f2f6b0a 100644
--- a/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp
+++ b/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp
@@ -165,18 +165,13 @@ inline visualization_msgs::Marker MarkerShape<BVH_RSS_t>::getMarker()
 
 FCL_CollisionObject MarkerShape<BVH_RSS_t>::getCollisionObject() const
 {
-    // FIXME
-    // fcl::Transform3f x(FCL_Quaternion(this->marker_.pose.orientation.w,
-    //                                   this->marker_.pose.orientation.x,
-    //                                   this->marker_.pose.orientation.y,
-    //                                   this->marker_.pose.orientation.z),
-    //                    FCL_Vec3(this->marker_.pose.position.x,
-    //                             this->marker_.pose.position.y,
-    //                             this->marker_.pose.position.z));
     fcl::Transform3f x(FCL_Quaternion(this->marker_.pose.orientation.w,
                                       this->marker_.pose.orientation.x,
                                       this->marker_.pose.orientation.y,
-                                      this->marker_.pose.orientation.z));
+                                      this->marker_.pose.orientation.z),
+                       FCL_Vec3(this->marker_.pose.position.x,
+                                this->marker_.pose.position.y,
+                                this->marker_.pose.position.z));
     FCL_CollisionObject cobj(this->ptr_fcl_bvh_, x);
     return cobj;
 }