Directly use Eigen representations of IMU data. (#91)

cartographer_ros/CMakeLists.txt

@@ -93,7 +93,6 @@ add_executable(cartographer_node
   src/ros_log_sink.h
   src/sensor_bridge.cc
   src/sensor_bridge.h
-  src/sensor_data.cc
   src/sensor_data.h
   src/time_conversion.cc
   src/time_conversion.h

cartographer_ros/src/cartographer_node_main.cc

@@ -125,7 +125,7 @@ class Node {
 
   void AddOdometry(int64 timestamp, const string& frame_id,
                    const SensorData::Odometry& odometry);
-  void AddImu(int64 timestamp, const string& frame_id, const proto::Imu& imu);
+  void AddImu(int64 timestamp, const string& frame_id, const SensorData::Imu& imu);
   void AddHorizontalLaserFan(int64 timestamp, const string& frame_id,
                              const proto::LaserScan& laser_scan);
   void AddLaserFan3D(int64 timestamp, const string& frame_id,
@@ -251,7 +251,7 @@ void Node::AddOdometry(const int64 timestamp, const string& frame_id,
 }
 
 void Node::AddImu(const int64 timestamp, const string& frame_id,
-                  const proto::Imu& imu) {
+                  const SensorData::Imu& imu) {
   const carto::common::Time time = carto::common::FromUniversal(timestamp);
   try {
     const Rigid3d sensor_to_tracking =
@@ -262,10 +262,8 @@ void Node::AddImu(const int64 timestamp, const string& frame_id,
            "otherwise be imprecise.";
     map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
         ->AddImuData(time,
-                     sensor_to_tracking.rotation() *
+                     sensor_to_tracking.rotation() * imu.linear_acceleration,
-                         carto::transform::ToEigen(imu.linear_acceleration()),
+                     sensor_to_tracking.rotation() * imu.angular_velocity);
-                     sensor_to_tracking.rotation() *
-                         carto::transform::ToEigen(imu.angular_velocity()));
   } catch (const tf2::TransformException& ex) {
     LOG(WARNING) << ex.what();
   }

cartographer_ros/src/msg_conversion.cc

@@ -48,21 +48,6 @@ constexpr float kPointCloudComponentFourMagic = 1.;
 using ::cartographer::transform::Rigid3d;
 using ::cartographer::kalman_filter::PoseCovariance;
 
-void ToMessage(const ::cartographer::transform::proto::Vector3d& proto,
-               geometry_msgs::Vector3* vector) {
-  vector->x = proto.x();
-  vector->y = proto.y();
-  vector->z = proto.z();
-}
-
-void ToMessage(const ::cartographer::transform::proto::Quaterniond& proto,
-               geometry_msgs::Quaternion* quaternion) {
-  quaternion->w = proto.w();
-  quaternion->x = proto.x();
-  quaternion->y = proto.y();
-  quaternion->z = proto.z();
-}
-
 sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64 timestamp,
                                                    const string& frame_id,
                                                    const int num_points) {
@@ -123,23 +108,6 @@ sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage(
   return msg;
 }
 
-sensor_msgs::Imu ToImuMessage(const int64 timestamp, const string& frame_id,
-                              const ::cartographer::sensor::proto::Imu& imu) {
-  sensor_msgs::Imu message;
-  message.header.stamp =
-      ToRos(::cartographer::common::FromUniversal(timestamp));
-  message.header.frame_id = frame_id;
-
-  ToMessage(imu.orientation(), &message.orientation);
-  message.orientation_covariance = {{0., 0., 0., 0., 0., 0., 0., 0., 0.}};
-  ToMessage(imu.angular_velocity(), &message.angular_velocity);
-  message.angular_velocity_covariance = {{0., 0., 0., 0., 0., 0., 0., 0., 0.}};
-  ToMessage(imu.linear_acceleration(), &message.linear_acceleration);
-  message.linear_acceleration_covariance = {
-      {0., 0., 0., 0., 0., 0., 0., 0., 0.}};
-  return message;
-}
-
 sensor_msgs::LaserScan ToLaserScan(
     const int64 timestamp, const string& frame_id,
     const ::cartographer::sensor::proto::LaserScan& laser_scan) {
@@ -215,33 +183,6 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
   return msg;
 }
 
-::cartographer::sensor::proto::Imu ToCartographer(const sensor_msgs::Imu& msg) {
-  ::cartographer::sensor::proto::Imu proto;
-
-  if (msg.orientation_covariance[0] != -1) {
-    auto* orientation = proto.mutable_orientation();
-    orientation->set_x(msg.orientation.x);
-    orientation->set_y(msg.orientation.y);
-    orientation->set_z(msg.orientation.z);
-    orientation->set_w(msg.orientation.w);
-  }
-
-  if (msg.angular_velocity_covariance[0] != -1) {
-    auto* angular_velocity = proto.mutable_angular_velocity();
-    angular_velocity->set_x(msg.angular_velocity.x);
-    angular_velocity->set_y(msg.angular_velocity.y);
-    angular_velocity->set_z(msg.angular_velocity.z);
-  }
-
-  if (msg.linear_acceleration_covariance[0] != -1) {
-    auto* linear_acceleration = proto.mutable_linear_acceleration();
-    linear_acceleration->set_x(msg.linear_acceleration.x);
-    linear_acceleration->set_y(msg.linear_acceleration.y);
-    linear_acceleration->set_z(msg.linear_acceleration.z);
-  }
-  return proto;
-}
-
 ::cartographer::sensor::proto::LaserScan ToCartographer(
     const sensor_msgs::LaserScan& msg) {
   ::cartographer::sensor::proto::LaserScan proto;
@@ -308,47 +249,49 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
 }
 
 Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) {
-  return Rigid3d(Eigen::Vector3d(transform.transform.translation.x,
+  return Rigid3d(ToEigen(transform.transform.translation),
-                                 transform.transform.translation.y,
+                 ToEigen(transform.transform.rotation));
-                                 transform.transform.translation.z),
-                 Eigen::Quaterniond(transform.transform.rotation.w,
-                                    transform.transform.rotation.x,
-                                    transform.transform.rotation.y,
-                                    transform.transform.rotation.z));
 }
 
 Rigid3d ToRigid3d(const geometry_msgs::Pose& pose) {
-  return Rigid3d(
+  return Rigid3d({pose.position.x, pose.position.y, pose.position.z},
-      Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z),
+                 ToEigen(pose.orientation));
-      Eigen::Quaterniond(pose.orientation.w, pose.orientation.x,
+}
-                         pose.orientation.y, pose.orientation.z));
+
+Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3) {
+  return Eigen::Vector3d(vector3.x, vector3.y, vector3.z);
+}
+
+Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion) {
+  return Eigen::Quaterniond(quaternion.w, quaternion.x, quaternion.y,
+                            quaternion.z);
 }
 
 PoseCovariance ToPoseCovariance(const boost::array<double, 36>& covariance) {
   return Eigen::Map<const Eigen::Matrix<double, 6, 6>>(covariance.data());
 }
 
-geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid) {
+geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) {
   geometry_msgs::Transform transform;
-  transform.translation.x = rigid.translation().x();
+  transform.translation.x = rigid3d.translation().x();
-  transform.translation.y = rigid.translation().y();
+  transform.translation.y = rigid3d.translation().y();
-  transform.translation.z = rigid.translation().z();
+  transform.translation.z = rigid3d.translation().z();
-  transform.rotation.w = rigid.rotation().w();
+  transform.rotation.w = rigid3d.rotation().w();
-  transform.rotation.x = rigid.rotation().x();
+  transform.rotation.x = rigid3d.rotation().x();
-  transform.rotation.y = rigid.rotation().y();
+  transform.rotation.y = rigid3d.rotation().y();
-  transform.rotation.z = rigid.rotation().z();
+  transform.rotation.z = rigid3d.rotation().z();
   return transform;
 }
 
-geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid) {
+geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) {
   geometry_msgs::Pose pose;
-  pose.position.x = rigid.translation().x();
+  pose.position.x = rigid3d.translation().x();
-  pose.position.y = rigid.translation().y();
+  pose.position.y = rigid3d.translation().y();
-  pose.position.z = rigid.translation().z();
+  pose.position.z = rigid3d.translation().z();
-  pose.orientation.w = rigid.rotation().w();
+  pose.orientation.w = rigid3d.rotation().w();
-  pose.orientation.x = rigid.rotation().x();
+  pose.orientation.x = rigid3d.rotation().x();
-  pose.orientation.y = rigid.rotation().y();
+  pose.orientation.y = rigid3d.rotation().y();
-  pose.orientation.z = rigid.rotation().z();
+  pose.orientation.z = rigid3d.rotation().z();
   return pose;
 }
 

cartographer_ros/src/msg_conversion.h

@@ -44,9 +44,6 @@ sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage(
     int64 timestamp, const string& frame_id,
     const ::cartographer::sensor::proto::LaserScan& laser_scan);
 
-sensor_msgs::Imu ToImuMessage(int64 timestamp, const string& frame_id,
-                              const ::cartographer::sensor::proto::Imu& imu);
-
 sensor_msgs::PointCloud2 ToPointCloud2Message(
     int64 timestamp, const string& frame_id,
     const ::cartographer::sensor::PointCloud& point_cloud);
