From 2a040bc75526bf6238faa22f8bfa9067d9d55b90 Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Mon, 10 Oct 2016 13:29:37 +0200 Subject: [PATCH] Directly use Eigen representations of IMU data. (#91) --- cartographer_ros/CMakeLists.txt | 1 - .../src/cartographer_node_main.cc | 10 +- cartographer_ros/src/msg_conversion.cc | 115 +++++------------- cartographer_ros/src/msg_conversion.h | 12 +- cartographer_ros/src/sensor_bridge.cc | 8 +- cartographer_ros/src/sensor_data.cc | 59 --------- cartographer_ros/src/sensor_data.h | 44 +++++-- 7 files changed, 80 insertions(+), 169 deletions(-) delete mode 100644 cartographer_ros/src/sensor_data.cc diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 4511a3c..a5d0fe2 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/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 diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 1b38532..66fc05f 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/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() * - carto::transform::ToEigen(imu.linear_acceleration()), - sensor_to_tracking.rotation() * - carto::transform::ToEigen(imu.angular_velocity())); + sensor_to_tracking.rotation() * imu.linear_acceleration, + sensor_to_tracking.rotation() * imu.angular_velocity); } catch (const tf2::TransformException& ex) { LOG(WARNING) << ex.what(); } diff --git a/cartographer_ros/src/msg_conversion.cc b/cartographer_ros/src/msg_conversion.cc index dbf2991..ed2edfd 100644 --- a/cartographer_ros/src/msg_conversion.cc +++ b/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, - transform.transform.translation.y, - transform.transform.translation.z), - Eigen::Quaterniond(transform.transform.rotation.w, - transform.transform.rotation.x, - transform.transform.rotation.y, - transform.transform.rotation.z)); + return Rigid3d(ToEigen(transform.transform.translation), + ToEigen(transform.transform.rotation)); } Rigid3d ToRigid3d(const geometry_msgs::Pose& pose) { - return Rigid3d( - Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z), - Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, - pose.orientation.y, pose.orientation.z)); + return Rigid3d({pose.position.x, pose.position.y, pose.position.z}, + ToEigen(pose.orientation)); +} + +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& covariance) { return Eigen::Map>(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.y = rigid.translation().y(); - transform.translation.z = rigid.translation().z(); - transform.rotation.w = rigid.rotation().w(); - transform.rotation.x = rigid.rotation().x(); - transform.rotation.y = rigid.rotation().y(); - transform.rotation.z = rigid.rotation().z(); + transform.translation.x = rigid3d.translation().x(); + transform.translation.y = rigid3d.translation().y(); + transform.translation.z = rigid3d.translation().z(); + transform.rotation.w = rigid3d.rotation().w(); + transform.rotation.x = rigid3d.rotation().x(); + transform.rotation.y = rigid3d.rotation().y(); + 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.y = rigid.translation().y(); - pose.position.z = rigid.translation().z(); - pose.orientation.w = rigid.rotation().w(); - pose.orientation.x = rigid.rotation().x(); - pose.orientation.y = rigid.rotation().y(); - pose.orientation.z = rigid.rotation().z(); + pose.position.x = rigid3d.translation().x(); + pose.position.y = rigid3d.translation().y(); + pose.position.z = rigid3d.translation().z(); + pose.orientation.w = rigid3d.rotation().w(); + pose.orientation.x = rigid3d.rotation().x(); + pose.orientation.y = rigid3d.rotation().y(); + pose.orientation.z = rigid3d.rotation().z(); return pose; } diff --git a/cartographer_ros/src/msg_conversion.h b/cartographer_ros/src/msg_conversion.h index 1e5f976..a14b18e 100644 --- a/cartographer_ros/src/msg_conversion.h +++ b/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); - -::cartographer::sensor::proto::Imu ToCartographer(const sensor_msgs::Imu& msg); + const ::cartographer::transform::Rigid3d& rigid3d); ::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& covariance); diff --git a/cartographer_ros/src/sensor_bridge.cc b/cartographer_ros/src/sensor_bridge.cc index ca4d8ae..e9c7b87 100644 --- a/cartographer_ros/src/sensor_bridge.cc +++ b/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( - 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, diff --git a/cartographer_ros/src/sensor_data.cc b/cartographer_ros/src/sensor_data.cc deleted file mode 100644 index 0f38a22..0000000 --- a/cartographer_ros/src/sensor_data.cc +++ /dev/null @@ -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 - -#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 diff --git a/cartographer_ros/src/sensor_data.h b/cartographer_ros/src/sensor_data.h index 5651af7..ec89a2a 100644 --- a/cartographer_ros/src/sensor_data.h +++ b/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 -// is only used for time ordering sensor data before passing it on. +// This type is a logical union, i.e. only one type of sensor data is actually +// 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); - 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 Odometry& odometry); + struct Imu { + Eigen::Vector3d angular_velocity; + Eigen::Vector3d linear_acceleration; + }; + + 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