Transforms odometry into the tracking frame. (cartographer-project#65)

ejimenezb · Sep 20, 2016 · 534f011 · 534f011
1 parent 45d18d5
commit 534f011
Show file tree

Hide file tree

Showing 4 changed files with 30 additions and 24 deletions.
diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc
@@ -204,8 +204,8 @@ class Node {
   void HandleSensorData(int64 timestamp,
                         std::unique_ptr<SensorData> sensor_data);
 
-  void AddOdometry(int64 timestamp, const string& frame_id, const Rigid3d& pose,
-                   const PoseCovariance& covariance);
+  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 AddHorizontalLaserFan(int64 timestamp, const string& frame_id,
                              const proto::LaserScan& laser_scan);
@@ -286,10 +286,10 @@ Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time,
       ::ros::Duration(options_.lookup_transform_timeout_sec)));
 }
 
-void Node::AddOdometry(int64 timestamp, const string& frame_id,
-                       const Rigid3d& pose, const PoseCovariance& covariance) {
+void Node::AddOdometry(const int64 timestamp, const string& frame_id,
+                       const SensorData::Odometry& odometry) {
   const carto::common::Time time = carto::common::FromUniversal(timestamp);
-  PoseCovariance applied_covariance = covariance;
+  PoseCovariance applied_covariance = odometry.covariance;
   if (options_.use_constant_odometry_variance) {
     const Eigen::Matrix3d translational =
         Eigen::Matrix3d::Identity() *
@@ -303,8 +303,16 @@ void Node::AddOdometry(int64 timestamp, const string& frame_id,
         Eigen::Matrix3d::Zero(), rotational;
     // clang-format on
   }
-  map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
-      ->AddOdometerPose(time, pose, applied_covariance);
+  try {
+    const Rigid3d sensor_to_tracking =
+        LookupToTrackingTransformOrThrow(time, frame_id);
+    map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
+        ->AddOdometerPose(time, odometry.pose * sensor_to_tracking.inverse(),
+                          applied_covariance);
+  } catch (const tf2::TransformException& ex) {
+    LOG(WARNING) << "Cannot transform " << frame_id << " -> "
+                 << options_.tracking_frame << ": " << ex.what();
+  }
 }
 
 void Node::AddImu(const int64 timestamp, const string& frame_id,
@@ -315,7 +323,7 @@ void Node::AddImu(const int64 timestamp, const string& frame_id,
         LookupToTrackingTransformOrThrow(time, frame_id);
     CHECK(sensor_to_tracking.translation().norm() < 1e-5)
         << "The IMU frame must be colocated with the tracking frame. "
-           "Transforming linear accelaration into the tracking frame will "
+           "Transforming linear acceleration into the tracking frame will "
            "otherwise be imprecise.";
     map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
         ->AddImuData(time,
@@ -328,6 +336,7 @@ void Node::AddImu(const int64 timestamp, const string& frame_id,
                  << options_.tracking_frame << ": " << ex.what();
   }
 }
+
 void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
                                  const proto::LaserScan& laser_scan) {
   const carto::common::Time time = carto::common::FromUniversal(timestamp);
@@ -713,8 +722,7 @@ void Node::HandleSensorData(const int64 timestamp,
       return;
 
     case SensorType::kOdometry:
-      AddOdometry(timestamp, sensor_data->frame_id, sensor_data->odometry.pose,
-                  sensor_data->odometry.covariance);
+      AddOdometry(timestamp, sensor_data->frame_id, sensor_data->odometry);
       return;
   }
   LOG(FATAL);

diff --git a/cartographer_ros/src/sensor_data.cc b/cartographer_ros/src/sensor_data.cc
@@ -53,10 +53,7 @@ SensorData::SensorData(const string& frame_id, proto::LaserFan3D laser_fan_3d)
       frame_id(CheckNoLeadingSlash(frame_id)),
       laser_fan_3d(laser_fan_3d) {}
 
-SensorData::SensorData(const string& frame_id, const Rigid3d& pose,
-                       const PoseCovariance& covariance)
-    : type(SensorType::kOdometry),
-      frame_id(frame_id),
-      odometry{pose, covariance} {}
+SensorData::SensorData(const string& frame_id, const Odometry& odometry)
+    : type(SensorType::kOdometry), frame_id(frame_id), odometry(odometry) {}
 
 }  // namespace cartorapher_ros
diff --git a/cartographer_ros/src/sensor_data.h b/cartographer_ros/src/sensor_data.h
@@ -29,24 +29,24 @@ namespace cartographer_ros {
 // is only used for time ordering sensor data before passing it on.
 enum class SensorType { kImu, kLaserScan, kLaserFan3D, kOdometry };
 struct SensorData {
+  struct Odometry {
+    ::cartographer::transform::Rigid3d pose;
+    ::cartographer::kalman_filter::PoseCovariance covariance;
+  };
+
   SensorData(const string& frame_id, ::cartographer::sensor::proto::Imu imu);
   SensorData(const string& frame_id,
              ::cartographer::sensor::proto::LaserScan laser_scan);
   SensorData(const string& frame_id,
              ::cartographer::sensor::proto::LaserFan3D laser_fan_3d);
-  SensorData(const string& frame_id,
-             const ::cartographer::transform::Rigid3d& pose,
-             const ::cartographer::kalman_filter::PoseCovariance& covariance);
+  SensorData(const string& frame_id, const Odometry& odometry);
 
   SensorType type;
   string frame_id;
   ::cartographer::sensor::proto::Imu imu;
   ::cartographer::sensor::proto::LaserScan laser_scan;
   ::cartographer::sensor::proto::LaserFan3D laser_fan_3d;
-  struct {
-    ::cartographer::transform::Rigid3d pose;
-    ::cartographer::kalman_filter::PoseCovariance covariance;
-  } odometry;
+  Odometry odometry;
 };
 
 }  // namespace cartographer_ros

diff --git a/cartographer_ros/src/sensor_data_producer.cc b/cartographer_ros/src/sensor_data_producer.cc
@@ -29,8 +29,9 @@ SensorDataProducer::SensorDataProducer(
 void SensorDataProducer::AddOdometryMessage(
     const string& topic, const nav_msgs::Odometry::ConstPtr& msg) {
   auto sensor_data = ::cartographer::common::make_unique<SensorData>(
-      msg->header.frame_id, ToRigid3d(msg->pose.pose),
-      ToPoseCovariance(msg->pose.covariance));
+      msg->child_frame_id,
+      SensorData::Odometry{ToRigid3d(msg->pose.pose),
+                           ToPoseCovariance(msg->pose.covariance)});
   sensor_collator_->AddSensorData(
       trajectory_id_,
       ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic,