Remove the LocalTrajectoryBuilderInterface in 3D. (#371)
Wolfgang Hess
GitHub
parent
471bcb6207
commit
e4e22e9f26
|
|
@ -238,7 +238,8 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
|||
GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
|
||||
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
|
||||
const mapping::SubmapId matching_id = submap_ids.front();
|
||||
const int num_trimmed_submaps = optimization_problem_.num_trimmed_submaps(trajectory_id);
|
||||
const int num_trimmed_submaps =
|
||||
optimization_problem_.num_trimmed_submaps(trajectory_id);
|
||||
const transform::Rigid2d optimized_pose =
|
||||
optimization_problem_.submap_data()
|
||||
.at(matching_id.trajectory_id)
|
||||
|
|
@ -461,8 +462,8 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
|||
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
|
||||
const int trajectory_id) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
|
||||
num_trimmed_submaps_at_last_optimization_,
|
||||
return ComputeLocalToGlobalTransform(
|
||||
optimized_submap_transforms_, num_trimmed_submaps_at_last_optimization_,
|
||||
trajectory_id);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -134,7 +134,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
|
|||
|
||||
// Set the starting point.
|
||||
// TODO(hrapp): Move ceres data into SubmapData.
|
||||
std::vector<std::vector<std::array<double, 3>>> C_submaps(submap_data_.size());
|
||||
std::vector<std::vector<std::array<double, 3>>> C_submaps(
|
||||
submap_data_.size());
|
||||
std::vector<std::vector<std::array<double, 3>>> C_nodes(node_data_.size());
|
||||
bool first_submap = true;
|
||||
for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
|
||||
|
|
@ -221,7 +222,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
|
|||
for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
|
||||
++trajectory_id) {
|
||||
for (size_t submap_data_index = 0;
|
||||
submap_data_index != submap_data_[trajectory_id].size(); ++submap_data_index) {
|
||||
submap_data_index != submap_data_[trajectory_id].size();
|
||||
++submap_data_index) {
|
||||
submap_data_[trajectory_id][submap_data_index].pose =
|
||||
ToPose(C_submaps[trajectory_id][submap_data_index]);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@
|
|||
#include <memory>
|
||||
|
||||
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||
#include "cartographer/mapping_3d/local_trajectory_builder.h"
|
||||
#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
|
||||
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
|
||||
#include "cartographer/mapping_3d/sparse_pose_graph.h"
|
||||
|
||||
|
|
@ -49,7 +49,7 @@ class GlobalTrajectoryBuilder
|
|||
private:
|
||||
const int trajectory_id_;
|
||||
mapping_3d::SparsePoseGraph* const sparse_pose_graph_;
|
||||
std::unique_ptr<LocalTrajectoryBuilderInterface> local_trajectory_builder_;
|
||||
std::unique_ptr<KalmanLocalTrajectoryBuilder> local_trajectory_builder_;
|
||||
};
|
||||
|
||||
} // namespace mapping_3d
|
||||
|
|
|
|||
|
|
@ -22,7 +22,6 @@
|
|||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/kalman_filter/pose_tracker.h"
|
||||
#include "cartographer/mapping/global_trajectory_builder_interface.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/scan_matching/ceres_scan_matcher.h"
|
||||
|
|
@ -30,30 +29,39 @@
|
|||
#include "cartographer/mapping_3d/submaps.h"
|
||||
#include "cartographer/sensor/range_data.h"
|
||||
#include "cartographer/sensor/voxel_filter.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
|
||||
// Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop
|
||||
// closure.
|
||||
class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
|
||||
class KalmanLocalTrajectoryBuilder {
|
||||
public:
|
||||
using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate;
|
||||
|
||||
struct InsertionResult {
|
||||
common::Time time;
|
||||
sensor::RangeData range_data_in_tracking;
|
||||
transform::Rigid3d pose_observation;
|
||||
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
|
||||
};
|
||||
|
||||
explicit KalmanLocalTrajectoryBuilder(
|
||||
const proto::LocalTrajectoryBuilderOptions& options);
|
||||
~KalmanLocalTrajectoryBuilder() override;
|
||||
~KalmanLocalTrajectoryBuilder();
|
||||
|
||||
KalmanLocalTrajectoryBuilder(const KalmanLocalTrajectoryBuilder&) = delete;
|
||||
KalmanLocalTrajectoryBuilder& operator=(const KalmanLocalTrajectoryBuilder&) =
|
||||
delete;
|
||||
|
||||
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity) override;
|
||||
const Eigen::Vector3d& angular_velocity);
|
||||
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;
|
||||
const sensor::PointCloud& ranges);
|
||||
void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
|
||||
const PoseEstimate& pose_estimate() const;
|
||||
|
||||
private:
|
||||
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
|
||||
|
|
|
|||
|
|
@ -17,12 +17,11 @@
|
|||
#include "cartographer/mapping_3d/local_trajectory_builder.h"
|
||||
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
|
||||
std::unique_ptr<LocalTrajectoryBuilderInterface> CreateLocalTrajectoryBuilder(
|
||||
std::unique_ptr<KalmanLocalTrajectoryBuilder> CreateLocalTrajectoryBuilder(
|
||||
const proto::LocalTrajectoryBuilderOptions&
|
||||
local_trajectory_builder_options) {
|
||||
return common::make_unique<KalmanLocalTrajectoryBuilder>(
|
||||
|
|
|
|||
|
|
@ -20,13 +20,13 @@
|
|||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/mapping_3d/local_trajectory_builder_interface.h"
|
||||
#include "cartographer/mapping_3d/kalman_local_trajectory_builder.h"
|
||||
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
|
||||
std::unique_ptr<LocalTrajectoryBuilderInterface> CreateLocalTrajectoryBuilder(
|
||||
std::unique_ptr<KalmanLocalTrajectoryBuilder> CreateLocalTrajectoryBuilder(
|
||||
const proto::LocalTrajectoryBuilderOptions&
|
||||
local_trajectory_builder_options);
|
||||
|
||||
|
|
|
|||
|
|
@ -1,68 +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_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_
|
||||
#define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||
#include "cartographer/mapping_3d/submaps.h"
|
||||
#include "cartographer/sensor/range_data.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
|
||||
class LocalTrajectoryBuilderInterface {
|
||||
public:
|
||||
using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate;
|
||||
|
||||
struct InsertionResult {
|
||||
common::Time time;
|
||||
sensor::RangeData range_data_in_tracking;
|
||||
transform::Rigid3d pose_observation;
|
||||
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
|
||||
};
|
||||
|
||||
virtual ~LocalTrajectoryBuilderInterface() {}
|
||||
|
||||
LocalTrajectoryBuilderInterface(const LocalTrajectoryBuilderInterface&) =
|
||||
delete;
|
||||
LocalTrajectoryBuilderInterface& operator=(
|
||||
const LocalTrajectoryBuilderInterface&) = delete;
|
||||
|
||||
virtual void AddImuData(common::Time time,
|
||||
const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity) = 0;
|
||||
virtual std::unique_ptr<InsertionResult> AddRangefinderData(
|
||||
common::Time time, const Eigen::Vector3f& origin,
|
||||
const sensor::PointCloud& ranges) = 0;
|
||||
virtual void AddOdometerData(common::Time time,
|
||||
const transform::Rigid3d& pose) = 0;
|
||||
|
||||
virtual const PoseEstimate& pose_estimate() const = 0;
|
||||
|
||||
protected:
|
||||
LocalTrajectoryBuilderInterface() {}
|
||||
};
|
||||
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_
|
||||
Loading…
Reference in New Issue