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

Removes LaserFan from public API. (#136)

master
Damon Kohler 2016-11-21 10:30:57 +01:00 committed by GitHub
parent c386bf050d
commit 5136a3a81e
15 changed files with 69 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/mapping/collated_trajectory_builder.cc
View File

@ -88,8 +88,9 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
data->imu.angular_velocity);
return;
case sensor::Data::Type::kLaserFan:
wrapped_trajectory_builder_->AddLaserFan(data->time, data->laser_fan);
case sensor::Data::Type::kRangefinder:
wrapped_trajectory_builder_->AddRangefinderData(
data->time, data->rangefinder.origin, data->rangefinder.ranges);
return;
case sensor::Data::Type::kOdometer:

5
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -50,8 +50,9 @@ class GlobalTrajectoryBuilderInterface {
virtual const Submaps* submaps() const = 0;
virtual const PoseEstimate& pose_estimate() const = 0;
virtual void AddLaserFan(common::Time time,
const sensor::LaserFan& laser_fan) = 0;
virtual void AddRangefinderData(common::Time time,
const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) = 0;
virtual void AddImuData(common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) = 0;

8
cartographer/mapping/trajectory_builder.h
View File

@ -83,10 +83,12 @@ class TrajectoryBuilder {
virtual void AddSensorData(const string& sensor_id,
std::unique_ptr<sensor::Data> data) = 0;
void AddLaserFan(const string& sensor_id, common::Time time,
const sensor::LaserFan& laser_fan) {
void AddRangefinderData(const string& sensor_id, common::Time time,
const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) {
AddSensorData(sensor_id,
common::make_unique<sensor::Data>(time, laser_fan));
common::make_unique<sensor::Data>(
time, sensor::Data::Rangefinder{origin, ranges}));
}
void AddImuData(const string& sensor_id, common::Time time,

8
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -32,10 +32,12 @@ const Submaps* GlobalTrajectoryBuilder::submaps() const {
return local_trajectory_builder_.submaps();
}
void GlobalTrajectoryBuilder::AddLaserFan(const common::Time time,
const sensor::LaserFan& laser_fan) {
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_.AddHorizontalLaserFan(time, laser_fan);
local_trajectory_builder_.AddHorizontalLaserFan(
time, sensor::LaserFan{origin, ranges, {}, {}});
if (insertion_result != nullptr) {
sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->tracking_to_tracking_2d,

8
cartographer/mapping_2d/global_trajectory_builder.h
View File

@ -38,10 +38,10 @@ class GlobalTrajectoryBuilder
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
const override;
// Projects 'laser_fan' to 2D, and therefore should be approximately
// horizontal.
void AddLaserFan(common::Time time,
const sensor::LaserFan& laser_fan) 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 AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override;
void AddOdometerData(

7
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -41,10 +41,11 @@ void GlobalTrajectoryBuilder::AddImuData(
sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity);
}
void GlobalTrajectoryBuilder::AddLaserFan(const common::Time time,
const sensor::LaserFan& laser_fan) {
void GlobalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) {
auto insertion_result =
local_trajectory_builder_->AddLaserFan(time, laser_fan);
local_trajectory_builder_->AddRangefinderData(time, origin, ranges);
if (insertion_result == nullptr) {
return;

4
cartographer/mapping_3d/global_trajectory_builder.h
View File

@ -40,8 +40,8 @@ class GlobalTrajectoryBuilder
const mapping_3d::Submaps* submaps() const override;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override;
void AddLaserFan(common::Time time,
const sensor::LaserFan& laser_fan) override;
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override;
void AddOdometerData(
common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) override;

8
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -70,8 +70,9 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
}
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::AddLaserFan(const common::Time time,
const sensor::LaserFan& laser_fan) {
KalmanLocalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) {
if (!pose_tracker_) {
LOG(INFO) << "PoseTracker not yet initialized.";
return nullptr;
@ -89,7 +90,8 @@ KalmanLocalTrajectoryBuilder::AddLaserFan(const common::Time time,
const transform::Rigid3f tracking_delta =
first_pose_prediction_.inverse() * pose_prediction.cast<float>();
const sensor::LaserFan laser_fan_in_first_tracking =
sensor::TransformLaserFan(laser_fan, tracking_delta);
sensor::TransformLaserFan(sensor::LaserFan{origin, ranges, {}, {}},
tracking_delta);
for (const Eigen::Vector3f& laser_return :
laser_fan_in_first_tracking.returns) {
const Eigen::Vector3f delta =

5
cartographer/mapping_3d/kalman_local_trajectory_builder.h
View File

@ -48,8 +48,9 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddLaserFan(
common::Time time, const sensor::LaserFan& laser_fan) 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,
const kalman_filter::PoseCovariance& covariance) override;

5
cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc
View File

@ -256,8 +256,9 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
int num_poses = 0;
for (const TrajectoryNode& node : expected_trajectory) {
AddLinearOnlyImuObservation(node.time, node.pose);
if (local_trajectory_builder_->AddLaserFan(
node.time, GenerateLaserFan(node.pose)) != nullptr) {
const auto laser_fan = GenerateLaserFan(node.pose);
if (local_trajectory_builder_->AddRangefinderData(
node.time, laser_fan.origin, laser_fan.returns) != nullptr) {
const auto pose_estimate = local_trajectory_builder_->pose_estimate();
EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
++num_poses;

5
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

@ -53,8 +53,9 @@ class LocalTrajectoryBuilderInterface {
virtual void AddImuData(common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) = 0;
virtual std::unique_ptr<InsertionResult> AddLaserFan(
common::Time time, const sensor::LaserFan& laser_fan) = 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,
const kalman_filter::PoseCovariance& covariance) = 0;

11
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -133,15 +133,16 @@ void OptimizingLocalTrajectoryBuilder::AddOdometerData(
}
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::AddLaserFan(
const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) {
CHECK_GT(laser_fan_in_tracking.returns.size(), 0);
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 laser_fan_in_tracking coming from?
sensor::PointCloud point_cloud;
for (const Eigen::Vector3f& laser_return : laser_fan_in_tracking.returns) {
const Eigen::Vector3f delta = laser_return - laser_fan_in_tracking.origin;
for (const Eigen::Vector3f& laser_return : ranges) {
const Eigen::Vector3f delta = laser_return - origin;
const float range = delta.norm();
if (range >= options_.laser_min_range()) {
if (range <= options_.laser_max_range()) {

7
cartographer/mapping_3d/optimizing_local_trajectory_builder.h
View File

@ -52,10 +52,9 @@ class OptimizingLocalTrajectoryBuilder
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddLaserFan(
common::Time time,
const sensor::LaserFan& laser_fan_in_tracking) override;
std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override;
void AddOdometerData(
const common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) override;

10
cartographer/sensor/collator_test.cc
View File

@ -31,12 +31,12 @@ namespace {
TEST(Collator, Ordering) {
const std::array<string, 4> kSensorId = {
{"horizontal_laser", "vertical_laser", "imu", "odometry"}};
Data zero(common::FromUniversal(0), sensor::LaserFan{});
Data first(common::FromUniversal(100), sensor::LaserFan{});
Data second(common::FromUniversal(200), sensor::LaserFan{});
Data zero(common::FromUniversal(0), sensor::Data::Rangefinder{});
Data first(common::FromUniversal(100), sensor::Data::Rangefinder{});
Data second(common::FromUniversal(200), sensor::Data::Rangefinder{});
Data third(common::FromUniversal(300), Data::Imu{});
Data fourth(common::FromUniversal(400), sensor::LaserFan{});
Data fifth(common::FromUniversal(500), sensor::LaserFan{});
Data fourth(common::FromUniversal(400), sensor::Data::Rangefinder{});
Data fifth(common::FromUniversal(500), sensor::Data::Rangefinder{});
Data sixth(common::FromUniversal(600), Data::Odometer{});
std::vector<std::pair<string, Data>> received;

25
cartographer/sensor/data.h
View File

@ -20,6 +20,7 @@
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/sensor/laser.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
@ -29,7 +30,17 @@ namespace sensor {
// filled in. It is only used for time ordering sensor data before passing it
// on.
struct Data {
enum class Type { kImu, kLaserFan, kOdometer };
enum class Type { kImu, kRangefinder, kOdometer };
struct Imu {
Eigen::Vector3d linear_acceleration;
Eigen::Vector3d angular_velocity;
};
struct Rangefinder {
Eigen::Vector3f origin;
PointCloud ranges;
};
struct Odometer {
transform::Rigid3d pose;
@ -38,17 +49,11 @@ struct Data {
kalman_filter::PoseCovariance covariance;
};
struct Imu {
Eigen::Vector3d linear_acceleration;
Eigen::Vector3d angular_velocity;
};
Data(const common::Time time, const Imu& imu)
: type(Type::kImu), time(time), imu(imu) {}
Data(const common::Time time,
const ::cartographer::sensor::LaserFan& laser_fan)
: type(Type::kLaserFan), time(time), laser_fan(laser_fan) {}
Data(const common::Time time, const Rangefinder& rangefinder)
: type(Type::kRangefinder), time(time), rangefinder(rangefinder) {}
Data(const common::Time time, const Odometer& odometer)
: type(Type::kOdometer), time(time), odometer(odometer) {}
@ -56,7 +61,7 @@ struct Data {
Type type;
common::Time time;
Imu imu;
sensor::LaserFan laser_fan;
Rangefinder rangefinder;
Odometer odometer;
};