[RST-1548] transform covariance

CMakeLists.txt

@@ -9,9 +9,12 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   tf2
 )
+find_package(Eigen3 REQUIRED)
 
 catkin_package(
-  INCLUDE_DIRS include
+  INCLUDE_DIRS
+    include
+    ${EIGEN3_INCLUDE_DIRS}
   CATKIN_DEPENDS
     roscpp
     tf2
@@ -41,11 +44,12 @@ if(CATKIN_ENABLE_TESTING)
   include_directories(
     include
     ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
   )
 
   find_package(roslint REQUIRED)
   find_package(rostest REQUIRED)
-  
+
   # Lint tests
   set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references")
   roslint_cpp()

include/tf2_2d/rotation.h

@@ -9,6 +9,8 @@
 #include <tf2/LinearMath/Scalar.h>
 #include <tf2_2d/vector2.h>
 
+#include <Eigen/Core>
+
 
 namespace tf2_2d
 {
@@ -168,6 +170,16 @@ class Rotation
    */
   bool fuzzyZero() const;
 
+  /**
+   * @brief Get a 2x2 rotation matrix
+   */
+  Eigen::Matrix2d getRotationMatrix() const;
+
+  /**
+   * @brief Get a 3x3 homogeneous transformation matrix with just the rotation portion populated
+   */
+  Eigen::Matrix3d getHomogeneousMatrix() const;
+
 private:
   tf2Scalar angle_;  //!< Storage for the angle in radians
   mutable tf2Scalar cos_angle_;  //!< Storage for cos(angle) so we only compute it once
@@ -225,6 +237,11 @@ Rotation operator/(Rotation lhs, const tf2Scalar rhs);
  */
 Vector2 operator*(const Rotation& lhs, const Vector2& rhs);
 
+/**
+ * @brief Stream the rotation in human-readable format
+ */
+std::ostream& operator<<(std::ostream& stream, const Rotation& rotation);
+
 }  // namespace tf2_2d
 
 #include <tf2_2d/rotation_impl.h>

include/tf2_2d/rotation_impl.h

@@ -10,6 +10,8 @@
 #include <tf2/LinearMath/MinMax.h>  // NOLINT: the order of the headers here is important.
 #include <tf2_2d/vector2.h>
 
+#include <Eigen/Core>
+
 #include <cmath>
 
 
@@ -174,6 +176,22 @@ inline bool Rotation::fuzzyZero() const
   return (angle_ * angle_) < TF2SIMD_EPSILON;
 }
 
+inline Eigen::Matrix2d Rotation::getRotationMatrix() const
+{
+  populateTrigCache();
+  Eigen::Matrix2d matrix;
+  matrix << cos_angle_, -sin_angle_, sin_angle_, cos_angle_;
+  return matrix;
+}
+
+inline Eigen::Matrix3d Rotation::getHomogeneousMatrix() const
+{
+  populateTrigCache();
+  Eigen::Matrix3d matrix;
+  matrix << cos_angle_, -sin_angle_, 0, sin_angle_, cos_angle_, 0, 0, 0, 1;
+  return matrix;
+}
+
 inline Rotation& Rotation::wrap()
 {
   // Handle the 2*Pi roll-over
@@ -225,6 +243,11 @@ inline Vector2 operator*(const Rotation& lhs, const Vector2& rhs)
   return lhs.rotate(rhs);
 }
 
+inline std::ostream& operator<<(std::ostream& stream, const Rotation& rotation)
+{
+  return stream << "angle: " << rotation.angle();
+}
+
 }  // namespace tf2_2d
 
 #endif  // TF2_2D_ROTATION_IMPL_H

include/tf2_2d/tf2_2d.h

@@ -25,6 +25,10 @@
 #include <tf2_2d/transform.h>
 #include <tf2_2d/vector2.h>
 
