12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Propagate LandmarksData to the PoseGraph. (#830) 

Propagate LandmarkData to the PoseGraph.
master
Alexander Belyaev 2018-01-19 09:41:24 +01:00 committed by GitHub
parent de5937856d
commit 746c9c83c8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 48 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/internal/mapping/global_trajectory_builder.h
View File

@ -106,6 +106,11 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
}
void AddSensorData(const std::string& sensor_id,
const sensor::LandmarkData& landmark_data) override {
pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
}
void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
local_slam_result_data) override {
CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "

6
cartographer/mapping/collated_trajectory_builder.h
View File

@ -69,6 +69,12 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
const sensor::FixedFramePoseData& fixed_frame_pose_data) override {
AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data));
}
void AddSensorData(const std::string& sensor_id,
const sensor::LandmarkData& landmark_data) override {
AddData(sensor::MakeDispatchable(sensor_id, landmark_data));
}
void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
local_slam_result_data) override {
AddData(std::move(local_slam_result_data));

5
cartographer/mapping/pose_graph.h
View File

@ -34,6 +34,7 @@
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/map_by_time.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/rigid_transform.h"
@ -71,6 +72,10 @@ class PoseGraph : public PoseGraphInterface {
int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) = 0;
// Inserts landmarks observations.
virtual void AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) = 0;
// Finishes the given trajectory.
virtual void FinishTrajectory(int trajectory_id) = 0;

4
cartographer/mapping/trajectory_builder_interface.h
View File

@ -29,6 +29,7 @@
#include "cartographer/mapping/submaps.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/timed_point_cloud_data.h"
@ -78,7 +79,8 @@ class TrajectoryBuilderInterface {
virtual void AddSensorData(
const std::string& sensor_id,
const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
virtual void AddSensorData(const std::string& sensor_id,
const sensor::LandmarkData& landmark_data) = 0;
// Allows to directly add local SLAM results to the 'PoseGraph'. Note that it
// is invalid to add local SLAM results for a trajectory that has a
// 'LocalTrajectoryBuilder'.

6
cartographer/mapping_2d/pose_graph.cc
View File

@ -172,6 +172,12 @@ void PoseGraph::AddFixedFramePoseData(
LOG(FATAL) << "Not yet implemented for 2D.";
}
void PoseGraph::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data)
EXCLUDES(mutex_) {
LOG(FATAL) << "Not yet implemented.";
}
void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);

3
cartographer/mapping_2d/pose_graph.h
View File

@ -85,6 +85,9 @@ class PoseGraph : public mapping::PoseGraph {
int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) override
EXCLUDES(mutex_);
void AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) override
EXCLUDES(mutex_);
void FinishTrajectory(int trajectory_id) override;
bool IsTrajectoryFinished(int trajectory_id) override;

6
cartographer/mapping_3d/pose_graph.cc
View File

@ -174,6 +174,12 @@ void PoseGraph::AddFixedFramePoseData(
});
}
void PoseGraph::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data)
EXCLUDES(mutex_) {
LOG(FATAL) << "Not yet implemented.";
}
void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);

3
cartographer/mapping_3d/pose_graph.h
View File

@ -85,6 +85,9 @@ class PoseGraph : public mapping::PoseGraph {
int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) override
EXCLUDES(mutex_);
void AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) override
EXCLUDES(mutex_);
void FinishTrajectory(int trajectory_id) override;
bool IsTrajectoryFinished(int trajectory_id) override;

2
cartographer_grpc/client_server_test.cc
View File

@ -78,6 +78,8 @@ class MockTrajectoryBuilder
MOCK_METHOD2(AddSensorData,
void(const std::string&,
const cartographer::sensor::FixedFramePoseData&));
MOCK_METHOD2(AddSensorData, void(const std::string&,
const cartographer::sensor::LandmarkData&));
// Some of the platforms we run on may ship with a version of gmock which does
// not yet support move-only types.

6
cartographer_grpc/mapping/trajectory_builder_stub.cc
View File

@ -127,6 +127,12 @@ void TrajectoryBuilderStub::AddSensorData(
fixed_frame_writer_.client_writer->Write(request);
}
void TrajectoryBuilderStub::AddSensorData(
const std::string& sensor_id,
const cartographer::sensor::LandmarkData& landmark_data) {
LOG(FATAL) << "Not implemented";
}
void TrajectoryBuilderStub::AddLocalSlamResultData(
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
local_slam_result_data) {

3
cartographer_grpc/mapping/trajectory_builder_stub.h
View File

@ -49,6 +49,9 @@ class TrajectoryBuilderStub
void AddSensorData(const std::string& sensor_id,
const cartographer::sensor::FixedFramePoseData&
fixed_frame_pose) override;
void AddSensorData(
const std::string& sensor_id,
const cartographer::sensor::LandmarkData& landmark_data) override;
void AddLocalSlamResultData(
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
local_slam_result_data) override;