Rename the KalmanLocalTrajectoryBuilder. (#372)
Wolfgang Hess
GitHub
parent
e4e22e9f26
commit
ffc939706d
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -17,10 +17,8 @@
|
|||
#ifndef CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_
|
||||
#define CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_
|
||||
|
||||
#include <memory>
|
||||
|
||||
#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<KalmanLocalTrajectoryBuilder> local_trajectory_builder_;
|
||||
LocalTrajectoryBuilder local_trajectory_builder_;
|
||||
};
|
||||
|
||||
} // namespace mapping_3d
|
||||
|
|
|
|||
|
|
@ -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<scan_matching::RealTimeCorrelativeScanMatcher>(
|
||||
options_.kalman_local_trajectory_builder_options()
|
||||
.real_time_correlative_scan_matcher_options())),
|
||||
ceres_scan_matcher_(common::make_unique<scan_matching::CeresScanMatcher>(
|
||||
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<kalman_filter::PoseTracker>(
|
||||
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::InsertionResult>
|
||||
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<float>();
|
||||
accumulated_range_data_ =
|
||||
sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
|
||||
}
|
||||
|
||||
const transform::Rigid3f tracking_delta =
|
||||
first_pose_prediction_.inverse() * pose_prediction.cast<float>();
|
||||
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::InsertionResult>
|
||||
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<const Submap> 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<float>())};
|
||||
|
||||
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::InsertionResult>
|
||||
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<std::shared_ptr<const Submap>> insertion_submaps;
|
||||
for (std::shared_ptr<Submap> submap : active_submaps_.submaps()) {
|
||||
insertion_submaps.push_back(submap);
|
||||
}
|
||||
active_submaps_.InsertRangeData(
|
||||
sensor::TransformRangeData(range_data_in_tracking,
|
||||
pose_observation.cast<float>()),
|
||||
pose_tracker_->gravity_orientation());
|
||||
return std::unique_ptr<InsertionResult>(
|
||||
new InsertionResult{time, range_data_in_tracking, pose_observation,
|
||||
std::move(insertion_submaps)});
|
||||
}
|
||||
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
||||
|
|
@ -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 <memory>
|
||||
|
||||
#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<std::shared_ptr<const Submap>> 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<InsertionResult> 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<InsertionResult> AddAccumulatedRangeData(
|
||||
common::Time time, const sensor::RangeData& range_data_in_tracking);
|
||||
|
||||
std::unique_ptr<InsertionResult> 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<scan_matching::RealTimeCorrelativeScanMatcher>
|
||||
real_time_correlative_scan_matcher_;
|
||||
std::unique_ptr<scan_matching::CeresScanMatcher> ceres_scan_matcher_;
|
||||
|
||||
std::unique_ptr<kalman_filter::PoseTracker> 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_
|
||||
|
|
@ -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<KalmanLocalTrajectoryBuilder> CreateLocalTrajectoryBuilder(
|
||||
const proto::LocalTrajectoryBuilderOptions&
|
||||
local_trajectory_builder_options) {
|
||||
return common::make_unique<KalmanLocalTrajectoryBuilder>(
|
||||
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<scan_matching::RealTimeCorrelativeScanMatcher>(
|
||||
options_.kalman_local_trajectory_builder_options()
|
||||
.real_time_correlative_scan_matcher_options())),
|
||||
ceres_scan_matcher_(common::make_unique<scan_matching::CeresScanMatcher>(
|
||||
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<kalman_filter::PoseTracker>(
|
||||
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::InsertionResult>
|
||||
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<float>();
|
||||
accumulated_range_data_ =
|
||||
sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
|
||||
}
|
||||
|
||||
const transform::Rigid3f tracking_delta =
|
||||
first_pose_prediction_.inverse() * pose_prediction.cast<float>();
|
||||
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::InsertionResult>
|
||||
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<const Submap> 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<float>())};
|
||||
|
||||
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::InsertionResult>
|
||||
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<std::shared_ptr<const Submap>> insertion_submaps;
|
||||
for (std::shared_ptr<Submap> submap : active_submaps_.submaps()) {
|
||||
insertion_submaps.push_back(submap);
|
||||
}
|
||||
active_submaps_.InsertRangeData(
|
||||
sensor::TransformRangeData(range_data_in_tracking,
|
||||
pose_observation.cast<float>()),
|
||||
pose_tracker_->gravity_orientation());
|
||||
return std::unique_ptr<InsertionResult>(
|
||||
new InsertionResult{time, range_data_in_tracking, pose_observation,
|
||||
std::move(insertion_submaps)});
|
||||
}
|
||||
|
||||
} // namespace mapping_3d
|
||||
|
|
|
|||
|
|
@ -18,17 +18,77 @@
|
|||
#define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#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<KalmanLocalTrajectoryBuilder> 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<std::shared_ptr<const Submap>> 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<InsertionResult> 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<InsertionResult> AddAccumulatedRangeData(
|
||||
common::Time time, const sensor::RangeData& range_data_in_tracking);
|
||||
|
||||
std::unique_ptr<InsertionResult> 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<scan_matching::RealTimeCorrelativeScanMatcher>
|
||||
real_time_correlative_scan_matcher_;
|
||||
std::unique_ptr<scan_matching::CeresScanMatcher> ceres_scan_matcher_;
|
||||
|
||||
std::unique_ptr<kalman_filter::PoseTracker> pose_tracker_;
|
||||
|
||||
int num_accumulated_;
|
||||
transform::Rigid3f first_pose_prediction_;
|
||||
sensor::RangeData accumulated_range_data_;
|
||||
};
|
||||
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
||||
|
|
|
|||
|
|
@ -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 <memory>
|
||||
#include <random>
|
||||
|
|
@ -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<KalmanLocalTrajectoryBuilder> local_trajectory_builder_;
|
||||
std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;
|
||||
std::vector<Eigen::Vector3f> bubbles_;
|
||||
};
|
||||
|
||||
TEST_F(KalmanLocalTrajectoryBuilderTest,
|
||||
MoveInsideCubeUsingOnlyCeresScanMatcher) {
|
||||
TEST_F(LocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) {
|
||||
local_trajectory_builder_.reset(
|
||||
new KalmanLocalTrajectoryBuilder(CreateTrajectoryBuilderOptions()));
|
||||
new LocalTrajectoryBuilder(CreateTrajectoryBuilderOptions()));
|
||||
VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue