diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index e28b16e..07238ea 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -32,7 +32,6 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options) : options_(options), active_submaps_(options.submaps_options()), - scan_matcher_pose_estimate_(transform::Rigid3d::Identity()), motion_filter_(options.motion_filter_options()), real_time_correlative_scan_matcher_( common::make_unique( @@ -40,8 +39,9 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder( .real_time_correlative_scan_matcher_options())), ceres_scan_matcher_(common::make_unique( options_.ceres_scan_matcher_options())), - num_accumulated_(0), - first_pose_prediction_(transform::Rigid3f::Identity()), + odometry_state_tracker_(options_.kalman_local_trajectory_builder_options() + .pose_tracker_options() + .num_odometry_states()), accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {} LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {} @@ -49,36 +49,40 @@ LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {} void LocalTrajectoryBuilder::AddImuData( const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { - if (!pose_tracker_) { - pose_tracker_ = common::make_unique( + if (!imu_tracker_) { + imu_tracker_ = common::make_unique( options_.kalman_local_trajectory_builder_options() - .pose_tracker_options(), + .pose_tracker_options() + .imu_gravity_time_constant(), time); + // Use the accelerometer to gravity align the first pose. + imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration); + pose_estimate_ = transform::Rigid3d::Rotation(imu_tracker_->orientation()); } - pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration); - pose_tracker_->AddImuAngularVelocityObservation(time, angular_velocity); + Predict(time); + imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration); + imu_tracker_->AddImuAngularVelocityObservation(angular_velocity); } std::unique_ptr LocalTrajectoryBuilder::AddRangefinderData(const common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) { - if (!pose_tracker_) { - LOG(INFO) << "PoseTracker not yet initialized."; + if (!imu_tracker_) { + LOG(INFO) << "ImuTracker not yet initialized."; return nullptr; } - const transform::Rigid3d pose_prediction = - pose_tracker_->GetPoseEstimateMean(time); + Predict(time); if (num_accumulated_ == 0) { - first_pose_prediction_ = pose_prediction.cast(); + first_pose_estimate_ = pose_estimate_.cast(); accumulated_range_data_ = sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}; } const transform::Rigid3f tracking_delta = - first_pose_prediction_.inverse() * pose_prediction.cast(); + first_pose_estimate_.inverse() * pose_estimate_.cast(); const sensor::RangeData range_data_in_first_tracking = sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}}, tracking_delta); @@ -109,6 +113,24 @@ LocalTrajectoryBuilder::AddRangefinderData(const common::Time time, return nullptr; } +void LocalTrajectoryBuilder::Predict(const common::Time time) { + CHECK(imu_tracker_ != nullptr); + CHECK_LE(time_, time); + const Eigen::Quaterniond last_orientation = imu_tracker_->orientation(); + imu_tracker_->Advance(time); + if (time_ > common::Time::min()) { + const double delta_t = common::ToSeconds(time - time_); + // Constant velocity model. + const Eigen::Vector3d translation = + pose_estimate_.translation() + delta_t * velocity_estimate_; + const Eigen::Quaterniond rotation = pose_estimate_.rotation() * + last_orientation.inverse() * + imu_tracker_->orientation(); + pose_estimate_ = transform::Rigid3d(translation, rotation); + } + time_ = time; +} + std::unique_ptr LocalTrajectoryBuilder::AddAccumulatedRangeData( const common::Time time, const sensor::RangeData& range_data_in_tracking) { @@ -124,8 +146,11 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( return nullptr; } - const transform::Rigid3d pose_prediction = - pose_tracker_->GetPoseEstimateMean(time); + Predict(time); + const transform::Rigid3d odometry_prediction = + pose_estimate_ * odometry_correction_; + const transform::Rigid3d model_prediction = pose_estimate_; + const transform::Rigid3d& pose_prediction = odometry_prediction; std::shared_ptr matching_submap = active_submaps_.submaps().front(); @@ -152,46 +177,62 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData( const sensor::PointCloud low_resolution_point_cloud_in_tracking = low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns); ceres_scan_matcher_->Match( - matching_submap->local_pose().inverse() * scan_matcher_pose_estimate_, + matching_submap->local_pose().inverse() * pose_prediction, initial_ceres_pose, {{&filtered_point_cloud_in_tracking, &matching_submap->high_resolution_hybrid_grid()}, {&low_resolution_point_cloud_in_tracking, &matching_submap->low_resolution_hybrid_grid()}}, &pose_observation_in_submap, &summary); - const transform::Rigid3d pose_observation = - matching_submap->local_pose() * pose_observation_in_submap; - pose_tracker_->AddPoseObservation( - time, pose_observation, - options_.kalman_local_trajectory_builder_options() - .scan_matcher_variance() * - kalman_filter::PoseCovariance::Identity()); + pose_estimate_ = matching_submap->local_pose() * pose_observation_in_submap; - scan_matcher_pose_estimate_ = pose_tracker_->GetPoseEstimateMean(time); + odometry_correction_ = transform::Rigid3d::Identity(); + if (!odometry_state_tracker_.empty()) { + // We add an odometry state, so that the correction from the scan matching + // is not removed by the next odometry data we get. + odometry_state_tracker_.AddOdometryState( + {time, odometry_state_tracker_.newest().odometer_pose, + odometry_state_tracker_.newest().state_pose * + odometry_prediction.inverse() * pose_estimate_}); + } + + // Improve the velocity estimate. + if (last_scan_match_time_ > common::Time::min() && + time > last_scan_match_time_) { + const double delta_t = common::ToSeconds(time - last_scan_match_time_); + velocity_estimate_ += + (pose_estimate_.translation() - model_prediction.translation()) / + delta_t; + } + last_scan_match_time_ = time_; last_pose_estimate_ = { - time, scan_matcher_pose_estimate_, + time, pose_estimate_, sensor::TransformPointCloud(filtered_range_data.returns, - pose_observation.cast())}; + pose_estimate_.cast())}; - return InsertIntoSubmap(time, filtered_range_data, pose_observation); + return InsertIntoSubmap(time, filtered_range_data, pose_estimate_); } -void LocalTrajectoryBuilder::AddOdometerData(const common::Time time, - const transform::Rigid3d& pose) { - if (!pose_tracker_) { - pose_tracker_.reset(new kalman_filter::PoseTracker( - options_.kalman_local_trajectory_builder_options() - .pose_tracker_options(), - time)); +void LocalTrajectoryBuilder::AddOdometerData( + const common::Time time, const transform::Rigid3d& odometer_pose) { + if (imu_tracker_ == nullptr) { + // Until we've initialized the IMU tracker we do not want to call Predict(). + LOG(INFO) << "ImuTracker not yet initialized."; + return; } - pose_tracker_->AddOdometerPoseObservation( - time, pose, - kalman_filter::BuildPoseCovariance( - options_.kalman_local_trajectory_builder_options() - .odometer_translational_variance(), - options_.kalman_local_trajectory_builder_options() - .odometer_rotational_variance())); + + Predict(time); + if (!odometry_state_tracker_.empty()) { + const auto& previous_odometry_state = odometry_state_tracker_.newest(); + const transform::Rigid3d delta = + previous_odometry_state.odometer_pose.inverse() * odometer_pose; + const transform::Rigid3d new_pose = + previous_odometry_state.state_pose * delta; + odometry_correction_ = pose_estimate_.inverse() * new_pose; + } + odometry_state_tracker_.AddOdometryState( + {time, odometer_pose, pose_estimate_ * odometry_correction_}); } const LocalTrajectoryBuilder::PoseEstimate& @@ -215,7 +256,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap( active_submaps_.InsertRangeData( sensor::TransformRangeData(range_data_in_tracking, pose_observation.cast()), - pose_tracker_->gravity_orientation()); + imu_tracker_->orientation()); return std::unique_ptr( new InsertionResult{time, range_data_in_tracking, pose_observation, std::move(insertion_submaps)}); diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index caa5d24..8306110 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -59,10 +59,13 @@ class LocalTrajectoryBuilder { std::unique_ptr AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges); - void AddOdometerData(common::Time time, const transform::Rigid3d& pose); + void AddOdometerData(common::Time time, + const transform::Rigid3d& odometer_pose); const PoseEstimate& pose_estimate() const; private: + void Predict(common::Time time); + std::unique_ptr AddAccumulatedRangeData( common::Time time, const sensor::RangeData& range_data_in_tracking); @@ -75,18 +78,25 @@ class LocalTrajectoryBuilder { PoseEstimate last_pose_estimate_; - // Pose of the last computed scan match. - transform::Rigid3d scan_matcher_pose_estimate_; - MotionFilter motion_filter_; std::unique_ptr real_time_correlative_scan_matcher_; std::unique_ptr ceres_scan_matcher_; - std::unique_ptr pose_tracker_; + // Current 'pose_estimate_' and 'velocity_estimate_' at 'time_'. + common::Time time_ = common::Time::min(); + transform::Rigid3d pose_estimate_ = transform::Rigid3d::Identity(); + Eigen::Vector3d velocity_estimate_ = Eigen::Vector3d::Zero(); + common::Time last_scan_match_time_ = common::Time::min(); + // This is the difference between the model (constant velocity, IMU) + // prediction 'pose_estimate_' and the odometry prediction. To get the + // odometry prediction, right-multiply this to 'pose_estimate_'. + transform::Rigid3d odometry_correction_ = transform::Rigid3d::Identity(); + std::unique_ptr imu_tracker_; + mapping::OdometryStateTracker odometry_state_tracker_; - int num_accumulated_; - transform::Rigid3f first_pose_prediction_; + int num_accumulated_ = 0; + transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity(); sensor::RangeData accumulated_range_data_; }; diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 3194c28..825f392 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -33,10 +33,10 @@ TRAJECTORY_BUILDER_3D = { }, ceres_scan_matcher = { - occupied_space_weight_0 = 5., - occupied_space_weight_1 = 30., - translation_weight = 0.3, - rotation_weight = 2e3, + occupied_space_weight_0 = 1., + occupied_space_weight_1 = 6., + translation_weight = 5., + rotation_weight = 4e2, only_optimize_yaw = false, ceres_solver_options = { use_nonmonotonic_steps = false,