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

Formatting changes. (#199)

master
Wolfgang Hess 2017-02-08 14:30:38 +01:00 committed by GitHub
parent ddb3c890a6
commit b4a1021538
16 changed files with 84 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/io/ply_writing_points_processor.cc
View File

@ -34,9 +34,10 @@ namespace {
// 'output_file'.
void WriteBinaryPlyHeader(const bool has_color, const int64 num_points,
FileWriter* const file_writer) {
string color_header = !has_color ? "" : "property uchar red\n"
"property uchar green\n"
"property uchar blue\n";
string color_header = !has_color ? ""
: "property uchar red\n"
"property uchar green\n"
"property uchar blue\n";
std::ostringstream stream;
stream << "ply\n"
<< "format binary_little_endian 1.0\n"

20
cartographer/mapping/trajectory_connectivity_test.cc
View File

@ -110,12 +110,10 @@ TEST_F(TrajectoryConnectivityTest, ConnectedComponents) {
five_cluster = &connections[0];
}
for (int i = 0; i <= 9; ++i) {
EXPECT_EQ(i <= 4,
std::find(zero_cluster->begin(), zero_cluster->end(),
trajectory(i)) != zero_cluster->end());
EXPECT_EQ(i > 4,
std::find(five_cluster->begin(), five_cluster->end(),
trajectory(i)) != five_cluster->end());
EXPECT_EQ(i <= 4, std::find(zero_cluster->begin(), zero_cluster->end(),
trajectory(i)) != zero_cluster->end());
EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(),
trajectory(i)) != five_cluster->end());
}
}
@ -123,14 +121,12 @@ TEST_F(TrajectoryConnectivityTest, ConnectionCount) {
for (int i = 0; i < 10; ++i) {
trajectory_connectivity_.Connect(trajectory(0), trajectory(1));
// Permute the arguments to check invariance.
EXPECT_EQ(
i + 1,
trajectory_connectivity_.ConnectionCount(trajectory(1), trajectory(0)));
EXPECT_EQ(i + 1, trajectory_connectivity_.ConnectionCount(trajectory(1),
trajectory(0)));
}
for (int i = 1; i < 9; ++i) {
EXPECT_EQ(0,
trajectory_connectivity_.ConnectionCount(trajectory(i),
trajectory(i + 1)));
EXPECT_EQ(0, trajectory_connectivity_.ConnectionCount(trajectory(i),
trajectory(i + 1)));
}
}

10
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -91,9 +91,8 @@ sensor::LaserFan LocalTrajectoryBuilder::TransformAndFilterLaserFan(
returns_and_misses.returns.push_back(return_);
} else {
returns_and_misses.misses.push_back(
laser_fan.origin +
options_.laser_missing_echo_ray_length() *
(return_ - laser_fan.origin).normalized());
laser_fan.origin + options_.laser_missing_echo_ray_length() *
(return_ - laser_fan.origin).normalized());
}
}
}
@ -101,8 +100,9 @@ sensor::LaserFan LocalTrajectoryBuilder::TransformAndFilterLaserFan(
sensor::TransformLaserFan(returns_and_misses, tracking_to_tracking_2d),
options_.laser_min_z(), options_.laser_max_z());
return sensor::LaserFan{
cropped.origin, sensor::VoxelFiltered(cropped.returns,
options_.laser_voxel_filter_size()),
cropped.origin,
sensor::VoxelFiltered(cropped.returns,
options_.laser_voxel_filter_size()),
sensor::VoxelFiltered(cropped.misses,
options_.laser_voxel_filter_size())};
}

5
cartographer/mapping_2d/probability_grid.h
View File

@ -106,9 +106,8 @@ class ProbabilityGrid {
// Returns true if the probability at the specified index is known.
bool IsKnown(const Eigen::Array2i& xy_index) const {
return limits_.Contains(xy_index) &&
cells_[GetIndexOfCell(xy_index)] !=
mapping::kUnknownProbabilityValue;
return limits_.Contains(xy_index) && cells_[GetIndexOfCell(xy_index)] !=
mapping::kUnknownProbabilityValue;
}
// Fills in 'offset' and 'limits' to define a subregion of that contains all

5
cartographer/mapping_2d/scan_matching/correlative_scan_matcher.cc
View File

@ -38,9 +38,8 @@ SearchParameters::SearchParameters(const double linear_search_window,
}
const double kSafetyMargin = 1. - 1e-3;
angular_perturbation_step_size =
kSafetyMargin *
std::acos(1. -
common::Pow2(resolution) / (2. * common::Pow2(max_scan_range)));
kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
(2. * common::Pow2(max_scan_range)));
num_angular_perturbations =
std::ceil(angular_search_window / angular_perturbation_step_size);
num_scans = 2 * num_angular_perturbations + 1;