@@ -56,12 +53,10 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
     const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d);
 
 geometry_msgs::Transform ToGeometryMsgTransform(
-    const ::cartographer::transform::Rigid3d& rigid);
+    const ::cartographer::transform::Rigid3d& rigid3d);
 
 geometry_msgs::Pose ToGeometryMsgPose(
-    const ::cartographer::transform::Rigid3d& rigid);
+    const ::cartographer::transform::Rigid3d& rigid3d);
-
-::cartographer::sensor::proto::Imu ToCartographer(const sensor_msgs::Imu& msg);
 
 ::cartographer::sensor::proto::LaserScan ToCartographer(
     const sensor_msgs::LaserScan& msg);
@@ -77,6 +72,9 @@ geometry_msgs::Pose ToGeometryMsgPose(
 
 ::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::Pose& pose);
 
+Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3);
+Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion);
+
 ::cartographer::kalman_filter::PoseCovariance ToPoseCovariance(
     const boost::array<double, 36>& covariance);
 

cartographer_ros/src/sensor_bridge.cc

@@ -40,8 +40,14 @@ void SensorBridge::AddOdometryMessage(const string& topic,
 
 void SensorBridge::AddImuMessage(const string& topic,
                                  const sensor_msgs::Imu::ConstPtr& msg) {
+  CHECK_NE(msg->angular_velocity_covariance[0], -1);
+  CHECK_NE(msg->linear_acceleration_covariance[0], -1);
   auto sensor_data = ::cartographer::common::make_unique<SensorData>(
-      msg->header.frame_id, ToCartographer(*msg));
+      msg->header.frame_id,
+      SensorData::Imu{{msg->angular_velocity.x, msg->angular_velocity.y,
+                       msg->angular_velocity.z},
+                      {msg->linear_acceleration.x, msg->linear_acceleration.y,
+                       msg->linear_acceleration.z}});
   sensor_collator_->AddSensorData(
       trajectory_id_,
       ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic,

cartographer_ros/src/sensor_data.cc

@@ -1,59 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "sensor_data.h"
-
-#include <string>
-
-#include "glog/logging.h"
-
-namespace cartographer_ros {
-
-namespace {
-
-namespace proto = ::cartographer::sensor::proto;
-
-using ::cartographer::transform::Rigid3d;
-using ::cartographer::kalman_filter::PoseCovariance;
-
-const string& CheckNoLeadingSlash(const string& frame_id) {
-  if (frame_id.size() > 0) {
-    CHECK_NE(frame_id[0], '/');
-  }
-  return frame_id;
-}
-
-}  // namespace
-
-SensorData::SensorData(const string& frame_id, proto::Imu imu)
-    : type(SensorType::kImu),
-      frame_id(CheckNoLeadingSlash(frame_id)),
-      imu(imu) {}
-
-SensorData::SensorData(const string& frame_id, proto::LaserScan laser_scan)
-    : type(SensorType::kLaserScan),
-      frame_id(CheckNoLeadingSlash(frame_id)),
-      laser_scan(laser_scan) {}
-
-SensorData::SensorData(const string& frame_id, proto::LaserFan3D laser_fan_3d)
-    : type(SensorType::kLaserFan3D),
-      frame_id(CheckNoLeadingSlash(frame_id)),
-      laser_fan_3d(laser_fan_3d) {}
-
-SensorData::SensorData(const string& frame_id, const Odometry& odometry)
-    : type(SensorType::kOdometry), frame_id(frame_id), odometry(odometry) {}
-
-}  // namespace cartographer_ros

cartographer_ros/src/sensor_data.h

@@ -22,11 +22,13 @@
 #include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/sensor/proto/sensor.pb.h"
 #include "cartographer/transform/rigid_transform.h"
+#include "glog/logging.h"
 
 namespace cartographer_ros {
 
-// This type is a logical union, i.e. only one proto is actually filled in. It
+// This type is a logical union, i.e. only one type of sensor data is actually
-// is only used for time ordering sensor data before passing it on.
+// filled in. It is only used for time ordering sensor data before passing it
+// on.
 enum class SensorType { kImu, kLaserScan, kLaserFan3D, kOdometry };
 struct SensorData {
   struct Odometry {
@@ -34,19 +36,43 @@ struct SensorData {
     ::cartographer::kalman_filter::PoseCovariance covariance;
   };
 
-  SensorData(const string& frame_id, ::cartographer::sensor::proto::Imu imu);
+  struct Imu {
-  SensorData(const string& frame_id,
+    Eigen::Vector3d angular_velocity;
-             ::cartographer::sensor::proto::LaserScan laser_scan);
+    Eigen::Vector3d linear_acceleration;
-  SensorData(const string& frame_id,
+  };
-             ::cartographer::sensor::proto::LaserFan3D laser_fan_3d);
+
-  SensorData(const string& frame_id, const Odometry& odometry);
+  SensorData(const string& frame_id, const Imu& imu)
+      : type(SensorType::kImu),
+        frame_id(CheckNoLeadingSlash(frame_id)),
+        imu(imu) {}
+
+  SensorData(const string& frame_id, const ::cartographer::sensor::proto::LaserScan& laser_scan)
+      : type(SensorType::kLaserScan),
+        frame_id(CheckNoLeadingSlash(frame_id)),
+        laser_scan(laser_scan) {}
+
+  SensorData(const string& frame_id, const ::cartographer::sensor::proto::LaserFan3D& laser_fan_3d)
+      : type(SensorType::kLaserFan3D),
+        frame_id(CheckNoLeadingSlash(frame_id)),
+        laser_fan_3d(laser_fan_3d) {}
+
+  SensorData(const string& frame_id, const Odometry& odometry)
+      : type(SensorType::kOdometry), frame_id(frame_id), odometry(odometry) {}
 
   SensorType type;
   string frame_id;
-  ::cartographer::sensor::proto::Imu imu;
+  Imu imu;
   ::cartographer::sensor::proto::LaserScan laser_scan;
   ::cartographer::sensor::proto::LaserFan3D laser_fan_3d;
   Odometry odometry;
+
+  private:
+    static const string& CheckNoLeadingSlash(const string& frame_id) {
+      if (frame_id.size() > 0) {
+        CHECK_NE(frame_id[0], '/');
+      }
+      return frame_id;
+    }
 };
 
 }  // namespace cartographer_ros