+#include <boost/array.hpp>
+#include <Eigen/Core>
+
+#include <array>
 #include <cmath>
 
 /**
@@ -35,12 +39,32 @@
 namespace tf2
 {
 
-inline geometry_msgs::Vector3 toMsg(const tf2_2d::Vector2& in)
+inline void toMsg(const tf2_2d::Vector2& in, geometry_msgs::Vector3& msg)
+{
+  msg.x = in.x();
+  msg.y = in.y();
+  msg.z = 0;
+}
+
+inline void toMsg(const tf2_2d::Vector2& in, geometry_msgs::Point& msg)
+{
+  msg.x = in.x();
+  msg.y = in.y();
+  msg.z = 0;
+}
+
+inline void toMsg(const tf2_2d::Vector2& in, geometry_msgs::Point32& msg)
 {
-  geometry_msgs::Vector3 msg;
   msg.x = in.x();
   msg.y = in.y();
   msg.z = 0;
+}
+
+// You can only choose one destination message type with this signature
+inline geometry_msgs::Vector3 toMsg(const tf2_2d::Vector2& in)
+{
+  geometry_msgs::Vector3 msg;
+  toMsg(in, msg);
   return msg;
 }
 
@@ -62,12 +86,24 @@ inline void fromMsg(const geometry_msgs::Point32& msg, tf2_2d::Vector2& out)
   out.setY(msg.y);
 }
 
-inline geometry_msgs::Vector3Stamped toMsg(const tf2::Stamped<tf2_2d::Vector2>& in)
+inline void toMsg(const tf2::Stamped<tf2_2d::Vector2>& in, geometry_msgs::Vector3Stamped& msg)
+{
+  msg.header.stamp = in.stamp_;
+  msg.header.frame_id = in.frame_id_;
+  toMsg(static_cast<const tf2_2d::Vector2&>(in), msg.vector);
+}
+
+inline void toMsg(const tf2::Stamped<tf2_2d::Vector2>& in, geometry_msgs::PointStamped& msg)
 {
-  geometry_msgs::Vector3Stamped msg;
   msg.header.stamp = in.stamp_;
   msg.header.frame_id = in.frame_id_;
-  msg.vector = toMsg(static_cast<const tf2_2d::Vector2&>(in));
+  toMsg(static_cast<const tf2_2d::Vector2&>(in), msg.point);
+}
+
+inline geometry_msgs::Vector3Stamped toMsg(const tf2::Stamped<tf2_2d::Vector2>& in)
+{
+  geometry_msgs::Vector3Stamped msg;
+  toMsg(in, msg);
   return msg;
 }
 
@@ -85,13 +121,18 @@ inline void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped<tf2_2d:
   fromMsg(msg.point, static_cast<tf2_2d::Vector2&>(out));
 }
 
-inline geometry_msgs::Quaternion toMsg(const tf2_2d::Rotation& in)
+inline void toMsg(const tf2_2d::Rotation& in, geometry_msgs::Quaternion& msg)
 {
-  geometry_msgs::Quaternion msg;
   msg.x = 0;
   msg.y = 0;
   msg.z = std::sin(0.5 * in.angle());
   msg.w = std::cos(0.5 * in.angle());
+}
+
+inline geometry_msgs::Quaternion toMsg(const tf2_2d::Rotation& in)
+{
+  geometry_msgs::Quaternion msg;
+  toMsg(in, msg);
   return msg;
 }
 
@@ -100,12 +141,17 @@ inline void fromMsg(const geometry_msgs::Quaternion& msg, tf2_2d::Rotation& out)
   out.setAngle(tf2::getYaw(msg));
 }
 
-inline geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2_2d::Rotation>& in)
+inline void toMsg(const tf2::Stamped<tf2_2d::Rotation>& in, geometry_msgs::QuaternionStamped& msg)
 {
-  geometry_msgs::QuaternionStamped msg;
   msg.header.stamp = in.stamp_;
   msg.header.frame_id = in.frame_id_;
-  msg.quaternion = toMsg(static_cast<const tf2_2d::Rotation&>(in));
+  toMsg(static_cast<const tf2_2d::Rotation&>(in), msg.quaternion);
+}
+
+inline geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2_2d::Rotation>& in)
+{
+  geometry_msgs::QuaternionStamped msg;
+  toMsg(in, msg);
   return msg;
 }
 
@@ -116,11 +162,29 @@ inline void fromMsg(const geometry_msgs::QuaternionStamped& msg, tf2::Stamped<tf
   fromMsg(msg.quaternion, static_cast<tf2_2d::Rotation&>(out));
 }
 
+inline void toMsg(const tf2_2d::Transform& in, geometry_msgs::Transform& msg)
+{
+  toMsg(in.translation(), msg.translation);
+  toMsg(in.rotation(), msg.rotation);
+}
+
+inline void toMsg(const tf2_2d::Transform& in, geometry_msgs::Pose& msg)
+{
+  toMsg(in.translation(), msg.position);
+  toMsg(in.rotation(), msg.orientation);
+}
+
+inline void toMsg(const tf2_2d::Transform& in, geometry_msgs::Pose2D& msg)
+{
+  msg.x = in.x();
+  msg.y = in.y();
+  msg.theta = in.theta();
+}
+
 inline geometry_msgs::Transform toMsg(const tf2_2d::Transform& in)
 {
   geometry_msgs::Transform msg;
-  msg.translation = toMsg(in.translation());
-  msg.rotation = toMsg(in.rotation());
+  toMsg(in, msg);
   return msg;
 }
 
@@ -151,6 +215,20 @@ inline void fromMsg(const geometry_msgs::Pose2D& msg, tf2_2d::Transform& out)
   out.setY(msg.y);
 }
 
+inline void toMsg(const tf2::Stamped<tf2_2d::Transform>& in, geometry_msgs::TransformStamped& msg)
+{
+  msg.header.stamp = in.stamp_;
+  msg.header.frame_id = in.frame_id_;
+  toMsg(static_cast<const tf2_2d::Transform&>(in), msg.transform);
+}
+
+inline void toMsg(const tf2::Stamped<tf2_2d::Transform>& in, geometry_msgs::PoseStamped& msg)
+{
+  msg.header.stamp = in.stamp_;
+  msg.header.frame_id = in.frame_id_;
+  toMsg(static_cast<const tf2_2d::Transform&>(in), msg.pose);
+}
+
 inline geometry_msgs::TransformStamped toMsg(const tf2::Stamped<tf2_2d::Transform>& in)
 {
   geometry_msgs::TransformStamped msg;
@@ -174,6 +252,37 @@ inline void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<tf2_2d::
   fromMsg(msg.pose, static_cast<tf2_2d::Transform&>(out));
 }
 
+
+
+inline
+Eigen::Matrix3d transformCovariance(const Eigen::Matrix3d& cov_in, const tf2_2d::Transform& transform)
+{
+  Eigen::Matrix3d R = transform.rotation().getHomogeneousMatrix();
+  return R * cov_in * R.transpose();
+}
+
+inline
+std::array<double, 9> transformCovariance(const std::array<double, 9>& cov_in, const tf2_2d::Transform& transform)
+{
+  Eigen::Matrix3d R = transform.rotation().getHomogeneousMatrix();
+  std::array<double, 9> cov_out;
+  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> cov_out_map(cov_out.data());
+  Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> cov_in_map(cov_in.data());
+  cov_out_map = R * cov_in_map * R.transpose();
+  return cov_out;
+}
+
+inline
+boost::array<double, 9> transformCovariance(const boost::array<double, 9>& cov_in, const tf2_2d::Transform& transform)
+{
+  Eigen::Matrix3d R = transform.rotation().getHomogeneousMatrix();
+  boost::array<double, 9> cov_out;
+  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> cov_out_map(cov_out.data());
+  Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> cov_in_map(cov_in.data());
+  cov_out_map = R * cov_in_map * R.transpose();
+  return cov_out;
+}
+
 }  // namespace tf2
 
 #endif  // TF2_2D_TF2_2D_H

include/tf2_2d/transform.h

@@ -11,6 +11,8 @@
 #include <tf2_2d/rotation.h>
 #include <tf2_2d/vector2.h>
 
+#include <Eigen/Core>
+
 
 namespace tf2_2d
 {
@@ -137,9 +139,9 @@ class Transform
    */
   void setYaw(const tf2Scalar yaw);
   // There are a lot of common aliases for yaw
