Remove the unused OptimizingLocalTrajectoryBuilder. (#368)
It has not been used for a long time and has to be replaced by a better implementation.master
Wolfgang Hess
GitHub
parent
010fefc204
commit
f88fcec851
|
|
@ -93,7 +93,6 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
|
|||
},
|
||||
},
|
||||
|
||||
use = "KALMAN",
|
||||
kalman_local_trajectory_builder = {
|
||||
use_online_correlative_scan_matching = false,
|
||||
real_time_correlative_scan_matcher = {
|
||||
|
|
@ -114,15 +113,6 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
|
|||
odometer_translational_variance = 1e-7,
|
||||
odometer_rotational_variance = 1e-7,
|
||||
},
|
||||
optimizing_local_trajectory_builder = {
|
||||
high_resolution_grid_weight = 5.,
|
||||
low_resolution_grid_weight = 15.,
|
||||
velocity_weight = 4e1,
|
||||
translation_weight = 1e2,
|
||||
rotation_weight = 1e3,
|
||||
odometry_translation_weight = 1e4,
|
||||
odometry_rotation_weight = 1e4,
|
||||
},
|
||||
}
|
||||
)text");
|
||||
return CreateLocalTrajectoryBuilderOptions(parameter_dictionary.get());
|
||||
|
|
|
|||
|
|
@ -18,7 +18,6 @@
|
|||
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
|
||||
#include "cartographer/mapping_3d/optimizing_local_trajectory_builder.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
|
|
@ -26,15 +25,8 @@ namespace mapping_3d {
|
|||
std::unique_ptr<LocalTrajectoryBuilderInterface> CreateLocalTrajectoryBuilder(
|
||||
const proto::LocalTrajectoryBuilderOptions&
|
||||
local_trajectory_builder_options) {
|
||||
switch (local_trajectory_builder_options.use()) {
|
||||
case proto::LocalTrajectoryBuilderOptions::KALMAN:
|
||||
return common::make_unique<KalmanLocalTrajectoryBuilder>(
|
||||
local_trajectory_builder_options);
|
||||
case proto::LocalTrajectoryBuilderOptions::OPTIMIZING:
|
||||
return common::make_unique<OptimizingLocalTrajectoryBuilder>(
|
||||
local_trajectory_builder_options);
|
||||
}
|
||||
LOG(FATAL);
|
||||
}
|
||||
|
||||
} // namespace mapping_3d
|
||||
|
|
|
|||
|
|
@ -18,7 +18,6 @@
|
|||
|
||||
#include "cartographer/mapping_3d/kalman_local_trajectory_builder_options.h"
|
||||
#include "cartographer/mapping_3d/motion_filter.h"
|
||||
#include "cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h"
|
||||
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
|
||||
#include "cartographer/mapping_3d/submaps.h"
|
||||
#include "cartographer/sensor/voxel_filter.h"
|
||||
|
|
@ -57,16 +56,6 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
|
|||
CreateKalmanLocalTrajectoryBuilderOptions(
|
||||
parameter_dictionary->GetDictionary("kalman_local_trajectory_builder")
|
||||
.get());
|
||||
*options.mutable_optimizing_local_trajectory_builder_options() =
|
||||
CreateOptimizingLocalTrajectoryBuilderOptions(
|
||||
parameter_dictionary
|
||||
->GetDictionary("optimizing_local_trajectory_builder")
|
||||
.get());
|
||||
const string use_string = parameter_dictionary->GetString("use");
|
||||
proto::LocalTrajectoryBuilderOptions::Use use;
|
||||
CHECK(proto::LocalTrajectoryBuilderOptions::Use_Parse(use_string, &use))
|
||||
<< "Unknown local_trajectory_builder kind: " << use_string;
|
||||
options.set_use(use);
|
||||
return options;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,460 +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/optimizing_local_trajectory_builder.h"
|
||||
|
||||
#include "cartographer/common/ceres_solver_options.h"
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h"
|
||||
#include "cartographer/mapping_3d/rotation_cost_function.h"
|
||||
#include "cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h"
|
||||
#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
|
||||
#include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h"
|
||||
#include "cartographer/mapping_3d/translation_cost_function.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "cartographer/transform/transform_interpolation_buffer.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
|
||||
namespace {
|
||||
|
||||
// Computes the cost of differences between two velocities. For the constant
|
||||
// velocity model the residuals are just the vector difference.
|
||||
class VelocityDeltaCostFunctor {
|
||||
public:
|
||||
explicit VelocityDeltaCostFunctor(const double scaling_factor)
|
||||
: scaling_factor_(scaling_factor) {}
|
||||
|
||||
VelocityDeltaCostFunctor(const VelocityDeltaCostFunctor&) = delete;
|
||||
VelocityDeltaCostFunctor& operator=(const VelocityDeltaCostFunctor&) = delete;
|
||||
|
||||
template <typename T>
|
||||
bool operator()(const T* const old_velocity, const T* const new_velocity,
|
||||
T* residual) const {
|
||||
residual[0] = scaling_factor_ * (new_velocity[0] - old_velocity[0]);
|
||||
residual[1] = scaling_factor_ * (new_velocity[1] - old_velocity[1]);
|
||||
residual[2] = scaling_factor_ * (new_velocity[2] - old_velocity[2]);
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
const double scaling_factor_;
|
||||
};
|
||||
|
||||
class RelativeTranslationAndYawCostFunction {
|
||||
public:
|
||||
RelativeTranslationAndYawCostFunction(const double translation_scaling_factor,
|
||||
const double rotation_scaling_factor,
|
||||
const transform::Rigid3d& delta)
|
||||
: translation_scaling_factor_(translation_scaling_factor),
|
||||
rotation_scaling_factor_(rotation_scaling_factor),
|
||||
delta_(delta) {}
|
||||
|
||||
RelativeTranslationAndYawCostFunction(
|
||||
const RelativeTranslationAndYawCostFunction&) = delete;
|
||||
RelativeTranslationAndYawCostFunction& operator=(
|
||||
const RelativeTranslationAndYawCostFunction&) = delete;
|
||||
|
||||
template <typename T>
|
||||
bool operator()(const T* const start_translation,
|
||||
const T* const start_rotation, const T* const end_translation,
|
||||
const T* const end_rotation, T* residual) const {
|
||||
const transform::Rigid3<T> start(
|
||||
Eigen::Map<const Eigen::Matrix<T, 3, 1>>(start_translation),
|
||||
Eigen::Quaternion<T>(start_rotation[0], start_rotation[1],
|
||||
start_rotation[2], start_rotation[3]));
|
||||
const transform::Rigid3<T> end(
|
||||
Eigen::Map<const Eigen::Matrix<T, 3, 1>>(end_translation),
|
||||
Eigen::Quaternion<T>(end_rotation[0], end_rotation[1], end_rotation[2],
|
||||
end_rotation[3]));
|
||||
|
||||
const transform::Rigid3<T> delta = end.inverse() * start;
|
||||
const transform::Rigid3<T> error = delta.inverse() * delta_.cast<T>();
|
||||
residual[0] = translation_scaling_factor_ * error.translation().x();
|
||||
residual[1] = translation_scaling_factor_ * error.translation().y();
|
||||
residual[2] = translation_scaling_factor_ * error.translation().z();
|
||||
residual[3] = rotation_scaling_factor_ * transform::GetYaw(error);
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
const double translation_scaling_factor_;
|
||||
const double rotation_scaling_factor_;
|
||||
const transform::Rigid3d delta_;
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
OptimizingLocalTrajectoryBuilder::OptimizingLocalTrajectoryBuilder(
|
||||
const proto::LocalTrajectoryBuilderOptions& options)
|
||||
: options_(options),
|
||||
ceres_solver_options_(common::CreateCeresSolverOptions(
|
||||
options.ceres_scan_matcher_options().ceres_solver_options())),
|
||||
active_submaps_(options.submaps_options()),
|
||||
num_accumulated_(0),
|
||||
motion_filter_(options.motion_filter_options()) {}
|
||||
|
||||
OptimizingLocalTrajectoryBuilder::~OptimizingLocalTrajectoryBuilder() {}
|
||||
|
||||
void OptimizingLocalTrajectoryBuilder::AddImuData(
|
||||
const common::Time time, const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity) {
|
||||
imu_data_.push_back(ImuData{
|
||||
time, linear_acceleration, angular_velocity,
|
||||
});
|
||||
RemoveObsoleteSensorData();
|
||||
}
|
||||
|
||||
void OptimizingLocalTrajectoryBuilder::AddOdometerData(
|
||||
const common::Time time, const transform::Rigid3d& pose) {
|
||||
odometer_data_.push_back(OdometerData{time, pose});
|
||||
RemoveObsoleteSensorData();
|
||||
}
|
||||
|
||||
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
|
||||
OptimizingLocalTrajectoryBuilder::AddRangefinderData(
|
||||
const common::Time time, const Eigen::Vector3f& origin,
|
||||
const sensor::PointCloud& ranges) {
|
||||
CHECK_GT(ranges.size(), 0);
|
||||
|
||||
// TODO(hrapp): Handle misses.
|
||||
// TODO(hrapp): Where are NaNs in range_data_in_tracking coming from?
|
||||
sensor::PointCloud point_cloud;
|
||||
for (const Eigen::Vector3f& hit : ranges) {
|
||||
const Eigen::Vector3f delta = hit - origin;
|
||||
const float range = delta.norm();
|
||||
if (range >= options_.min_range()) {
|
||||
if (range <= options_.max_range()) {
|
||||
point_cloud.push_back(hit);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
auto high_resolution_options =
|
||||
options_.high_resolution_adaptive_voxel_filter_options();
|
||||
high_resolution_options.set_min_num_points(
|
||||
high_resolution_options.min_num_points() /
|
||||
options_.scans_per_accumulation());
|
||||
sensor::AdaptiveVoxelFilter high_resolution_adaptive_voxel_filter(
|
||||
high_resolution_options);
|
||||
const sensor::PointCloud high_resolution_filtered_points =
|
||||
high_resolution_adaptive_voxel_filter.Filter(point_cloud);
|
||||
|
||||
auto low_resolution_options =
|
||||
options_.low_resolution_adaptive_voxel_filter_options();
|
||||
low_resolution_options.set_min_num_points(
|
||||
low_resolution_options.min_num_points() /
|
||||
options_.scans_per_accumulation());
|
||||
sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
|
||||
low_resolution_options);
|
||||
const sensor::PointCloud low_resolution_filtered_points =
|
||||
low_resolution_adaptive_voxel_filter.Filter(point_cloud);
|
||||
|
||||
if (batches_.empty()) {
|
||||
// First rangefinder data ever. Initialize to the origin.
|
||||
batches_.push_back(
|
||||
Batch{time, point_cloud, high_resolution_filtered_points,
|
||||
low_resolution_filtered_points,
|
||||
State(Eigen::Vector3d::Zero(), Eigen::Quaterniond::Identity(),
|
||||
Eigen::Vector3d::Zero())});
|
||||
} else {
|
||||
const Batch& last_batch = batches_.back();
|
||||
batches_.push_back(Batch{
|
||||
time, point_cloud, high_resolution_filtered_points,
|
||||
low_resolution_filtered_points,
|
||||
PredictState(last_batch.state, last_batch.time, time),
|
||||
});
|
||||
}
|
||||
++num_accumulated_;
|
||||
|
||||
RemoveObsoleteSensorData();
|
||||
return MaybeOptimize(time);
|
||||
}
|
||||
|
||||
void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
|
||||
if (imu_data_.empty()) {
|
||||
batches_.clear();
|
||||
return;
|
||||
}
|
||||
|
||||
while (batches_.size() >
|
||||
static_cast<size_t>(options_.scans_per_accumulation())) {
|
||||
batches_.pop_front();
|
||||
}
|
||||
|
||||
while (imu_data_.size() > 1 &&
|
||||
(batches_.empty() || imu_data_[1].time <= batches_.front().time)) {
|
||||
imu_data_.pop_front();
|
||||
}
|
||||
|
||||
while (
|
||||
odometer_data_.size() > 1 &&
|
||||
(batches_.empty() || odometer_data_[1].time <= batches_.front().time)) {
|
||||
odometer_data_.pop_front();
|
||||
}
|
||||
}
|
||||
|
||||
void OptimizingLocalTrajectoryBuilder::TransformStates(
|
||||
const transform::Rigid3d& transform) {
|
||||
for (Batch& batch : batches_) {
|
||||
const transform::Rigid3d new_pose = transform * batch.state.ToRigid();
|
||||
const auto& velocity = batch.state.velocity;
|
||||
const Eigen::Vector3d new_velocity =
|
||||
transform.rotation() *
|
||||
Eigen::Vector3d(velocity[0], velocity[1], velocity[2]);
|
||||
batch.state =
|
||||
State(new_pose.translation(), new_pose.rotation(), new_velocity);
|
||||
}
|
||||
}
|
||||
|
||||
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
|
||||
OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
|
||||
// TODO(hrapp): Make the number of optimizations configurable.
|
||||
if (num_accumulated_ < options_.scans_per_accumulation() &&
|
||||
num_accumulated_ % 10 != 0) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
ceres::Problem problem;
|
||||
std::shared_ptr<const Submap> matching_submap =
|
||||
active_submaps_.submaps().front();
|
||||
// We transform the states in 'batches_' in place to be in the submap frame as
|
||||
// expected by the OccupiedSpaceCostFunctor. This is reverted after solving
|
||||
// the optimization problem.
|
||||
TransformStates(matching_submap->local_pose().inverse());
|
||||
for (size_t i = 0; i < batches_.size(); ++i) {
|
||||
Batch& batch = batches_[i];
|
||||
problem.AddResidualBlock(
|
||||
new ceres::AutoDiffCostFunction<scan_matching::OccupiedSpaceCostFunctor,
|
||||
ceres::DYNAMIC, 3, 4>(
|
||||
new scan_matching::OccupiedSpaceCostFunctor(
|
||||
options_.optimizing_local_trajectory_builder_options()
|
||||
.high_resolution_grid_weight() /
|
||||
std::sqrt(static_cast<double>(
|
||||
batch.high_resolution_filtered_points.size())),
|
||||
batch.high_resolution_filtered_points,
|
||||
matching_submap->high_resolution_hybrid_grid()),
|
||||
batch.high_resolution_filtered_points.size()),
|
||||
nullptr, batch.state.translation.data(), batch.state.rotation.data());
|
||||
problem.AddResidualBlock(
|
||||
new ceres::AutoDiffCostFunction<scan_matching::OccupiedSpaceCostFunctor,
|
||||
ceres::DYNAMIC, 3, 4>(
|
||||
new scan_matching::OccupiedSpaceCostFunctor(
|
||||
options_.optimizing_local_trajectory_builder_options()
|
||||
.low_resolution_grid_weight() /
|
||||
std::sqrt(static_cast<double>(
|
||||
batch.low_resolution_filtered_points.size())),
|
||||
batch.low_resolution_filtered_points,
|
||||
matching_submap->low_resolution_hybrid_grid()),
|
||||
batch.low_resolution_filtered_points.size()),
|
||||
nullptr, batch.state.translation.data(), batch.state.rotation.data());
|
||||
|
||||
if (i == 0) {
|
||||
problem.SetParameterBlockConstant(batch.state.translation.data());
|
||||
problem.SetParameterBlockConstant(batch.state.rotation.data());
|
||||
problem.AddParameterBlock(batch.state.velocity.data(), 3);
|
||||
problem.SetParameterBlockConstant(batch.state.velocity.data());
|
||||
} else {
|
||||
problem.SetParameterization(batch.state.rotation.data(),
|
||||
new ceres::QuaternionParameterization());
|
||||
}
|
||||
}
|
||||
|
||||
auto it = imu_data_.cbegin();
|
||||
for (size_t i = 1; i < batches_.size(); ++i) {
|
||||
problem.AddResidualBlock(
|
||||
new ceres::AutoDiffCostFunction<VelocityDeltaCostFunctor, 3, 3, 3>(
|
||||
new VelocityDeltaCostFunctor(
|
||||
options_.optimizing_local_trajectory_builder_options()
|
||||
.velocity_weight())),
|
||||
nullptr, batches_[i - 1].state.velocity.data(),
|
||||
batches_[i].state.velocity.data());
|
||||
|
||||
problem.AddResidualBlock(
|
||||
new ceres::AutoDiffCostFunction<TranslationCostFunction, 3, 3, 3, 3>(
|
||||
new TranslationCostFunction(
|
||||
options_.optimizing_local_trajectory_builder_options()
|
||||
.translation_weight(),
|
||||
common::ToSeconds(batches_[i].time - batches_[i - 1].time))),
|
||||
nullptr, batches_[i - 1].state.translation.data(),
|
||||
batches_[i].state.translation.data(),
|
||||
batches_[i - 1].state.velocity.data());
|
||||
|
||||
const IntegrateImuResult<double> result =
|
||||
IntegrateImu(imu_data_, batches_[i - 1].time, batches_[i].time, &it);
|
||||
problem.AddResidualBlock(
|
||||
new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>(
|
||||
new RotationCostFunction(
|
||||
options_.optimizing_local_trajectory_builder_options()
|
||||
.rotation_weight(),
|
||||
result.delta_rotation)),
|
||||
nullptr, batches_[i - 1].state.rotation.data(),
|
||||
batches_[i].state.rotation.data());
|
||||
}
|
||||
|
||||
if (odometer_data_.size() > 1) {
|
||||
transform::TransformInterpolationBuffer interpolation_buffer;
|
||||
for (const auto& odometer_data : odometer_data_) {
|
||||
interpolation_buffer.Push(odometer_data.time, odometer_data.pose);
|
||||
}
|
||||
for (size_t i = 1; i < batches_.size(); ++i) {
|
||||
// Only add constraints for this range data if we have bracketing data
|
||||
// from the odometer.
|
||||
if (!(interpolation_buffer.earliest_time() <= batches_[i - 1].time &&
|
||||
batches_[i].time <= interpolation_buffer.latest_time())) {
|
||||
continue;
|
||||
}
|
||||
const transform::Rigid3d previous_odometer_pose =
|
||||
interpolation_buffer.Lookup(batches_[i - 1].time);
|
||||
const transform::Rigid3d current_odometer_pose =
|
||||
interpolation_buffer.Lookup(batches_[i].time);
|
||||
const transform::Rigid3d delta_pose =
|
||||
current_odometer_pose.inverse() * previous_odometer_pose;
|
||||
problem.AddResidualBlock(
|
||||
new ceres::AutoDiffCostFunction<RelativeTranslationAndYawCostFunction,
|
||||
4, 3, 4, 3, 4>(
|
||||
new RelativeTranslationAndYawCostFunction(
|
||||
options_.optimizing_local_trajectory_builder_options()
|
||||
.odometry_translation_weight(),
|
||||
options_.optimizing_local_trajectory_builder_options()
|
||||
.odometry_rotation_weight(),
|
||||
delta_pose)),
|
||||
nullptr, batches_[i - 1].state.translation.data(),
|
||||
batches_[i - 1].state.rotation.data(),
|
||||
batches_[i].state.translation.data(),
|
||||
batches_[i].state.rotation.data());
|
||||
}
|
||||
}
|
||||
|
||||
ceres::Solver::Summary summary;
|
||||
ceres::Solve(ceres_solver_options_, &problem, &summary);
|
||||
// The optimized states in 'batches_' are in the submap frame and we transform
|
||||
// them in place to be in the local SLAM frame again.
|
||||
TransformStates(matching_submap->local_pose());
|
||||
if (num_accumulated_ < options_.scans_per_accumulation()) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
num_accumulated_ = 0;
|
||||
|
||||
const transform::Rigid3d optimized_pose = batches_.back().state.ToRigid();
|
||||
sensor::RangeData accumulated_range_data_in_tracking = {
|
||||
Eigen::Vector3f::Zero(), {}, {}};
|
||||
|
||||
for (const auto& batch : batches_) {
|
||||
const transform::Rigid3f transform =
|
||||
(optimized_pose.inverse() * batch.state.ToRigid()).cast<float>();
|
||||
for (const Eigen::Vector3f& point : batch.points) {
|
||||
accumulated_range_data_in_tracking.returns.push_back(transform * point);
|
||||
}
|
||||
}
|
||||
|
||||
return AddAccumulatedRangeData(time, optimized_pose,
|
||||
accumulated_range_data_in_tracking);
|
||||
}
|
||||
|
||||
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
|
||||
OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData(
|
||||
const common::Time time, const transform::Rigid3d& optimized_pose,
|
||||
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;
|
||||
}
|
||||
|
||||
last_pose_estimate_ = {
|
||||
time, optimized_pose,
|
||||
sensor::TransformPointCloud(filtered_range_data.returns,
|
||||
optimized_pose.cast<float>())};
|
||||
|
||||
return InsertIntoSubmap(time, filtered_range_data, optimized_pose);
|
||||
}
|
||||
|
||||
const OptimizingLocalTrajectoryBuilder::PoseEstimate&
|
||||
OptimizingLocalTrajectoryBuilder::pose_estimate() const {
|
||||
return last_pose_estimate_;
|
||||
}
|
||||
|
||||
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
|
||||
OptimizingLocalTrajectoryBuilder::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);
|
||||
}
|
||||
// TODO(whess): Use an ImuTracker to track the gravity direction.
|
||||
const Eigen::Quaterniond kFakeGravityOrientation =
|
||||
Eigen::Quaterniond::Identity();
|
||||
active_submaps_.InsertRangeData(
|
||||
sensor::TransformRangeData(range_data_in_tracking,
|
||||
pose_observation.cast<float>()),
|
||||
kFakeGravityOrientation);
|
||||
|
||||
return std::unique_ptr<InsertionResult>(
|
||||
new InsertionResult{time, range_data_in_tracking, pose_observation,
|
||||
std::move(insertion_submaps)});
|
||||
}
|
||||
|
||||
OptimizingLocalTrajectoryBuilder::State
|
||||
OptimizingLocalTrajectoryBuilder::PredictState(const State& start_state,
|
||||
const common::Time start_time,
|
||||
const common::Time end_time) {
|
||||
auto it = --imu_data_.cend();
|
||||
while (it->time > start_time) {
|
||||
CHECK(it != imu_data_.cbegin());
|
||||
--it;
|
||||
}
|
||||
|
||||
const IntegrateImuResult<double> result =
|
||||
IntegrateImu(imu_data_, start_time, end_time, &it);
|
||||
|
||||
const Eigen::Quaterniond start_rotation(
|
||||
start_state.rotation[0], start_state.rotation[1], start_state.rotation[2],
|
||||
start_state.rotation[3]);
|
||||
const Eigen::Quaterniond orientation = start_rotation * result.delta_rotation;
|
||||
const double delta_time_seconds = common::ToSeconds(end_time - start_time);
|
||||
|
||||
// TODO(hrapp): IntegrateImu should integration position as well.
|
||||
const Eigen::Vector3d position =
|
||||
Eigen::Map<const Eigen::Vector3d>(start_state.translation.data()) +
|
||||
delta_time_seconds *
|
||||
Eigen::Map<const Eigen::Vector3d>(start_state.velocity.data());
|
||||
const Eigen::Vector3d velocity =
|
||||
Eigen::Map<const Eigen::Vector3d>(start_state.velocity.data()) +
|
||||
start_rotation * result.delta_velocity -
|
||||
gravity_constant_ * delta_time_seconds * Eigen::Vector3d::UnitZ();
|
||||
|
||||
return State(position, orientation, velocity);
|
||||
}
|
||||
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
||||
|
|
@ -1,134 +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_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_
|
||||
#define CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_
|
||||
|
||||
#include <array>
|
||||
#include <deque>
|
||||
#include <memory>
|
||||
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||
#include "cartographer/mapping_3d/imu_integration.h"
|
||||
#include "cartographer/mapping_3d/local_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/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 {
|
||||
|
||||
// Batches up some sensor data and optimizes them in one go to get a locally
|
||||
// consistent trajectory.
|
||||
class OptimizingLocalTrajectoryBuilder
|
||||
: public LocalTrajectoryBuilderInterface {
|
||||
public:
|
||||
explicit OptimizingLocalTrajectoryBuilder(
|
||||
const proto::LocalTrajectoryBuilderOptions& options);
|
||||
~OptimizingLocalTrajectoryBuilder() override;
|
||||
|
||||
OptimizingLocalTrajectoryBuilder(const OptimizingLocalTrajectoryBuilder&) =
|
||||
delete;
|
||||
OptimizingLocalTrajectoryBuilder& operator=(
|
||||
const OptimizingLocalTrajectoryBuilder&) = delete;
|
||||
|
||||
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity) override;
|
||||
std::unique_ptr<InsertionResult> AddRangefinderData(
|
||||
common::Time time, const Eigen::Vector3f& origin,
|
||||
const sensor::PointCloud& ranges) override;
|
||||
void AddOdometerData(common::Time time,
|
||||
const transform::Rigid3d& pose) override;
|
||||
const PoseEstimate& pose_estimate() const override;
|
||||
|
||||
private:
|
||||
struct State {
|
||||
std::array<double, 3> translation;
|
||||
std::array<double, 4> rotation; // Rotation quaternion as (w, x, y, z).
|
||||
std::array<double, 3> velocity;
|
||||
|
||||
State(const Eigen::Vector3d& translation,
|
||||
const Eigen::Quaterniond& rotation, const Eigen::Vector3d& velocity)
|
||||
: translation{{translation.x(), translation.y(), translation.z()}},
|
||||
rotation{{rotation.w(), rotation.x(), rotation.y(), rotation.z()}},
|
||||
velocity{{velocity.x(), velocity.y(), velocity.z()}} {}
|
||||
|
||||
Eigen::Quaterniond ToQuaternion() const {
|
||||
return Eigen::Quaterniond(rotation[0], rotation[1], rotation[2],
|
||||
rotation[3]);
|
||||
}
|
||||
|
||||
transform::Rigid3d ToRigid() const {
|
||||
return transform::Rigid3d(
|
||||
Eigen::Vector3d(translation[0], translation[1], translation[2]),
|
||||
ToQuaternion());
|
||||
}
|
||||
};
|
||||
|
||||
struct Batch {
|
||||
common::Time time;
|
||||
sensor::PointCloud points;
|
||||
sensor::PointCloud high_resolution_filtered_points;
|
||||
sensor::PointCloud low_resolution_filtered_points;
|
||||
State state;
|
||||
};
|
||||
|
||||
struct OdometerData {
|
||||
common::Time time;
|
||||
|
||||
// Dead-reckoning pose of the odometry.
|
||||
transform::Rigid3d pose;
|
||||
};
|
||||
|
||||
State PredictState(const State& start_state, const common::Time start_time,
|
||||
const common::Time end_time);
|
||||
|
||||
void RemoveObsoleteSensorData();
|
||||
|
||||
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
|
||||
common::Time time, const transform::Rigid3d& pose_observation,
|
||||
const sensor::RangeData& range_data_in_tracking);
|
||||
|
||||
std::unique_ptr<InsertionResult> InsertIntoSubmap(
|
||||
const common::Time time, const sensor::RangeData& range_data_in_tracking,
|
||||
const transform::Rigid3d& pose_observation);
|
||||
|
||||
void TransformStates(const transform::Rigid3d& transform);
|
||||
std::unique_ptr<InsertionResult> MaybeOptimize(common::Time time);
|
||||
|
||||
const proto::LocalTrajectoryBuilderOptions options_;
|
||||
const ceres::Solver::Options ceres_solver_options_;
|
||||
mapping_3d::ActiveSubmaps active_submaps_;
|
||||
int num_accumulated_;
|
||||
|
||||
std::deque<Batch> batches_;
|
||||
double gravity_constant_ = 9.8;
|
||||
std::deque<ImuData> imu_data_;
|
||||
std::deque<OdometerData> odometer_data_;
|
||||
|
||||
PoseEstimate last_pose_estimate_;
|
||||
|
||||
MotionFilter motion_filter_;
|
||||
};
|
||||
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_
|
||||
|
|
@ -1,44 +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/optimizing_local_trajectory_builder_options.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
|
||||
proto::OptimizingLocalTrajectoryBuilderOptions
|
||||
CreateOptimizingLocalTrajectoryBuilderOptions(
|
||||
common::LuaParameterDictionary* const parameter_dictionary) {
|
||||
proto::OptimizingLocalTrajectoryBuilderOptions options;
|
||||
options.set_high_resolution_grid_weight(
|
||||
parameter_dictionary->GetDouble("high_resolution_grid_weight"));
|
||||
options.set_low_resolution_grid_weight(
|
||||
parameter_dictionary->GetDouble("low_resolution_grid_weight"));
|
||||
options.set_velocity_weight(
|
||||
parameter_dictionary->GetDouble("velocity_weight"));
|
||||
options.set_translation_weight(
|
||||
parameter_dictionary->GetDouble("translation_weight"));
|
||||
options.set_rotation_weight(
|
||||
parameter_dictionary->GetDouble("rotation_weight"));
|
||||
options.set_odometry_translation_weight(
|
||||
parameter_dictionary->GetDouble("odometry_translation_weight"));
|
||||
options.set_odometry_rotation_weight(
|
||||
parameter_dictionary->GetDouble("odometry_rotation_weight"));
|
||||
return options;
|
||||
}
|
||||
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
||||
|
|
@ -1,33 +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_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
|
||||
#define CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
|
||||
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
|
||||
proto::OptimizingLocalTrajectoryBuilderOptions
|
||||
CreateOptimizingLocalTrajectoryBuilderOptions(
|
||||
common::LuaParameterDictionary* parameter_dictionary);
|
||||
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
|
||||
|
|
@ -17,18 +17,12 @@ syntax = "proto2";
|
|||
import "cartographer/mapping_3d/proto/motion_filter_options.proto";
|
||||
import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
|
||||
import "cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto";
|
||||
import "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto";
|
||||
import "cartographer/mapping_3d/proto/submaps_options.proto";
|
||||
import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
|
||||
|
||||
package cartographer.mapping_3d.proto;
|
||||
|
||||
message LocalTrajectoryBuilderOptions {
|
||||
enum Use {
|
||||
KALMAN = 0;
|
||||
OPTIMIZING = 1;
|
||||
}
|
||||
|
||||
// Rangefinder points outside these ranges will be dropped.
|
||||
optional float min_range = 1;
|
||||
optional float max_range = 2;
|
||||
|
|
@ -52,10 +46,6 @@ message LocalTrajectoryBuilderOptions {
|
|||
optional MotionFilterOptions motion_filter_options = 7;
|
||||
optional SubmapsOptions submaps_options = 8;
|
||||
|
||||
// Which one of the implementation to instantiate and use.
|
||||
optional Use use = 9;
|
||||
optional KalmanLocalTrajectoryBuilderOptions
|
||||
kalman_local_trajectory_builder_options = 10;
|
||||
optional OptimizingLocalTrajectoryBuilderOptions
|
||||
optimizing_local_trajectory_builder_options = 11;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,27 +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.
|
||||
|
||||
syntax = "proto2";
|
||||
|
||||
package cartographer.mapping_3d.proto;
|
||||
|
||||
message OptimizingLocalTrajectoryBuilderOptions {
|
||||
optional double high_resolution_grid_weight = 6;
|
||||
optional double low_resolution_grid_weight = 7;
|
||||
optional double velocity_weight = 1;
|
||||
optional double translation_weight = 2;
|
||||
optional double rotation_weight = 3;
|
||||
optional double odometry_translation_weight = 4;
|
||||
optional double odometry_rotation_weight = 5;
|
||||
}
|
||||
|
|
@ -63,7 +63,6 @@ TRAJECTORY_BUILDER_3D = {
|
|||
},
|
||||
},
|
||||
|
||||
use = "KALMAN", -- or "OPTIMIZING".
|
||||
kalman_local_trajectory_builder = {
|
||||
pose_tracker = {
|
||||
orientation_model_variance = 5e-3,
|
||||
|
|
@ -86,14 +85,4 @@ TRAJECTORY_BUILDER_3D = {
|
|||
odometer_translational_variance = 1e-7,
|
||||
odometer_rotational_variance = 1e-7,
|
||||
},
|
||||
|
||||
optimizing_local_trajectory_builder = {
|
||||
high_resolution_grid_weight = 5.,
|
||||
low_resolution_grid_weight = 15.,
|
||||
velocity_weight = 4e1,
|
||||
translation_weight = 1e2,
|
||||
rotation_weight = 1e3,
|
||||
odometry_translation_weight = 1e4,
|
||||
odometry_rotation_weight = 1e4,
|
||||
},
|
||||
}
|
||||
|
|
|
|||
|
|
@ -344,6 +344,38 @@ double odometer_rotational_variance
|
|||
cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions
|
||||
===========================================================
|
||||
|
||||
float min_range
|
||||
Rangefinder points outside these ranges will be dropped.
|
||||
|
||||
float max_range
|
||||
Not yet documented.
|
||||
|
||||
int32 scans_per_accumulation
|
||||
Number of scans to accumulate into one unwarped, combined scan to use for
|
||||
scan matching.
|
||||
|
||||
float voxel_filter_size
|
||||
Voxel filter that gets applied to the range data immediately after
|
||||
cropping.
|
||||
|
||||
cartographer.sensor.proto.AdaptiveVoxelFilterOptions high_resolution_adaptive_voxel_filter_options
|
||||
Voxel filter used to compute a sparser point cloud for matching.
|
||||
|
||||
cartographer.sensor.proto.AdaptiveVoxelFilterOptions low_resolution_adaptive_voxel_filter_options
|
||||
Not yet documented.
|
||||
|
||||
cartographer.mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options
|
||||
Not yet documented.
|
||||
|
||||
cartographer.mapping_3d.proto.MotionFilterOptions motion_filter_options
|
||||
Not yet documented.
|
||||
|
||||
cartographer.mapping_3d.proto.SubmapsOptions submaps_options
|
||||
Not yet documented.
|
||||
|
||||
cartographer.mapping_3d.proto.KalmanLocalTrajectoryBuilderOptions kalman_local_trajectory_builder_options
|
||||
Not yet documented.
|
||||
|
||||
|
||||
cartographer.mapping_3d.proto.MotionFilterOptions
|
||||
=================================================
|
||||
|
|
@ -358,31 +390,6 @@ double max_angle_radians
|
|||
Threshold above which a new scan is inserted based on rotational motion.
|
||||
|
||||
|
||||
cartographer.mapping_3d.proto.OptimizingLocalTrajectoryBuilderOptions
|
||||
=====================================================================
|
||||
|
||||
double high_resolution_grid_weight
|
||||
Not yet documented.
|
||||
|
||||
double low_resolution_grid_weight
|
||||
Not yet documented.
|
||||
|
||||
double velocity_weight
|
||||
Not yet documented.
|
||||
|
||||
double translation_weight
|
||||
Not yet documented.
|
||||
|
||||
double rotation_weight
|
||||
Not yet documented.
|
||||
|
||||
double odometry_translation_weight
|
||||
Not yet documented.
|
||||
|
||||
double odometry_rotation_weight
|
||||
Not yet documented.
|
||||
|
||||
|
||||
cartographer.mapping_3d.proto.RangeDataInserterOptions
|
||||
======================================================
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue