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

3
cartographer/io/ply_writing_points_processor.cc
View File

@ -34,7 +34,8 @@ 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"
string color_header = !has_color ? ""
: "property uchar red\n"
"property uchar green\n"
"property uchar blue\n";
std::ostringstream stream;

14
cartographer/mapping/trajectory_connectivity_test.cc
View File

@ -110,11 +110,9 @@ 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(),
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(),
EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(),
trajectory(i)) != five_cluster->end());
}
}
@ -123,13 +121,11 @@ 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),
EXPECT_EQ(0, trajectory_connectivity_.ConnectionCount(trajectory(i),
trajectory(i + 1)));
}
}

6
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -91,8 +91,7 @@ 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() *
laser_fan.origin + options_.laser_missing_echo_ray_length() *
(return_ - laser_fan.origin).normalized());
}
}
@ -101,7 +100,8 @@ 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,
cropped.origin,
sensor::VoxelFiltered(cropped.returns,
options_.laser_voxel_filter_size()),
sensor::VoxelFiltered(cropped.misses,
options_.laser_voxel_filter_size())};

3
cartographer/mapping_2d/probability_grid.h
View File

@ -106,8 +106,7 @@ 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)] !=
return limits_.Contains(xy_index) && cells_[GetIndexOfCell(xy_index)] !=
mapping::kUnknownProbabilityValue;
}

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;

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

@ -228,8 +228,7 @@ 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() *
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,

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

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

3
cartographer/mapping_2d/sparse_pose_graph.cc
View File

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

3
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -193,7 +193,8 @@ void KalmanLocalTrajectoryBuilder::AddOdometerData(
kalman_filter::PoseTracker::ModelFunction::k3D, time));
}
pose_tracker_->AddOdometerPoseObservation(
time, pose, kalman_filter::BuildPoseCovariance(
time, pose,
kalman_filter::BuildPoseCovariance(
options_.kalman_local_trajectory_builder_options()
.odometer_translational_variance(),
options_.kalman_local_trajectory_builder_options()

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

@ -228,8 +228,7 @@ 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_) /
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);

7
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(
EXPECT_LT(0., grid_difference * (interpolated_grid_.GetProbability(
x + sample + kSampleStep, y, z) -
interpolated_grid_.GetProbability(x + sample, y, z)));
interpolated_grid_.GetProbability(
x + sample, y, z)));
}
}
}

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

@ -61,9 +61,8 @@ 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(
max_probability = std::max(
max_probability, hybrid_grid.GetProbability(
Eigen::Array3i(x + dx, y + dy, z + dz)));
}
}

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

@ -68,8 +68,7 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms(
}
const float kSafetyMargin = 1.f - 1e-3f;
const float angular_step_size =
kSafetyMargin * std::acos(1.f -
common::Pow2(resolution) /
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);

13
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -215,12 +215,12 @@ 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,
constraints_.push_back(
Constraint{submap_index,
scan_index,
{constraint_transform, common::ComputeSpdMatrixSqrtInverse(
covariance,
options_.constraint_builder_options()
{constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())},
Constraint::INTRA_SUBMAP});
}
@ -293,7 +293,8 @@ void SparsePoseGraph::WaitForAllComputations() {
common::FromSeconds(1.))) {
std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. * (constraint_builder_.GetNumFinishedScans() -
<< 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
(trajectory_nodes_.size() -
num_finished_scans_at_start)

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

@ -132,19 +132,22 @@ 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{
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{
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{
2, j,
OptimizationProblem::Constraint::Pose{
kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
RandomTransform(1e3, 3.),
1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}});

14
cartographer/mapping_3d/submaps.cc
View File

@ -82,10 +82,10 @@ 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,
segments->push_back(
LaserSegment{laser_origin_xy,
laser_origin_xy + (kSliceHalfHeight - laser_origin_z) /
delta_z * delta_xy,
false});
}
} else {
@ -100,9 +100,9 @@ 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,
segments->push_back(
LaserSegment{laser_origin_xy + (kSliceHalfHeight - laser_origin_z) /
delta_z * delta_xy,
hit.head<2>(), true});
}
}