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 = {
|
kalman_local_trajectory_builder = {
|
||||||
use_online_correlative_scan_matching = false,
|
use_online_correlative_scan_matching = false,
|
||||||
real_time_correlative_scan_matcher = {
|
real_time_correlative_scan_matcher = {
|
||||||
|
|
@ -114,15 +113,6 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
|
||||||
odometer_translational_variance = 1e-7,
|
odometer_translational_variance = 1e-7,
|
||||||
odometer_rotational_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");
|
)text");
|
||||||
return CreateLocalTrajectoryBuilderOptions(parameter_dictionary.get());
|
return CreateLocalTrajectoryBuilderOptions(parameter_dictionary.get());
|
||||||
|
|
|
||||||
|
|
@ -18,7 +18,6 @@
|
||||||
|
|
||||||
#include "cartographer/common/make_unique.h"
|
#include "cartographer/common/make_unique.h"
|
||||||
#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
|
#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
|
||||||
#include "cartographer/mapping_3d/optimizing_local_trajectory_builder.h"
|
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_3d {
|
namespace mapping_3d {
|
||||||
|
|
@ -26,15 +25,8 @@ namespace mapping_3d {
|
||||||
std::unique_ptr<LocalTrajectoryBuilderInterface> CreateLocalTrajectoryBuilder(
|
std::unique_ptr<LocalTrajectoryBuilderInterface> CreateLocalTrajectoryBuilder(
|
||||||
const proto::LocalTrajectoryBuilderOptions&
|
const proto::LocalTrajectoryBuilderOptions&
|
||||||
local_trajectory_builder_options) {
|
local_trajectory_builder_options) {
|
||||||
switch (local_trajectory_builder_options.use()) {
|
|
||||||
case proto::LocalTrajectoryBuilderOptions::KALMAN:
|
|
||||||
return common::make_unique<KalmanLocalTrajectoryBuilder>(
|
return common::make_unique<KalmanLocalTrajectoryBuilder>(
|
||||||
local_trajectory_builder_options);
|
local_trajectory_builder_options);
|
||||||
case proto::LocalTrajectoryBuilderOptions::OPTIMIZING:
|
|
||||||
return common::make_unique<OptimizingLocalTrajectoryBuilder>(
|
|
||||||
local_trajectory_builder_options);
|
|
||||||
}
|
|
||||||
LOG(FATAL);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace mapping_3d
|
} // namespace mapping_3d
|
||||||
|
|
|
||||||
|
|
@ -18,7 +18,6 @@
|
||||||
|
|
||||||
#include "cartographer/mapping_3d/kalman_local_trajectory_builder_options.h"
|
#include "cartographer/mapping_3d/kalman_local_trajectory_builder_options.h"
|
||||||
#include "cartographer/mapping_3d/motion_filter.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/scan_matching/ceres_scan_matcher.h"
|
||||||
#include "cartographer/mapping_3d/submaps.h"
|
#include "cartographer/mapping_3d/submaps.h"
|
||||||
#include "cartographer/sensor/voxel_filter.h"
|
#include "cartographer/sensor/voxel_filter.h"
|
||||||
|
|
@ -57,16 +56,6 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
|
||||||
CreateKalmanLocalTrajectoryBuilderOptions(
|
CreateKalmanLocalTrajectoryBuilderOptions(
|
||||||
parameter_dictionary->GetDictionary("kalman_local_trajectory_builder")
|
parameter_dictionary->GetDictionary("kalman_local_trajectory_builder")
|
||||||
.get());
|
.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;
|
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/mapping_3d/proto/motion_filter_options.proto";
|
||||||
import "cartographer/sensor/proto/adaptive_voxel_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/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/proto/submaps_options.proto";
|
||||||
import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
|
import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
|
||||||
|
|
||||||
package cartographer.mapping_3d.proto;
|
package cartographer.mapping_3d.proto;
|
||||||
|
|
||||||
message LocalTrajectoryBuilderOptions {
|
message LocalTrajectoryBuilderOptions {
|
||||||
enum Use {
|
|
||||||
KALMAN = 0;
|
|
||||||
OPTIMIZING = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Rangefinder points outside these ranges will be dropped.
|
// Rangefinder points outside these ranges will be dropped.
|
||||||
optional float min_range = 1;
|
optional float min_range = 1;
|
||||||
optional float max_range = 2;
|
optional float max_range = 2;
|
||||||
|
|
@ -52,10 +46,6 @@ message LocalTrajectoryBuilderOptions {
|
||||||
optional MotionFilterOptions motion_filter_options = 7;
|
optional MotionFilterOptions motion_filter_options = 7;
|
||||||
optional SubmapsOptions submaps_options = 8;
|
optional SubmapsOptions submaps_options = 8;
|
||||||
|
|
||||||
// Which one of the implementation to instantiate and use.
|
|
||||||
optional Use use = 9;
|
|
||||||
optional KalmanLocalTrajectoryBuilderOptions
|
optional KalmanLocalTrajectoryBuilderOptions
|
||||||
kalman_local_trajectory_builder_options = 10;
|
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 = {
|
kalman_local_trajectory_builder = {
|
||||||
pose_tracker = {
|
pose_tracker = {
|
||||||
orientation_model_variance = 5e-3,
|
orientation_model_variance = 5e-3,
|
||||||
|
|
@ -86,14 +85,4 @@ TRAJECTORY_BUILDER_3D = {
|
||||||
odometer_translational_variance = 1e-7,
|
odometer_translational_variance = 1e-7,
|
||||||
odometer_rotational_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
|
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
|
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.
|
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
|
cartographer.mapping_3d.proto.RangeDataInserterOptions
|
||||||
======================================================
|
======================================================
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue