Transforms odometry into the tracking frame. (#65)
parent
45d18d53eb
commit
534f0119c4
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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,
|
||||
|
|
Loading…
Reference in New Issue