7
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -228,10 +228,9 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
M_PI, // Angular search window, 180 degrees in both directions.
point_cloud, limits_.resolution());
const transform::Rigid2d center = transform::Rigid2d::Translation(
limits_.max() -
0.5 * limits_.resolution() *
Eigen::Vector2d(limits_.cell_limits().num_y_cells,
limits_.cell_limits().num_x_cells));
limits_.max() - 0.5 * limits_.resolution() *
Eigen::Vector2d(limits_.cell_limits().num_y_cells,
limits_.cell_limits().num_x_cells));
return MatchWithSearchParameters(search_parameters, center, point_cloud,
min_score, score, pose_estimate);
}

10
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

@ -63,8 +63,9 @@ TEST(PrecomputationGridTest, CorrectValues) {
probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y)));
}
}
EXPECT_NEAR(max_score, PrecomputationGrid::ToProbability(
precomputation_grid.GetValue(xy_index)),
EXPECT_NEAR(max_score,
PrecomputationGrid::ToProbability(
precomputation_grid.GetValue(xy_index)),
1e-4);
}
}
@ -97,8 +98,9 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y)));
}
}
EXPECT_NEAR(max_score, PrecomputationGrid::ToProbability(
precomputation_grid.GetValue(xy_index)),
EXPECT_NEAR(max_score,
PrecomputationGrid::ToProbability(
precomputation_grid.GetValue(xy_index)),
1e-4);
}
}

5
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -302,8 +302,9 @@ void SparsePoseGraph::WaitForAllComputations() {
common::FromSeconds(1.))) {
std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. * (constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
<< 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
(trajectory_nodes_.size() -
num_finished_scans_at_start)
<< "%...";

11
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -193,11 +193,12 @@ void KalmanLocalTrajectoryBuilder::AddOdometerData(
kalman_filter::PoseTracker::ModelFunction::k3D, time));
}
pose_tracker_->AddOdometerPoseObservation(
time, pose, kalman_filter::BuildPoseCovariance(
options_.kalman_local_trajectory_builder_options()
.odometer_translational_variance(),
options_.kalman_local_trajectory_builder_options()
.odometer_rotational_variance()));
time, pose,
kalman_filter::BuildPoseCovariance(
options_.kalman_local_trajectory_builder_options()
.odometer_translational_variance(),
options_.kalman_local_trajectory_builder_options()
.odometer_rotational_variance()));
}
const KalmanLocalTrajectoryBuilder::PoseEstimate&

5
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -228,9 +228,8 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
}
const float kSafetyMargin = 1.f - 1e-2f;
const float angular_step_size =
kSafetyMargin * std::acos(1.f -
common::Pow2(resolution_) /
(2.f * common::Pow2(max_scan_range)));
kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) /
(2.f * common::Pow2(max_scan_range)));
const int angular_window_size = common::RoundToInt(
search_parameters.angular_search_window / angular_step_size);
// TODO(whess): Should there be a small search window for rotations around

9
cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc
View File

@ -75,11 +75,10 @@ TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) {
for (double sample = kSampleStep;
sample < hybrid_grid_.resolution() - 2 * kSampleStep;
sample += kSampleStep) {
EXPECT_LT(0.,
grid_difference *
(interpolated_grid_.GetProbability(
x + sample + kSampleStep, y, z) -
interpolated_grid_.GetProbability(x + sample, y, z)));
EXPECT_LT(0., grid_difference * (interpolated_grid_.GetProbability(
x + sample + kSampleStep, y, z) -
interpolated_grid_.GetProbability(
x + sample, y, z)));
}
}
}

7
cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc
View File

@ -61,10 +61,9 @@ TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
for (int dx = 0; dx < width; ++dx) {
for (int dy = 0; dy < width; ++dy) {
for (int dz = 0; dz < width; ++dz) {
max_probability =
std::max(max_probability,
hybrid_grid.GetProbability(
Eigen::Array3i(x + dx, y + dy, z + dz)));
max_probability = std::max(
max_probability, hybrid_grid.GetProbability(
Eigen::Array3i(x + dx, y + dy, z + dz)));
}
}
}

5
cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc
View File

@ -68,9 +68,8 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms(
}
const float kSafetyMargin = 1.f - 1e-3f;
const float angular_step_size =
kSafetyMargin * std::acos(1.f -
common::Pow2(resolution) /
(2.f * common::Pow2(max_scan_range)));
kSafetyMargin * std::acos(1.f - common::Pow2(resolution) /
(2.f * common::Pow2(max_scan_range)));
const int angular_window_size =
common::RoundToInt(options_.angular_search_window() / angular_step_size);
for (int z = -linear_window_size; z <= linear_window_size; ++z) {

21
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -215,14 +215,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
// Unchanged covariance as (submap <- map) is a translation.
const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose;
constraints_.push_back(Constraint{
submap_index,
scan_index,
{constraint_transform, common::ComputeSpdMatrixSqrtInverse(
covariance,
options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())},
Constraint::INTRA_SUBMAP});
constraints_.push_back(
Constraint{submap_index,
scan_index,
{constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())},
Constraint::INTRA_SUBMAP});
}
CHECK_LT(submap_states_.size(), std::numeric_limits<int>::max());
@ -293,8 +293,9 @@ void SparsePoseGraph::WaitForAllComputations() {
common::FromSeconds(1.))) {
std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. * (constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
<< 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
(trajectory_nodes_.size() -
num_finished_scans_at_start)
<< "%...";

25
cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc
View File

@ -132,22 +132,25 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
std::vector<OptimizationProblem::Constraint> constraints;
for (int j = 0; j != kNumNodes; ++j) {
constraints.push_back(OptimizationProblem::Constraint{
0, j, OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose, test_data[j].noise),
Eigen::Matrix<double, 6, 6>::Identity()}});
0, j,
OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose, test_data[j].noise),
Eigen::Matrix<double, 6, 6>::Identity()}});
// We add an additional independent, but equally noisy observation.
constraints.push_back(OptimizationProblem::Constraint{
1, j, OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose,
RandomYawOnlyTransform(0.2, 0.3)),
Eigen::Matrix<double, 6, 6>::Identity()}});
1, j,
OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose,
RandomYawOnlyTransform(0.2, 0.3)),
Eigen::Matrix<double, 6, 6>::Identity()}});
// We add very noisy data with high covariance (i.e. small Lambda) to verify
// it is mostly ignored.
constraints.push_back(OptimizationProblem::Constraint{
2, j, OptimizationProblem::Constraint::Pose{
kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
RandomTransform(1e3, 3.),
1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}});
2, j,
OptimizationProblem::Constraint::Pose{
kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
RandomTransform(1e3, 3.),
1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}});
}
std::vector<transform::Rigid3d> submap_transforms = {

18
cartographer/mapping_3d/submaps.cc
View File

@ -82,11 +82,11 @@ void GenerateSegmentForSlice(const sensor::LaserFan& laser_fan,
segments->push_back(LaserSegment{laser_origin_xy, hit.head<2>(), true});
} else {
// Laser hit is above.
segments->push_back(LaserSegment{
laser_origin_xy,
laser_origin_xy +
(kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy,
false});
segments->push_back(
LaserSegment{laser_origin_xy,
laser_origin_xy + (kSliceHalfHeight - laser_origin_z) /
delta_z * delta_xy,
false});
}
} else {
// Laser ray originates above the slice.
@ -100,10 +100,10 @@ void GenerateSegmentForSlice(const sensor::LaserFan& laser_fan,
false});
} else if (hit.z() < kSliceHalfHeight) {
// Laser return is inside the slice.
segments->push_back(LaserSegment{
laser_origin_xy +
(kSliceHalfHeight - laser_origin_z) / delta_z * delta_xy,
hit.head<2>(), true});
segments->push_back(
LaserSegment{laser_origin_xy + (kSliceHalfHeight - laser_origin_z) /
delta_z * delta_xy,
hit.head<2>(), true});
}
}
}