diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index 21141b0..4dd55e0 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -16,8 +16,6 @@ #include "cartographer/mapping_3d/global_trajectory_builder.h" -#include "cartographer/mapping_3d/local_trajectory_builder.h" - namespace cartographer { namespace mapping_3d { @@ -26,15 +24,15 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder( const int trajectory_id, SparsePoseGraph* sparse_pose_graph) : trajectory_id_(trajectory_id), sparse_pose_graph_(sparse_pose_graph), - local_trajectory_builder_(CreateLocalTrajectoryBuilder(options)) {} + local_trajectory_builder_(options) {} GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {} void GlobalTrajectoryBuilder::AddImuData( const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { - local_trajectory_builder_->AddImuData(time, linear_acceleration, - angular_velocity); + local_trajectory_builder_.AddImuData(time, linear_acceleration, + angular_velocity); sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration, angular_velocity); } @@ -43,7 +41,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData( const common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) { auto insertion_result = - local_trajectory_builder_->AddRangefinderData(time, origin, ranges); + local_trajectory_builder_.AddRangefinderData(time, origin, ranges); if (insertion_result == nullptr) { return; } @@ -55,12 +53,12 @@ void GlobalTrajectoryBuilder::AddRangefinderData( void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, const transform::Rigid3d& pose) { - local_trajectory_builder_->AddOdometerData(time, pose); + local_trajectory_builder_.AddOdometerData(time, pose); } const GlobalTrajectoryBuilder::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const { - return local_trajectory_builder_->pose_estimate(); + return local_trajectory_builder_.pose_estimate(); } } // namespace mapping_3d diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h index a0513b8..5e229fc 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ b/cartographer/mapping_3d/global_trajectory_builder.h @@ -17,10 +17,8 @@ #ifndef CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_ #define CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_ -#include - #include "cartographer/mapping/global_trajectory_builder_interface.h" -#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h" +#include "cartographer/mapping_3d/local_trajectory_builder.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/sparse_pose_graph.h" @@ -49,7 +47,7 @@ class GlobalTrajectoryBuilder private: const int trajectory_id_; mapping_3d::SparsePoseGraph* const sparse_pose_graph_; - std::unique_ptr local_trajectory_builder_; + LocalTrajectoryBuilder local_trajectory_builder_; }; } // namespace mapping_3d diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc deleted file mode 100644 index 8a51be8..0000000 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ /dev/null @@ -1,225 +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 "cartographer/mapping_3d/kalman_local_trajectory_builder.h" - -#include "cartographer/common/make_unique.h" -#include "cartographer/common/time.h" -#include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h" -#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" -#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" -#include "cartographer/mapping_3d/proto/submaps_options.pb.h" -#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" -#include "glog/logging.h" - -namespace cartographer { -namespace mapping_3d { - -KalmanLocalTrajectoryBuilder::KalmanLocalTrajectoryBuilder( - 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( - options_.kalman_local_trajectory_builder_options() - .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()), - accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {} - -KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {} - -void KalmanLocalTrajectoryBuilder::AddImuData( - const common::Time time, const Eigen::Vector3d& linear_acceleration, - const Eigen::Vector3d& angular_velocity) { - if (!pose_tracker_) { - pose_tracker_ = common::make_unique( - options_.kalman_local_trajectory_builder_options() - .pose_tracker_options(), - time); - } - - pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration); - pose_tracker_->AddImuAngularVelocityObservation(time, angular_velocity); -} - -std::unique_ptr -KalmanLocalTrajectoryBuilder::AddRangefinderData( - const common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges) { - if (!pose_tracker_) { - LOG(INFO) << "PoseTracker not yet initialized."; - return nullptr; - } - - const transform::Rigid3d pose_prediction = - pose_tracker_->GetPoseEstimateMean(time); - if (num_accumulated_ == 0) { - first_pose_prediction_ = pose_prediction.cast(); - accumulated_range_data_ = - sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}; - } - - const transform::Rigid3f tracking_delta = - first_pose_prediction_.inverse() * pose_prediction.cast(); - const sensor::RangeData range_data_in_first_tracking = - sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}}, - tracking_delta); - for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) { - const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin; - const float range = delta.norm(); - if (range >= options_.min_range()) { - if (range <= options_.max_range()) { - accumulated_range_data_.returns.push_back(hit); - } else { - // We insert a ray cropped to 'max_range' as a miss for hits beyond the - // maximum range. This way the free space up to the maximum range will - // be updated. - accumulated_range_data_.misses.push_back( - range_data_in_first_tracking.origin + - options_.max_range() / range * delta); - } - } - } - ++num_accumulated_; - - if (num_accumulated_ >= options_.scans_per_accumulation()) { - num_accumulated_ = 0; - return AddAccumulatedRangeData( - time, sensor::TransformRangeData(accumulated_range_data_, - tracking_delta.inverse())); - } - return nullptr; -} - -std::unique_ptr -KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData( - const common::Time time, const sensor::RangeData& range_data_in_tracking) { - const sensor::RangeData filtered_range_data = { - range_data_in_tracking.origin, - sensor::VoxelFiltered(range_data_in_tracking.returns, - options_.voxel_filter_size()), - sensor::VoxelFiltered(range_data_in_tracking.misses, - options_.voxel_filter_size())}; - - if (filtered_range_data.returns.empty()) { - LOG(WARNING) << "Dropped empty range data."; - return nullptr; - } - - const transform::Rigid3d pose_prediction = - pose_tracker_->GetPoseEstimateMean(time); - - std::shared_ptr matching_submap = - active_submaps_.submaps().front(); - transform::Rigid3d initial_ceres_pose = - matching_submap->local_pose().inverse() * pose_prediction; - sensor::AdaptiveVoxelFilter adaptive_voxel_filter( - options_.high_resolution_adaptive_voxel_filter_options()); - const sensor::PointCloud filtered_point_cloud_in_tracking = - adaptive_voxel_filter.Filter(filtered_range_data.returns); - if (options_.kalman_local_trajectory_builder_options() - .use_online_correlative_scan_matching()) { - // We take a copy since we use 'intial_ceres_pose' as an output argument. - const transform::Rigid3d initial_pose = initial_ceres_pose; - real_time_correlative_scan_matcher_->Match( - initial_pose, filtered_point_cloud_in_tracking, - matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose); - } - - transform::Rigid3d pose_observation_in_submap; - ceres::Solver::Summary summary; - - sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( - options_.low_resolution_adaptive_voxel_filter_options()); - 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_, - 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()); - - scan_matcher_pose_estimate_ = pose_tracker_->GetPoseEstimateMean(time); - - last_pose_estimate_ = { - time, scan_matcher_pose_estimate_, - sensor::TransformPointCloud(filtered_range_data.returns, - pose_observation.cast())}; - - return InsertIntoSubmap(time, filtered_range_data, pose_observation); -} - -void KalmanLocalTrajectoryBuilder::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)); - } - 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())); -} - -const KalmanLocalTrajectoryBuilder::PoseEstimate& -KalmanLocalTrajectoryBuilder::pose_estimate() const { - return last_pose_estimate_; -} - -std::unique_ptr -KalmanLocalTrajectoryBuilder::InsertIntoSubmap( - const common::Time time, const sensor::RangeData& range_data_in_tracking, - const transform::Rigid3d& pose_observation) { - if (motion_filter_.IsSimilar(time, pose_observation)) { - return nullptr; - } - // Querying the active submaps must be done here before calling - // InsertRangeData() since the queried values are valid for next insertion. - std::vector> insertion_submaps; - for (std::shared_ptr submap : active_submaps_.submaps()) { - insertion_submaps.push_back(submap); - } - active_submaps_.InsertRangeData( - sensor::TransformRangeData(range_data_in_tracking, - pose_observation.cast()), - pose_tracker_->gravity_orientation()); - return std::unique_ptr( - new InsertionResult{time, range_data_in_tracking, pose_observation, - std::move(insertion_submaps)}); -} - -} // namespace mapping_3d -} // namespace cartographer diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.h b/cartographer/mapping_3d/kalman_local_trajectory_builder.h deleted file mode 100644 index 775270b..0000000 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.h +++ /dev/null @@ -1,97 +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. - */ - -#ifndef CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_H_ -#define CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_H_ - -#include - -#include "cartographer/common/time.h" -#include "cartographer/kalman_filter/pose_tracker.h" -#include "cartographer/mapping/global_trajectory_builder_interface.h" -#include "cartographer/mapping_3d/motion_filter.h" -#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" -#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" -#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" -#include "cartographer/mapping_3d/submaps.h" -#include "cartographer/sensor/range_data.h" -#include "cartographer/sensor/voxel_filter.h" -#include "cartographer/transform/rigid_transform.h" - -namespace cartographer { -namespace mapping_3d { - -// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop -// closure. -class KalmanLocalTrajectoryBuilder { - public: - using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate; - - struct InsertionResult { - common::Time time; - sensor::RangeData range_data_in_tracking; - transform::Rigid3d pose_observation; - std::vector> insertion_submaps; - }; - - explicit KalmanLocalTrajectoryBuilder( - const proto::LocalTrajectoryBuilderOptions& options); - ~KalmanLocalTrajectoryBuilder(); - - KalmanLocalTrajectoryBuilder(const KalmanLocalTrajectoryBuilder&) = delete; - KalmanLocalTrajectoryBuilder& operator=(const KalmanLocalTrajectoryBuilder&) = - delete; - - void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, - const Eigen::Vector3d& angular_velocity); - std::unique_ptr AddRangefinderData( - common::Time time, const Eigen::Vector3f& origin, - const sensor::PointCloud& ranges); - void AddOdometerData(common::Time time, const transform::Rigid3d& pose); - const PoseEstimate& pose_estimate() const; - - private: - std::unique_ptr AddAccumulatedRangeData( - common::Time time, const sensor::RangeData& range_data_in_tracking); - - std::unique_ptr InsertIntoSubmap( - common::Time time, const sensor::RangeData& range_data_in_tracking, - const transform::Rigid3d& pose_observation); - - const proto::LocalTrajectoryBuilderOptions options_; - mapping_3d::ActiveSubmaps active_submaps_; - - 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_; - - int num_accumulated_; - transform::Rigid3f first_pose_prediction_; - sensor::RangeData accumulated_range_data_; -}; - -} // namespace mapping_3d -} // namespace cartographer - -#endif // CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_H_ diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index 2d093de..e28b16e 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -17,15 +17,208 @@ #include "cartographer/mapping_3d/local_trajectory_builder.h" #include "cartographer/common/make_unique.h" +#include "cartographer/common/time.h" +#include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h" +#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" +#include "cartographer/mapping_3d/proto/submaps_options.pb.h" +#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" +#include "glog/logging.h" namespace cartographer { namespace mapping_3d { -std::unique_ptr CreateLocalTrajectoryBuilder( - const proto::LocalTrajectoryBuilderOptions& - local_trajectory_builder_options) { - return common::make_unique( - local_trajectory_builder_options); +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( + options_.kalman_local_trajectory_builder_options() + .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()), + accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {} + +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( + options_.kalman_local_trajectory_builder_options() + .pose_tracker_options(), + time); + } + + pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration); + pose_tracker_->AddImuAngularVelocityObservation(time, 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."; + return nullptr; + } + + const transform::Rigid3d pose_prediction = + pose_tracker_->GetPoseEstimateMean(time); + if (num_accumulated_ == 0) { + first_pose_prediction_ = pose_prediction.cast(); + accumulated_range_data_ = + sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}; + } + + const transform::Rigid3f tracking_delta = + first_pose_prediction_.inverse() * pose_prediction.cast(); + const sensor::RangeData range_data_in_first_tracking = + sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}}, + tracking_delta); + for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) { + const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin; + const float range = delta.norm(); + if (range >= options_.min_range()) { + if (range <= options_.max_range()) { + accumulated_range_data_.returns.push_back(hit); + } else { + // We insert a ray cropped to 'max_range' as a miss for hits beyond the + // maximum range. This way the free space up to the maximum range will + // be updated. + accumulated_range_data_.misses.push_back( + range_data_in_first_tracking.origin + + options_.max_range() / range * delta); + } + } + } + ++num_accumulated_; + + if (num_accumulated_ >= options_.scans_per_accumulation()) { + num_accumulated_ = 0; + return AddAccumulatedRangeData( + time, sensor::TransformRangeData(accumulated_range_data_, + tracking_delta.inverse())); + } + return nullptr; +} + +std::unique_ptr +LocalTrajectoryBuilder::AddAccumulatedRangeData( + const common::Time time, const sensor::RangeData& range_data_in_tracking) { + const sensor::RangeData filtered_range_data = { + range_data_in_tracking.origin, + sensor::VoxelFiltered(range_data_in_tracking.returns, + options_.voxel_filter_size()), + sensor::VoxelFiltered(range_data_in_tracking.misses, + options_.voxel_filter_size())}; + + if (filtered_range_data.returns.empty()) { + LOG(WARNING) << "Dropped empty range data."; + return nullptr; + } + + const transform::Rigid3d pose_prediction = + pose_tracker_->GetPoseEstimateMean(time); + + std::shared_ptr matching_submap = + active_submaps_.submaps().front(); + transform::Rigid3d initial_ceres_pose = + matching_submap->local_pose().inverse() * pose_prediction; + sensor::AdaptiveVoxelFilter adaptive_voxel_filter( + options_.high_resolution_adaptive_voxel_filter_options()); + const sensor::PointCloud filtered_point_cloud_in_tracking = + adaptive_voxel_filter.Filter(filtered_range_data.returns); + if (options_.kalman_local_trajectory_builder_options() + .use_online_correlative_scan_matching()) { + // We take a copy since we use 'intial_ceres_pose' as an output argument. + const transform::Rigid3d initial_pose = initial_ceres_pose; + real_time_correlative_scan_matcher_->Match( + initial_pose, filtered_point_cloud_in_tracking, + matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose); + } + + transform::Rigid3d pose_observation_in_submap; + ceres::Solver::Summary summary; + + sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( + options_.low_resolution_adaptive_voxel_filter_options()); + 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_, + 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()); + + scan_matcher_pose_estimate_ = pose_tracker_->GetPoseEstimateMean(time); + + last_pose_estimate_ = { + time, scan_matcher_pose_estimate_, + sensor::TransformPointCloud(filtered_range_data.returns, + pose_observation.cast())}; + + return InsertIntoSubmap(time, filtered_range_data, pose_observation); +} + +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)); + } + 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())); +} + +const LocalTrajectoryBuilder::PoseEstimate& +LocalTrajectoryBuilder::pose_estimate() const { + return last_pose_estimate_; +} + +std::unique_ptr +LocalTrajectoryBuilder::InsertIntoSubmap( + const common::Time time, const sensor::RangeData& range_data_in_tracking, + const transform::Rigid3d& pose_observation) { + if (motion_filter_.IsSimilar(time, pose_observation)) { + return nullptr; + } + // Querying the active submaps must be done here before calling + // InsertRangeData() since the queried values are valid for next insertion. + std::vector> insertion_submaps; + for (std::shared_ptr submap : active_submaps_.submaps()) { + insertion_submaps.push_back(submap); + } + active_submaps_.InsertRangeData( + sensor::TransformRangeData(range_data_in_tracking, + pose_observation.cast()), + pose_tracker_->gravity_orientation()); + return std::unique_ptr( + new InsertionResult{time, range_data_in_tracking, pose_observation, + std::move(insertion_submaps)}); } } // namespace mapping_3d diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index 5338452..caa5d24 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -18,17 +18,77 @@ #define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ #include -#include -#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h" +#include "cartographer/common/time.h" +#include "cartographer/kalman_filter/pose_tracker.h" +#include "cartographer/mapping/global_trajectory_builder_interface.h" +#include "cartographer/mapping_3d/motion_filter.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" +#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" +#include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping_3d/submaps.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/sensor/voxel_filter.h" +#include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping_3d { -std::unique_ptr CreateLocalTrajectoryBuilder( - const proto::LocalTrajectoryBuilderOptions& - local_trajectory_builder_options); +// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop +// closure. +class LocalTrajectoryBuilder { + public: + using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate; + + struct InsertionResult { + common::Time time; + sensor::RangeData range_data_in_tracking; + transform::Rigid3d pose_observation; + std::vector> insertion_submaps; + }; + + explicit LocalTrajectoryBuilder( + const proto::LocalTrajectoryBuilderOptions& options); + ~LocalTrajectoryBuilder(); + + LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete; + LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete; + + void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, + const Eigen::Vector3d& angular_velocity); + std::unique_ptr AddRangefinderData( + common::Time time, const Eigen::Vector3f& origin, + const sensor::PointCloud& ranges); + void AddOdometerData(common::Time time, const transform::Rigid3d& pose); + const PoseEstimate& pose_estimate() const; + + private: + std::unique_ptr AddAccumulatedRangeData( + common::Time time, const sensor::RangeData& range_data_in_tracking); + + std::unique_ptr InsertIntoSubmap( + common::Time time, const sensor::RangeData& range_data_in_tracking, + const transform::Rigid3d& pose_observation); + + const proto::LocalTrajectoryBuilderOptions options_; + mapping_3d::ActiveSubmaps active_submaps_; + + 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_; + + int num_accumulated_; + transform::Rigid3f first_pose_prediction_; + sensor::RangeData accumulated_range_data_; +}; } // namespace mapping_3d } // namespace cartographer diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc b/cartographer/mapping_3d/local_trajectory_builder_test.cc similarity index 96% rename from cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc rename to cartographer/mapping_3d/local_trajectory_builder_test.cc index fafef74..0688b70 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/local_trajectory_builder_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h" +#include "cartographer/mapping_3d/local_trajectory_builder.h" #include #include @@ -35,7 +35,7 @@ namespace cartographer { namespace mapping_3d { namespace { -class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { +class LocalTrajectoryBuilderTest : public ::testing::Test { protected: struct TrajectoryNode { common::Time time; @@ -258,14 +258,13 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { } } - std::unique_ptr local_trajectory_builder_; + std::unique_ptr local_trajectory_builder_; std::vector bubbles_; }; -TEST_F(KalmanLocalTrajectoryBuilderTest, - MoveInsideCubeUsingOnlyCeresScanMatcher) { +TEST_F(LocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) { local_trajectory_builder_.reset( - new KalmanLocalTrajectoryBuilder(CreateTrajectoryBuilderOptions())); + new LocalTrajectoryBuilder(CreateTrajectoryBuilderOptions())); VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1); }