diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc index 2b321b4..fafef74 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc @@ -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()); diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index 8214434..25f65d3 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -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 CreateLocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& local_trajectory_builder_options) { - switch (local_trajectory_builder_options.use()) { - case proto::LocalTrajectoryBuilderOptions::KALMAN: - return common::make_unique( - local_trajectory_builder_options); - case proto::LocalTrajectoryBuilderOptions::OPTIMIZING: - return common::make_unique( - local_trajectory_builder_options); - } - LOG(FATAL); + return common::make_unique( + local_trajectory_builder_options); } } // namespace mapping_3d diff --git a/cartographer/mapping_3d/local_trajectory_builder_options.cc b/cartographer/mapping_3d/local_trajectory_builder_options.cc index ac7bdb4..637d702 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_options.cc +++ b/cartographer/mapping_3d/local_trajectory_builder_options.cc @@ -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; } diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc deleted file mode 100644 index 471bee8..0000000 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ /dev/null @@ -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 - 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 - 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 start( - Eigen::Map>(start_translation), - Eigen::Quaternion(start_rotation[0], start_rotation[1], - start_rotation[2], start_rotation[3])); - const transform::Rigid3 end( - Eigen::Map>(end_translation), - Eigen::Quaternion(end_rotation[0], end_rotation[1], end_rotation[2], - end_rotation[3])); - - const transform::Rigid3 delta = end.inverse() * start; - const transform::Rigid3 error = delta.inverse() * delta_.cast(); - 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::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(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::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 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( - new scan_matching::OccupiedSpaceCostFunctor( - options_.optimizing_local_trajectory_builder_options() - .high_resolution_grid_weight() / - std::sqrt(static_cast( - 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( - new scan_matching::OccupiedSpaceCostFunctor( - options_.optimizing_local_trajectory_builder_options() - .low_resolution_grid_weight() / - std::sqrt(static_cast( - 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( - 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( - 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 result = - IntegrateImu(imu_data_, batches_[i - 1].time, batches_[i].time, &it); - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction( - 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( - 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(); - 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::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())}; - - return InsertIntoSubmap(time, filtered_range_data, optimized_pose); -} - -const OptimizingLocalTrajectoryBuilder::PoseEstimate& -OptimizingLocalTrajectoryBuilder::pose_estimate() const { - return last_pose_estimate_; -} - -std::unique_ptr -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> insertion_submaps; - for (std::shared_ptr 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()), - kFakeGravityOrientation); - - return std::unique_ptr( - 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 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(start_state.translation.data()) + - delta_time_seconds * - Eigen::Map(start_state.velocity.data()); - const Eigen::Vector3d velocity = - Eigen::Map(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 diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h deleted file mode 100644 index 2e2c7ec..0000000 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h +++ /dev/null @@ -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 -#include -#include - -#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 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 translation; - std::array rotation; // Rotation quaternion as (w, x, y, z). - std::array 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 AddAccumulatedRangeData( - common::Time time, const transform::Rigid3d& pose_observation, - const sensor::RangeData& range_data_in_tracking); - - std::unique_ptr 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 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 batches_; - double gravity_constant_ = 9.8; - std::deque imu_data_; - std::deque odometer_data_; - - PoseEstimate last_pose_estimate_; - - MotionFilter motion_filter_; -}; - -} // namespace mapping_3d -} // namespace cartographer - -#endif // CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_ diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.cc deleted file mode 100644 index 7ba7507..0000000 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.cc +++ /dev/null @@ -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 diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h b/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h deleted file mode 100644 index 5580759..0000000 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h +++ /dev/null @@ -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_ diff --git a/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto index 32638a1..782e05d 100644 --- a/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto +++ b/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto @@ -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; } diff --git a/cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto b/cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto deleted file mode 100644 index 209f6a2..0000000 --- a/cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto +++ /dev/null @@ -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; -} diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 2a59c8b..d47db69 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -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, - }, } diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 60bdc9a..a495484 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -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 ======================================================