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

Simplify 3D visualization code. (#88) 

Removes the non-const Submaps accessor.
Adds -Werror=missing-braces.
master
Wolfgang Hess 2016-10-20 11:29:12 +02:00 committed by GitHub
parent 4b7e2efbd9
commit 5b16f4bcb6
24 changed files with 63 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/collated_trajectory_builder.cc
View File

@ -50,10 +50,6 @@ const Submaps* CollatedTrajectoryBuilder::submaps() const {
return wrapped_trajectory_builder_->submaps();
}
Submaps* CollatedTrajectoryBuilder::submaps() {
return wrapped_trajectory_builder_->submaps();
}
kalman_filter::PoseTracker* CollatedTrajectoryBuilder::pose_tracker() const {
return wrapped_trajectory_builder_->pose_tracker();
}

1
cartographer/mapping/collated_trajectory_builder.h
View File

@ -51,7 +51,6 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
delete;
const Submaps* submaps() const override;
Submaps* submaps() override;
kalman_filter::PoseTracker* pose_tracker() const override;
const PoseEstimate& pose_estimate() const override;

1
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -48,7 +48,6 @@ class GlobalTrajectoryBuilderInterface {
const GlobalTrajectoryBuilderInterface&) = delete;
virtual const Submaps* submaps() const = 0;
virtual Submaps* submaps() = 0;
virtual kalman_filter::PoseTracker* pose_tracker() const = 0;
virtual const PoseEstimate& pose_estimate() const = 0;

2
cartographer/mapping/sparse_pose_graph_test.cc
View File

@ -33,7 +33,7 @@ class FakeSubmaps : public Submaps {
void SubmapToProto(int, const std::vector<mapping::TrajectoryNode>&,
const transform::Rigid3d&,
proto::SubmapQuery::Response*) override {
proto::SubmapQuery::Response*) const override {
LOG(FATAL) << "Not implemented.";
}
};

2
cartographer/mapping/submaps.h
View File

@ -121,7 +121,7 @@ class Submaps {
virtual void SubmapToProto(
int index, const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
const transform::Rigid3d& global_submap_pose,
proto::SubmapQuery::Response* response) = 0;
proto::SubmapQuery::Response* response) const = 0;
protected:
static void AddProbabilityGridToResponse(

1
cartographer/mapping/trajectory_builder.h
View File

@ -78,7 +78,6 @@ class TrajectoryBuilder {
TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
virtual const Submaps* submaps() const = 0;
virtual Submaps* submaps() = 0;
virtual kalman_filter::PoseTracker* pose_tracker() const = 0;
virtual const PoseEstimate& pose_estimate() const = 0;

4
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -32,10 +32,6 @@ const Submaps* GlobalTrajectoryBuilder::submaps() const {
return local_trajectory_builder_.submaps();
}
Submaps* GlobalTrajectoryBuilder::submaps() {
return local_trajectory_builder_.submaps();
}
kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const {
return local_trajectory_builder_.pose_tracker();
}

1
cartographer/mapping_2d/global_trajectory_builder.h
View File

@ -35,7 +35,6 @@ class GlobalTrajectoryBuilder
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
const Submaps* submaps() const override;
Submaps* submaps() override;
kalman_filter::PoseTracker* pose_tracker() const override;
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
const override;

2
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -72,8 +72,6 @@ LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; }
Submaps* LocalTrajectoryBuilder::submaps() { return &submaps_; }
kalman_filter::PoseTracker* LocalTrajectoryBuilder::pose_tracker() const {
return pose_tracker_.get();
}

1
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -71,7 +71,6 @@ class LocalTrajectoryBuilder {
const kalman_filter::PoseCovariance& covariance);
const Submaps* submaps() const;
Submaps* submaps();
kalman_filter::PoseTracker* pose_tracker() const;
private:

2
cartographer/mapping_2d/submaps.cc
View File

@ -140,7 +140,7 @@ int Submaps::size() const { return submaps_.size(); }
void Submaps::SubmapToProto(
const int index, const std::vector<mapping::TrajectoryNode>&,
const transform::Rigid3d&,
mapping::proto::SubmapQuery::Response* const response) {
mapping::proto::SubmapQuery::Response* const response) const {
AddProbabilityGridToResponse(Get(index)->local_pose(),
Get(index)->probability_grid, response);
}

2
cartographer/mapping_2d/submaps.h
View File

@ -61,7 +61,7 @@ class Submaps : public mapping::Submaps {
void SubmapToProto(
int index, const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) override;
mapping::proto::SubmapQuery::Response* response) const override;
// Inserts 'laser_fan' into the Submap collection.
void InsertLaserFan(const sensor::LaserFan& laser_fan);

4
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -33,10 +33,6 @@ const mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() const {
return local_trajectory_builder_->submaps();
}
mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() {
return local_trajectory_builder_->submaps();
}
kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const {
return local_trajectory_builder_->pose_tracker();
}

1
cartographer/mapping_3d/global_trajectory_builder.h
View File

@ -38,7 +38,6 @@ class GlobalTrajectoryBuilder
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
const mapping_3d::Submaps* submaps() const override;
mapping_3d::Submaps* submaps() override;
kalman_filter::PoseTracker* pose_tracker() const override;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override;

2
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -46,7 +46,7 @@ KalmanLocalTrajectoryBuilder::KalmanLocalTrajectoryBuilder(
KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {}
mapping_3d::Submaps* KalmanLocalTrajectoryBuilder::submaps() {
const mapping_3d::Submaps* KalmanLocalTrajectoryBuilder::submaps() const {
return submaps_.get();
}

2
cartographer/mapping_3d/kalman_local_trajectory_builder.h
View File

@ -55,7 +55,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
const kalman_filter::PoseCovariance& covariance) override;
void AddTrajectoryNodeIndex(int trajectory_node_index) override;
mapping_3d::Submaps* submaps() override;
const mapping_3d::Submaps* submaps() const override;
const PoseEstimate& pose_estimate() const override;
kalman_filter::PoseTracker* pose_tracker() const override;

2
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

@ -63,7 +63,7 @@ class LocalTrajectoryBuilderInterface {
// to the latest inserted laser scan. This is used to remember which
// trajectory node should be used to visualize a Submap.
virtual void AddTrajectoryNodeIndex(int trajectory_node_index) = 0;
virtual mapping_3d::Submaps* submaps() = 0;
virtual const mapping_3d::Submaps* submaps() const = 0;
virtual const PoseEstimate& pose_estimate() const = 0;
virtual kalman_filter::PoseTracker* pose_tracker() const = 0;

2
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -112,7 +112,7 @@ OptimizingLocalTrajectoryBuilder::OptimizingLocalTrajectoryBuilder(
OptimizingLocalTrajectoryBuilder::~OptimizingLocalTrajectoryBuilder() {}
mapping_3d::Submaps* OptimizingLocalTrajectoryBuilder::submaps() {
const mapping_3d::Submaps* OptimizingLocalTrajectoryBuilder::submaps() const {
return submaps_.get();
}

2
cartographer/mapping_3d/optimizing_local_trajectory_builder.h
View File

@ -63,7 +63,7 @@ class OptimizingLocalTrajectoryBuilder
kalman_filter::PoseTracker* pose_tracker() const override { return nullptr; }
void AddTrajectoryNodeIndex(int trajectory_node_index) override;
mapping_3d::Submaps* submaps() override;
const mapping_3d::Submaps* submaps() const override;
const PoseEstimate& pose_estimate() const override;
private:

67
cartographer/mapping_3d/submaps.cc
View File

@ -20,7 +20,6 @@
#include <limits>
#include "cartographer/common/math.h"
#include "cartographer/common/port.h"
#include "cartographer/sensor/laser.h"
#include "glog/logging.h"
@ -235,7 +234,7 @@ int Submaps::size() const { return submaps_.size(); }
void Submaps::SubmapToProto(
int index, const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* const response) {
mapping::proto::SubmapQuery::Response* const response) const {
// Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane
// in the global map frame.
const HybridGrid& hybrid_grid = Get(index)->high_resolution_hybrid_grid;
@ -244,9 +243,10 @@ void Submaps::SubmapToProto(
// Compute a bounding box for the texture.
Eigen::Array2i min_index(INT_MAX, INT_MAX);
Eigen::Array2i max_index(INT_MIN, INT_MIN);
ExtractVoxelData(
hybrid_grid,
(global_submap_pose * Get(index)->local_pose().inverse()).cast<float>(),
const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
ExtractVoxelData(hybrid_grid,
(global_submap_pose * Get(index)->local_pose().inverse())
.cast<float>(),
&min_index, &max_index);
const int width = max_index.y() - min_index.y() + 1;
@ -254,10 +254,11 @@ void Submaps::SubmapToProto(
response->set_width(width);
response->set_height(height);
AccumulatePixelData(width, height, min_index, max_index);
ComputePixelValues(width, height);
const std::vector<PixelData> accumulated_pixel_data = AccumulatePixelData(
width, height, min_index, max_index, voxel_indices_and_probabilities);
const string cell_data = ComputePixelValues(accumulated_pixel_data);
common::FastGzipString(celldata_, response->mutable_cells());
common::FastGzipString(cell_data, response->mutable_cells());
*response->mutable_slice_pose() =
transform::ToProto(global_submap_pose.inverse() *
transform::Rigid3d::Translation(Eigen::Vector3d(
@ -315,13 +316,13 @@ void Submaps::AddSubmap(const Eigen::Vector3f& origin) {
num_laser_fans_in_last_submap_ = 0;
}
void Submaps::AccumulatePixelData(const int width, const int height,
const Eigen::Array2i& min_index,
const Eigen::Array2i& max_index) {
accumulated_pixel_data_.clear();
accumulated_pixel_data_.resize(width * height);
std::vector<Submaps::PixelData> Submaps::AccumulatePixelData(
const int width, const int height, const Eigen::Array2i& min_index,
const Eigen::Array2i& max_index,
const std::vector<Eigen::Array4i>& voxel_indices_and_probabilities) const {
std::vector<PixelData> accumulated_pixel_data(width * height);
for (const Eigen::Array4i& voxel_index_and_probability :
voxel_indices_and_probabilities_) {
voxel_indices_and_probabilities) {
const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
if ((pixel_index < min_index).any() || (pixel_index > max_index).any()) {
// Out of bounds. This could happen because of floating point inaccuracy.
@ -329,7 +330,7 @@ void Submaps::AccumulatePixelData(const int width, const int height,
}
const int x = max_index.x() - pixel_index[0];
const int y = max_index.y() - pixel_index[1];
PixelData& pixel = accumulated_pixel_data_[x * width + y];
PixelData& pixel = accumulated_pixel_data[x * width + y];
++pixel.count;
pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]);
pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]);
@ -338,13 +339,13 @@ void Submaps::AccumulatePixelData(const int width, const int height,
pixel.probability_sum += probability;
pixel.max_probability = std::max(pixel.max_probability, probability);
}
return accumulated_pixel_data;
}
void Submaps::ExtractVoxelData(const HybridGrid& hybrid_grid,
const transform::Rigid3f& transform,
Eigen::Array2i* min_index,
Eigen::Array2i* max_index) {
voxel_indices_and_probabilities_.clear();
std::vector<Eigen::Array4i> Submaps::ExtractVoxelData(
const HybridGrid& hybrid_grid, const transform::Rigid3f& transform,
Eigen::Array2i* min_index, Eigen::Array2i* max_index) const {
std::vector<Eigen::Array4i> voxel_indices_and_probabilities;
const float resolution_inverse = 1. / hybrid_grid.resolution();
constexpr double kXrayObstructedCellProbabilityLimit = 0.501;
@ -365,29 +366,28 @@ void Submaps::ExtractVoxelData(const HybridGrid& hybrid_grid,
common::RoundToInt(cell_center_global.z() * resolution_inverse),
probability_value);
voxel_indices_and_probabilities_.push_back(voxel_index_and_probability);
voxel_indices_and_probabilities.push_back(voxel_index_and_probability);
const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
*min_index = min_index->cwiseMin(pixel_index);
*max_index = max_index->cwiseMax(pixel_index);
}
return voxel_indices_and_probabilities;
}
void Submaps::ComputePixelValues(const int width, const int height) {
celldata_.resize(2 * width * height);
string Submaps::ComputePixelValues(
const std::vector<Submaps::PixelData>& accumulated_pixel_data) const {
string cell_data;
cell_data.reserve(2 * accumulated_pixel_data.size());
constexpr float kMinZDifference = 3.f;
constexpr float kFreeSpaceWeight = 0.15f;
auto it = celldata_.begin();
for (size_t i = 0; i < accumulated_pixel_data_.size(); ++i) {
const PixelData& pixel = accumulated_pixel_data_.at(i);
for (const PixelData& pixel : accumulated_pixel_data) {
// TODO(whess): Take into account submap rotation.
// TODO(whess): Document the approach and make it more independent from the
// chosen resolution.
const float z_difference = pixel.count > 0 ? pixel.max_z - pixel.min_z : 0;
if (z_difference < kMinZDifference) {
*it = 0; // value
++it;
*it = 0; // alpha
++it;
cell_data.push_back(0); // value
cell_data.push_back(0); // alpha
continue;
}
const float free_space = std::max(z_difference - pixel.count, 0.f);
@ -401,11 +401,10 @@ void Submaps::ComputePixelValues(const int width, const int height) {
128 - mapping::ProbabilityToLogOddsInteger(average_probability);
const uint8 alpha = delta > 0 ? 0 : -delta;
const uint8 value = delta > 0 ? delta : 0;
*it = value; // value
++it;
*it = (value || alpha) ? alpha : 1; // alpha
++it;
cell_data.push_back(value); // value
cell_data.push_back((value || alpha) ? alpha : 1); // alpha
}
return cell_data;
}
} // namespace mapping_3d

34
cartographer/mapping_3d/submaps.h
View File

@ -22,6 +22,7 @@
#include <vector>
#include "Eigen/Geometry"
#include "cartographer/common/port.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping_2d/laser_fan_inserter.h"
@ -66,7 +67,7 @@ class Submaps : public mapping::Submaps {
void SubmapToProto(
int index, const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) override;
mapping::proto::SubmapQuery::Response* response) const override;
// Inserts 'laser_fan' into the Submap collection.
void InsertLaserFan(const sensor::LaserFan& laser_fan);
@ -90,15 +91,21 @@ class Submaps : public mapping::Submaps {
};
void AddSubmap(const Eigen::Vector3f& origin);
void AccumulatePixelData(const int width, const int height,
const Eigen::Array2i& min_index,
const Eigen::Array2i& max_index);
void ExtractVoxelData(const HybridGrid& hybrid_grid,
const transform::Rigid3f& transform,
Eigen::Array2i* min_index, Eigen::Array2i* max_index);
std::vector<PixelData> AccumulatePixelData(
const int width, const int height, const Eigen::Array2i& min_index,
const Eigen::Array2i& max_index,
const std::vector<Eigen::Array4i>& voxel_indices_and_probabilities) const;
// The first three entries of each returned value are a cell_index and the
// last is the corresponding probability value. We batch them together like
// this to only have one vector and have better cache locality.
std::vector<Eigen::Array4i> ExtractVoxelData(
const HybridGrid& hybrid_grid, const transform::Rigid3f& transform,
Eigen::Array2i* min_index, Eigen::Array2i* max_index) const;
// Builds texture data containing interleaved value and alpha for the
// visualization from 'accumulated_pixel_data_' into 'celldata_'.
void ComputePixelValues(const int width, const int height);
// visualization from 'accumulated_pixel_data'.
string ComputePixelValues(
const std::vector<PixelData>& accumulated_pixel_data) const;
const proto::SubmapsOptions options_;
@ -110,15 +117,6 @@ class Submaps : public mapping::Submaps {
// Number of LaserFans inserted since the last Submap was added.
int num_laser_fans_in_last_submap_ = 0;
// The following members are used for visualization and kept around for
// performance reasons (mainly to avoid reallocations).
std::vector<PixelData> accumulated_pixel_data_;
string celldata_;
// The first three entries of this is are a cell_index and the last is the
// corresponding probability value. We batch them together like this to only
// have one vector and have better cache locality.
std::vector<Eigen::Array4i> voxel_indices_and_probabilities_;
};
} // namespace mapping_3d

4
cartographer/sensor/collator_test.cc
View File

@ -29,8 +29,8 @@ namespace sensor {
namespace {
TEST(Collator, Ordering) {
const std::array<string, 4> kSensorId = {"horizontal_laser", "vertical_laser",
"imu", "odometry"};
const std::array<string, 4> kSensorId = {
{"horizontal_laser", "vertical_laser", "imu", "odometry"}};
Data first(common::FromUniversal(100), sensor::LaserFan{});
Data second(common::FromUniversal(200), sensor::LaserFan{});
Data third(common::FromUniversal(300), Data::Imu{});

1
cmake/functions.cmake
View File

@ -331,6 +331,7 @@ macro(google_initialize_cartographer_project)
# Turn some warnings into errors.
google_add_flag(GOOG_CXX_FLAGS "-Werror=format-security")
google_add_flag(GOOG_CXX_FLAGS "-Werror=missing-braces")
google_add_flag(GOOG_CXX_FLAGS "-Werror=reorder")
google_add_flag(GOOG_CXX_FLAGS "-Werror=return-type")
google_add_flag(GOOG_CXX_FLAGS "-Werror=uninitialized")