Transforms odometry into the tracking frame. (#65)

master
Damon Kohler 2016-09-20 08:42:56 +02:00 committed by GitHub
parent 45d18d53eb
commit 534f0119c4
4 changed files with 30 additions and 24 deletions

View File

@ -204,8 +204,8 @@ class Node {
void HandleSensorData(int64 timestamp, void HandleSensorData(int64 timestamp,
std::unique_ptr<SensorData> sensor_data); std::unique_ptr<SensorData> sensor_data);
void AddOdometry(int64 timestamp, const string& frame_id, const Rigid3d& pose, void AddOdometry(int64 timestamp, const string& frame_id,
const PoseCovariance& covariance); 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 proto::Imu& imu);
void AddHorizontalLaserFan(int64 timestamp, const string& frame_id, void AddHorizontalLaserFan(int64 timestamp, const string& frame_id,
const proto::LaserScan& laser_scan); const proto::LaserScan& laser_scan);
@ -286,10 +286,10 @@ Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time,
::ros::Duration(options_.lookup_transform_timeout_sec))); ::ros::Duration(options_.lookup_transform_timeout_sec)));
} }
void Node::AddOdometry(int64 timestamp, const string& frame_id, void Node::AddOdometry(const int64 timestamp, const string& frame_id,
const Rigid3d& pose, const PoseCovariance& covariance) { const SensorData::Odometry& odometry) {
const carto::common::Time time = carto::common::FromUniversal(timestamp); const carto::common::Time time = carto::common::FromUniversal(timestamp);
PoseCovariance applied_covariance = covariance; PoseCovariance applied_covariance = odometry.covariance;
if (options_.use_constant_odometry_variance) { if (options_.use_constant_odometry_variance) {
const Eigen::Matrix3d translational = const Eigen::Matrix3d translational =
Eigen::Matrix3d::Identity() * Eigen::Matrix3d::Identity() *
@ -303,8 +303,16 @@ void Node::AddOdometry(int64 timestamp, const string& frame_id,
Eigen::Matrix3d::Zero(), rotational; Eigen::Matrix3d::Zero(), rotational;
// clang-format on // clang-format on
} }
try {
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id);
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
->AddOdometerPose(time, pose, applied_covariance); ->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, 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); LookupToTrackingTransformOrThrow(time, frame_id);
CHECK(sensor_to_tracking.translation().norm() < 1e-5) CHECK(sensor_to_tracking.translation().norm() < 1e-5)
<< "The IMU frame must be colocated with the tracking frame. " << "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."; "otherwise be imprecise.";
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
->AddImuData(time, ->AddImuData(time,
@ -328,6 +336,7 @@ void Node::AddImu(const int64 timestamp, const string& frame_id,
<< options_.tracking_frame << ": " << ex.what(); << options_.tracking_frame << ": " << ex.what();
} }
} }
void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
const proto::LaserScan& laser_scan) { const proto::LaserScan& laser_scan) {
const carto::common::Time time = carto::common::FromUniversal(timestamp); const carto::common::Time time = carto::common::FromUniversal(timestamp);
@ -713,8 +722,7 @@ void Node::HandleSensorData(const int64 timestamp,
return; return;
case SensorType::kOdometry: case SensorType::kOdometry:
AddOdometry(timestamp, sensor_data->frame_id, sensor_data->odometry.pose, AddOdometry(timestamp, sensor_data->frame_id, sensor_data->odometry);
sensor_data->odometry.covariance);
return; return;
} }
LOG(FATAL); LOG(FATAL);

View File

@ -53,10 +53,7 @@ SensorData::SensorData(const string& frame_id, proto::LaserFan3D laser_fan_3d)
frame_id(CheckNoLeadingSlash(frame_id)), frame_id(CheckNoLeadingSlash(frame_id)),
laser_fan_3d(laser_fan_3d) {} laser_fan_3d(laser_fan_3d) {}
SensorData::SensorData(const string& frame_id, const Rigid3d& pose, SensorData::SensorData(const string& frame_id, const Odometry& odometry)
const PoseCovariance& covariance) : type(SensorType::kOdometry), frame_id(frame_id), odometry(odometry) {}
: type(SensorType::kOdometry),
frame_id(frame_id),
odometry{pose, covariance} {}
} // namespace cartorapher_ros } // namespace cartorapher_ros

View File

@ -29,24 +29,24 @@ namespace cartographer_ros {
// is only used for time ordering sensor data before passing it on. // is only used for time ordering sensor data before passing it on.
enum class SensorType { kImu, kLaserScan, kLaserFan3D, kOdometry }; enum class SensorType { kImu, kLaserScan, kLaserFan3D, kOdometry };
struct SensorData { 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::Imu imu);
SensorData(const string& frame_id, SensorData(const string& frame_id,
::cartographer::sensor::proto::LaserScan laser_scan); ::cartographer::sensor::proto::LaserScan laser_scan);
SensorData(const string& frame_id, SensorData(const string& frame_id,
::cartographer::sensor::proto::LaserFan3D laser_fan_3d); ::cartographer::sensor::proto::LaserFan3D laser_fan_3d);
SensorData(const string& frame_id, SensorData(const string& frame_id, const Odometry& odometry);
const ::cartographer::transform::Rigid3d& pose,
const ::cartographer::kalman_filter::PoseCovariance& covariance);
SensorType type; SensorType type;
string frame_id; string frame_id;
::cartographer::sensor::proto::Imu imu; ::cartographer::sensor::proto::Imu imu;
::cartographer::sensor::proto::LaserScan laser_scan; ::cartographer::sensor::proto::LaserScan laser_scan;
::cartographer::sensor::proto::LaserFan3D laser_fan_3d; ::cartographer::sensor::proto::LaserFan3D laser_fan_3d;
struct { Odometry odometry;
::cartographer::transform::Rigid3d pose;
::cartographer::kalman_filter::PoseCovariance covariance;
} odometry;
}; };
} // namespace cartographer_ros } // namespace cartographer_ros

View File

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