Unify GlobalTrajectoryBuilder across 2D and 3D (#489)
This PR unifies GlobalTrajectoryBuilder across 2D and 3D by templating it with the LocalTrajectoryBuilder and the SparsePoseGraph. It thus includes small changes to interfaces in LocalTrajectoryBuilder and SparsePoseGraph.master
parent
3c22c8253a
commit
06e9112bc8
|
@ -0,0 +1,83 @@
|
|||
/*
|
||||
* Copyright 2017 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_GLOBAL_TRAJECTORY_BUILDER_H_
|
||||
#define CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_
|
||||
|
||||
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
template <typename LocalTrajectoryBuilder,
|
||||
typename LocalTrajectoryBuilderOptions, typename SparsePoseGraph>
|
||||
class GlobalTrajectoryBuilder
|
||||
: public mapping::GlobalTrajectoryBuilderInterface {
|
||||
public:
|
||||
GlobalTrajectoryBuilder(const LocalTrajectoryBuilderOptions& options,
|
||||
const int trajectory_id,
|
||||
SparsePoseGraph* const sparse_pose_graph)
|
||||
: trajectory_id_(trajectory_id),
|
||||
sparse_pose_graph_(sparse_pose_graph),
|
||||
local_trajectory_builder_(options) {}
|
||||
~GlobalTrajectoryBuilder() override {}
|
||||
|
||||
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
|
||||
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
|
||||
|
||||
const mapping::PoseEstimate& pose_estimate() const override {
|
||||
return local_trajectory_builder_.pose_estimate();
|
||||
}
|
||||
|
||||
void AddRangefinderData(const common::Time time,
|
||||
const Eigen::Vector3f& origin,
|
||||
const sensor::PointCloud& ranges) override {
|
||||
std::unique_ptr<typename LocalTrajectoryBuilder::InsertionResult>
|
||||
insertion_result = local_trajectory_builder_.AddRangeData(
|
||||
time, sensor::RangeData{origin, ranges, {}});
|
||||
if (insertion_result == nullptr) {
|
||||
return;
|
||||
}
|
||||
sparse_pose_graph_->AddScan(
|
||||
insertion_result->constant_data, insertion_result->pose_observation,
|
||||
trajectory_id_, insertion_result->insertion_submaps);
|
||||
}
|
||||
|
||||
void AddSensorData(const sensor::ImuData& imu_data) override {
|
||||
local_trajectory_builder_.AddImuData(imu_data);
|
||||
sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
|
||||
}
|
||||
|
||||
void AddSensorData(const sensor::OdometryData& odometry_data) override {
|
||||
local_trajectory_builder_.AddOdometerData(odometry_data);
|
||||
sparse_pose_graph_->AddOdometerData(trajectory_id_, odometry_data);
|
||||
}
|
||||
|
||||
void AddSensorData(
|
||||
const sensor::FixedFramePoseData& fixed_frame_pose) override {
|
||||
sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
|
||||
}
|
||||
|
||||
private:
|
||||
const int trajectory_id_;
|
||||
SparsePoseGraph* const sparse_pose_graph_;
|
||||
LocalTrajectoryBuilder local_trajectory_builder_;
|
||||
};
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_H_
|
|
@ -24,8 +24,9 @@
|
|||
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/mapping/collated_trajectory_builder.h"
|
||||
#include "cartographer/mapping_2d/global_trajectory_builder.h"
|
||||
#include "cartographer/mapping_3d/global_trajectory_builder.h"
|
||||
#include "cartographer/mapping/global_trajectory_builder.h"
|
||||
#include "cartographer/mapping_2d/local_trajectory_builder.h"
|
||||
#include "cartographer/mapping_3d/local_trajectory_builder.h"
|
||||
#include "cartographer/sensor/range_data.h"
|
||||
#include "cartographer/sensor/voxel_filter.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
|
@ -76,7 +77,10 @@ int MapBuilder::AddTrajectoryBuilder(
|
|||
trajectory_builders_.push_back(
|
||||
common::make_unique<CollatedTrajectoryBuilder>(
|
||||
&sensor_collator_, trajectory_id, expected_sensor_ids,
|
||||
common::make_unique<mapping_3d::GlobalTrajectoryBuilder>(
|
||||
common::make_unique<mapping::GlobalTrajectoryBuilder<
|
||||
mapping_3d::LocalTrajectoryBuilder,
|
||||
mapping_3d::proto::LocalTrajectoryBuilderOptions,
|
||||
mapping_3d::SparsePoseGraph>>(
|
||||
trajectory_options.trajectory_builder_3d_options(),
|
||||
trajectory_id, sparse_pose_graph_3d_.get())));
|
||||
} else {
|
||||
|
@ -84,7 +88,10 @@ int MapBuilder::AddTrajectoryBuilder(
|
|||
trajectory_builders_.push_back(
|
||||
common::make_unique<CollatedTrajectoryBuilder>(
|
||||
&sensor_collator_, trajectory_id, expected_sensor_ids,
|
||||
common::make_unique<mapping_2d::GlobalTrajectoryBuilder>(
|
||||
common::make_unique<mapping::GlobalTrajectoryBuilder<
|
||||
mapping_2d::LocalTrajectoryBuilder,
|
||||
mapping_2d::proto::LocalTrajectoryBuilderOptions,
|
||||
mapping_2d::SparsePoseGraph>>(
|
||||
trajectory_options.trajectory_builder_2d_options(),
|
||||
trajectory_id, sparse_pose_graph_2d_.get())));
|
||||
}
|
||||
|
|
|
@ -1,66 +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_2d/global_trajectory_builder.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
|
||||
GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
|
||||
const proto::LocalTrajectoryBuilderOptions& options,
|
||||
const int trajectory_id, SparsePoseGraph* sparse_pose_graph)
|
||||
: trajectory_id_(trajectory_id),
|
||||
sparse_pose_graph_(sparse_pose_graph),
|
||||
local_trajectory_builder_(options) {}
|
||||
|
||||
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
|
||||
|
||||
void GlobalTrajectoryBuilder::AddRangefinderData(
|
||||
const common::Time time, const Eigen::Vector3f& origin,
|
||||
const sensor::PointCloud& ranges) {
|
||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
|
||||
local_trajectory_builder_.AddHorizontalRangeData(
|
||||
time, sensor::RangeData{origin, ranges, {}});
|
||||
if (insertion_result == nullptr) {
|
||||
return;
|
||||
}
|
||||
sparse_pose_graph_->AddScan(
|
||||
insertion_result->constant_data, insertion_result->pose_estimate_2d,
|
||||
trajectory_id_, insertion_result->insertion_submaps);
|
||||
}
|
||||
|
||||
void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) {
|
||||
local_trajectory_builder_.AddImuData(imu_data);
|
||||
sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
|
||||
}
|
||||
|
||||
void GlobalTrajectoryBuilder::AddSensorData(
|
||||
const sensor::OdometryData& odometry_data) {
|
||||
local_trajectory_builder_.AddOdometerData(odometry_data);
|
||||
sparse_pose_graph_->AddOdometerData(trajectory_id_, odometry_data);
|
||||
}
|
||||
|
||||
void GlobalTrajectoryBuilder::AddSensorData(
|
||||
const sensor::FixedFramePoseData& fixed_frame_pose) {
|
||||
sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
|
||||
}
|
||||
|
||||
const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const {
|
||||
return local_trajectory_builder_.pose_estimate();
|
||||
}
|
||||
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
|
@ -1,59 +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_2D_GLOBAL_TRAJECTORY_BUILDER_H_
|
||||
#define CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_
|
||||
|
||||
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||
#include "cartographer/mapping_2d/local_trajectory_builder.h"
|
||||
#include "cartographer/mapping_2d/sparse_pose_graph.h"
|
||||
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
|
||||
class GlobalTrajectoryBuilder
|
||||
: public mapping::GlobalTrajectoryBuilderInterface {
|
||||
public:
|
||||
GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options,
|
||||
int trajectory_id,
|
||||
SparsePoseGraph* sparse_pose_graph);
|
||||
~GlobalTrajectoryBuilder() override;
|
||||
|
||||
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
|
||||
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
|
||||
|
||||
const mapping::PoseEstimate& pose_estimate() const override;
|
||||
|
||||
// Projects 'ranges' into 2D. Therefore, 'ranges' should be approximately
|
||||
// parallel to the ground plane.
|
||||
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
|
||||
const sensor::PointCloud& ranges) override;
|
||||
void AddSensorData(const sensor::ImuData& imu_data) override;
|
||||
void AddSensorData(const sensor::OdometryData& odometry_data) override;
|
||||
void AddSensorData(
|
||||
const sensor::FixedFramePoseData& fixed_frame_pose) override;
|
||||
|
||||
private:
|
||||
const int trajectory_id_;
|
||||
SparsePoseGraph* const sparse_pose_graph_;
|
||||
LocalTrajectoryBuilder local_trajectory_builder_;
|
||||
};
|
||||
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_
|
|
@ -82,7 +82,7 @@ void LocalTrajectoryBuilder::ScanMatch(
|
|||
}
|
||||
|
||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
||||
LocalTrajectoryBuilder::AddHorizontalRangeData(
|
||||
LocalTrajectoryBuilder::AddRangeData(
|
||||
const common::Time time, const sensor::RangeData& range_data) {
|
||||
// Initialize extrapolator now if we do not ever use an IMU.
|
||||
if (!options_.use_imu_data()) {
|
||||
|
|
|
@ -42,7 +42,7 @@ class LocalTrajectoryBuilder {
|
|||
public:
|
||||
struct InsertionResult {
|
||||
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
|
||||
transform::Rigid2d pose_estimate_2d;
|
||||
transform::Rigid2d pose_observation;
|
||||
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
|
||||
};
|
||||
|
||||
|
@ -54,7 +54,9 @@ class LocalTrajectoryBuilder {
|
|||
LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
|
||||
|
||||
const mapping::PoseEstimate& pose_estimate() const;
|
||||
std::unique_ptr<InsertionResult> AddHorizontalRangeData(
|
||||
|
||||
// Range data must be approximately horizontal for 2D SLAM.
|
||||
std::unique_ptr<InsertionResult> AddRangeData(
|
||||
common::Time, const sensor::RangeData& range_data);
|
||||
void AddImuData(const sensor::ImuData& imu_data);
|
||||
void AddOdometerData(const sensor::OdometryData& odometry_data);
|
||||
|
|
|
@ -1,64 +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/global_trajectory_builder.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
|
||||
GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
|
||||
const proto::LocalTrajectoryBuilderOptions& options,
|
||||
const int trajectory_id, SparsePoseGraph* sparse_pose_graph)
|
||||
: trajectory_id_(trajectory_id),
|
||||
sparse_pose_graph_(sparse_pose_graph),
|
||||
local_trajectory_builder_(options) {}
|
||||
|
||||
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
|
||||
|
||||
void GlobalTrajectoryBuilder::AddRangefinderData(
|
||||
const common::Time time, const Eigen::Vector3f& origin,
|
||||
const sensor::PointCloud& ranges) {
|
||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
|
||||
local_trajectory_builder_.AddRangefinderData(time, origin, ranges);
|
||||
if (insertion_result == nullptr) {
|
||||
return;
|
||||
}
|
||||
sparse_pose_graph_->AddScan(
|
||||
insertion_result->constant_data, insertion_result->pose_observation,
|
||||
trajectory_id_, insertion_result->insertion_submaps);
|
||||
}
|
||||
|
||||
void GlobalTrajectoryBuilder::AddSensorData(const sensor::ImuData& imu_data) {
|
||||
local_trajectory_builder_.AddImuData(imu_data);
|
||||
sparse_pose_graph_->AddImuData(trajectory_id_, imu_data);
|
||||
}
|
||||
|
||||
void GlobalTrajectoryBuilder::AddSensorData(
|
||||
const sensor::OdometryData& odometry_data) {
|
||||
local_trajectory_builder_.AddOdometerData(odometry_data);
|
||||
}
|
||||
|
||||
void GlobalTrajectoryBuilder::AddSensorData(
|
||||
const sensor::FixedFramePoseData& fixed_frame_pose) {
|
||||
sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
|
||||
}
|
||||
|
||||
const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const {
|
||||
return local_trajectory_builder_.pose_estimate();
|
||||
}
|
||||
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
|
@ -1,57 +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_GLOBAL_TRAJECTORY_BUILDER_H_
|
||||
#define CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_
|
||||
|
||||
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||
#include "cartographer/mapping_3d/local_trajectory_builder.h"
|
||||
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
|
||||
#include "cartographer/mapping_3d/sparse_pose_graph.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
|
||||
class GlobalTrajectoryBuilder
|
||||
: public mapping::GlobalTrajectoryBuilderInterface {
|
||||
public:
|
||||
GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options,
|
||||
int trajectory_id,
|
||||
SparsePoseGraph* sparse_pose_graph);
|
||||
~GlobalTrajectoryBuilder() override;
|
||||
|
||||
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
|
||||
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
|
||||
|
||||
const mapping::PoseEstimate& pose_estimate() const override;
|
||||
|
||||
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
|
||||
const sensor::PointCloud& ranges) override;
|
||||
void AddSensorData(const sensor::ImuData& imu_data) override;
|
||||
void AddSensorData(const sensor::OdometryData& odometry_data) override;
|
||||
void AddSensorData(
|
||||
const sensor::FixedFramePoseData& fixed_frame_pose) override;
|
||||
|
||||
private:
|
||||
const int trajectory_id_;
|
||||
SparsePoseGraph* const sparse_pose_graph_;
|
||||
LocalTrajectoryBuilder local_trajectory_builder_;
|
||||
};
|
||||
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_
|
|
@ -58,9 +58,8 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
|
|||
}
|
||||
|
||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
||||
LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
|
||||
const Eigen::Vector3f& origin,
|
||||
const sensor::PointCloud& ranges) {
|
||||
LocalTrajectoryBuilder::AddRangeData(const common::Time time,
|
||||
const sensor::RangeData& range_data) {
|
||||
if (extrapolator_ == nullptr) {
|
||||
// Until we've initialized the extrapolator with our first IMU message, we
|
||||
// cannot compute the orientation of the rangefinder.
|
||||
|
@ -77,8 +76,7 @@ LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
|
|||
first_pose_estimate_.inverse() *
|
||||
extrapolator_->ExtrapolatePose(time).cast<float>();
|
||||
const sensor::RangeData range_data_in_first_tracking =
|
||||
sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}},
|
||||
tracking_delta);
|
||||
sensor::TransformRangeData(range_data, tracking_delta);
|
||||
for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) {
|
||||
const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin;
|
||||
const float range = delta.norm();
|
||||
|
|
|
@ -54,9 +54,8 @@ class LocalTrajectoryBuilder {
|
|||
LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
|
||||
|
||||
void AddImuData(const sensor::ImuData& imu_data);
|
||||
std::unique_ptr<InsertionResult> AddRangefinderData(
|
||||
common::Time time, const Eigen::Vector3f& origin,
|
||||
const sensor::PointCloud& ranges);
|
||||
std::unique_ptr<InsertionResult> AddRangeData(
|
||||
common::Time time, const sensor::RangeData& range_data);
|
||||
void AddOdometerData(const sensor::OdometryData& odometry_data);
|
||||
const mapping::PoseEstimate& pose_estimate() const;
|
||||
|
||||
|
|
|
@ -239,8 +239,10 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
|
|||
for (const TrajectoryNode& node : expected_trajectory) {
|
||||
AddLinearOnlyImuObservation(node.time, node.pose);
|
||||
const auto range_data = GenerateRangeData(node.pose);
|
||||
if (local_trajectory_builder_->AddRangefinderData(
|
||||
node.time, range_data.origin, range_data.returns) != nullptr) {
|
||||
if (local_trajectory_builder_->AddRangeData(
|
||||
node.time,
|
||||
sensor::RangeData{range_data.origin, range_data.returns, {}}) !=
|
||||
nullptr) {
|
||||
const auto pose_estimate = local_trajectory_builder_->pose_estimate();
|
||||
EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
|
||||
++num_poses;
|
||||
|
|
|
@ -150,6 +150,13 @@ void SparsePoseGraph::AddImuData(const int trajectory_id,
|
|||
});
|
||||
}
|
||||
|
||||
void SparsePoseGraph::AddOdometerData(
|
||||
int trajectory_id,
|
||||
const sensor::OdometryData& odometry_data) {
|
||||
// TODO(cschuet): Add support for handling OdometryData in the optimization
|
||||
// problem.
|
||||
}
|
||||
|
||||
void SparsePoseGraph::AddFixedFramePoseData(
|
||||
const int trajectory_id,
|
||||
const sensor::FixedFramePoseData& fixed_frame_pose_data) {
|
||||
|
|
|
@ -39,6 +39,7 @@
|
|||
#include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"
|
||||
#include "cartographer/mapping_3d/submaps.h"
|
||||
#include "cartographer/sensor/fixed_frame_pose_data.h"
|
||||
#include "cartographer/sensor/odometry_data.h"
|
||||
#include "cartographer/sensor/point_cloud.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
|
@ -76,6 +77,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
|||
EXCLUDES(mutex_);
|
||||
|
||||
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
|
||||
void AddOdometerData(int trajectory_id,
|
||||
const sensor::OdometryData& odometry_data);
|
||||
void AddFixedFramePoseData(
|
||||
int trajectory_id,
|
||||
const sensor::FixedFramePoseData& fixed_frame_pose_data);
|
||||
|
|
Loading…
Reference in New Issue