-  void setAngle(const tf2Scalar angle) { setYaw(angle); };
-  void setHeading(const tf2Scalar heading) { setYaw(heading); };
-  void setTheta(const tf2Scalar theta) { setYaw(theta); };
+  void setAngle(const tf2Scalar angle) { setYaw(angle); }
+  void setHeading(const tf2Scalar heading) { setYaw(heading); }
+  void setTheta(const tf2Scalar theta) { setYaw(theta); }
 
   /**
    * @brief Set this transformation to the identity
@@ -160,6 +162,11 @@ class Transform
    */
   Transform inverseTimes(const Transform& other) const;
 
+  /**
+   * @brief Get a 3x3 homogeneous transformation matrix
+   */
+  Eigen::Matrix3d getHomogeneousMatrix() const;
+
 private:
   Rotation rotation_;  //!< Storage for the rotation
   Vector2 translation_;  //!< Storage for the translation
@@ -182,6 +189,11 @@ Vector2 operator*(const Transform& lhs, const Vector2& rhs);
  */
 Rotation operator*(const Transform& lhs, const Rotation& rhs);
 
+/**
+ * @brief Stream the transformation in human-readable format
+ */
+std::ostream& operator<<(std::ostream& stream, const Transform& transform);
+
 }  // namespace tf2_2d
 
 #include <tf2_2d/transform_impl.h>

include/tf2_2d/transform_impl.h

@@ -13,6 +13,7 @@
 #include <tf2_2d/rotation.h>
 #include <tf2_2d/vector2.h>
 
+#include <Eigen/Core>
 
 namespace tf2_2d
 {
@@ -129,6 +130,15 @@ inline Transform Transform::inverseTimes(const Transform& other) const
   return inverse() * other;
 }
 
+inline Eigen::Matrix3d Transform::getHomogeneousMatrix() const
+{
+  Eigen::Matrix3d matrix;
+  matrix.topLeftCorner<2, 2>() = rotation_.getRotationMatrix();
+  matrix.topRightCorner<2, 1>() = translation_.getVector();
+  matrix.bottomRows<1>() << 0, 0, 1;
+  return matrix;
+}
+
 inline Transform operator*(Transform lhs, const Transform& rhs)
 {
   lhs *= rhs;
@@ -145,6 +155,11 @@ inline Rotation operator*(const Transform& lhs, const Rotation& rhs)
   return lhs.rotation() + rhs;
 }
 
+inline std::ostream& operator<<(std::ostream& stream, const Transform& transform)
+{
+  return stream << "x: " << transform.x() << ", y: " << transform.y() << ", yaw: " << transform.yaw();
+}
+
 }  // namespace tf2_2d
 
 #endif  // TF2_2D_TRANSFORM_IMPL_H