models: Add geometry utility functions

Author: tobifalk

models/include/cloe/utility/geometry.hpp

@@ -52,11 +52,34 @@ inline Eigen::Isometry3d pose_from_rotation_translation(const Eigen::Quaterniond
 }
 
 /**
- * Change a point's frame of reference.
- * Both the point and the child frame need to have the same parent frame.
+ * Compute the roll, pitch and yaw angles from a given pose.
+ *
+ * \param pose: Pose of which the rotation matrix shall be expressed in Euler angles.
+ */
+inline Eigen::Vector3d get_pose_roll_pitch_yaw(const Eigen::Isometry3d& pose) {
+  return pose.rotation().matrix().eulerAngles(2, 1, 0).reverse();
+}
+
+/**
+ * Change a point's frame of reference from the parent frame to the child frame.
+ *
+ * \param child_frame: Pose of the child reference frame w.r.t. the parent frame.
+ * \param pt_vec: Point coordinate vector w.r.t. the parent frame.
+ */
+inline void transform_point_to_child_frame(const Eigen::Isometry3d& child_frame,
+                                           Eigen::Vector3d* pt_vec) {
+  *pt_vec = child_frame.inverse() * (*pt_vec);
+}
+
+/**
+ * Change a point's frame of reference from the child frame to the parent frame.
+ *
+ * \param child_frame: Pose of the child reference frame w.r.t. the parent frame.
+ * \param pt_vec_child: Point coordinate vector w.r.t. the child frame.
  */
-inline void transform_to_child_frame(const Eigen::Isometry3d& child_frame, Eigen::Vector3d* point) {
-  *point = child_frame.inverse() * (*point);
+inline void transform_point_to_parent_frame(const Eigen::Isometry3d& child_frame,
+                                            Eigen::Vector3d* pt_vec_child) {
+  *pt_vec_child = child_frame * (*pt_vec_child);
 }
 
 }  // namespace utility

optional/vtd/src/osi_omni_sensor.cpp

@@ -627,8 +627,8 @@ void OsiOmniSensor::from_osi_boundary_points(const osi3::LaneBoundary& osi_lb,
     const auto& osi_pt = osi_lb.boundary_line(i);
     Eigen::Vector3d position = osi_vector3d_xyz_to_vector3d(osi_pt.position());
     // Transform points from the inertial into the sensor reference frame.
-    cloe::utility::transform_to_child_frame(osi_ego_pose_, &position);
-    cloe::utility::transform_to_child_frame(osi_sensor_pose_, &position);
+    cloe::utility::transform_point_to_child_frame(osi_ego_pose_, &position);
+    cloe::utility::transform_point_to_child_frame(osi_sensor_pose_, &position);
     lb.points.push_back(position);
   }
   // Compute clothoid segment. TODO(tobias): implement curved segments.

optional/vtd/src/osi_utils.cpp

@@ -115,7 +115,7 @@ void osi_transform_base_moving(const osi3::BaseMoving& base_ref, osi3::BaseMovin
 
   // Transform base.position/orientation/velocity/acceleration/orientation_rate:
   Eigen::Vector3d pos_ref_frame = pose.translation();
-  cloe::utility::transform_to_child_frame(pose_ref, &pos_ref_frame);
+  cloe::utility::transform_point_to_child_frame(pose_ref, &pos_ref_frame);
   pose.translation() = pos_ref_frame;          // location in reference frame
   pose.rotate(pose_ref.rotation().inverse());  // orientation in reference frame
   // Write new pose to base.