12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

master
Damon Kohler 2016-10-10 13:29:37 +02:00 committed by GitHub
parent a02dfa084a
commit 2a040bc755
7 changed files with 80 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cartographer_ros/CMakeLists.txt
View File

@ -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

10
cartographer_ros/src/cartographer_node_main.cc
View File

@ -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();
}

115
cartographer_ros/src/msg_conversion.cc
View File

@ -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<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.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;
}

12
cartographer_ros/src/msg_conversion.h
View File

@ -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<double, 36>& covariance);

8
cartographer_ros/src/sensor_bridge.cc
View File

@ -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,

59
cartographer_ros/src/sensor_data.cc
View File

@ -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

44
cartographer_ros/src/sensor_data.h
View File

@ -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