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

Remove dead code. (#26)

master
Wolfgang Hess 2016-09-22 17:53:32 +02:00 committed by GitHub
parent 0796d50b00
commit 14355a91a1
12 changed files with 10 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cartographer/mapping/map_builder.cc
View File

@ -30,7 +30,6 @@
#include "cartographer/sensor/voxel_filter.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "glog/log_severity.h"
#include "glog/logging.h"
namespace cartographer {

3
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -115,10 +115,9 @@ void LocalTrajectoryBuilder::ScanMatch(
const sensor::PointCloud2D filtered_point_cloud_in_tracking_2d =
adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.point_cloud);
if (options_.use_online_correlative_scan_matching()) {
kalman_filter::Pose2DCovariance unused_covariance_observation;
real_time_correlative_scan_matcher_.Match(
pose_prediction_2d, filtered_point_cloud_in_tracking_2d,
probability_grid, &initial_ceres_pose, &unused_covariance_observation);
probability_grid, &initial_ceres_pose);
}
transform::Rigid2d tracking_2d_to_map;

1
cartographer/mapping_2d/scan_matching/CMakeLists.txt
View File

@ -99,7 +99,6 @@ google_library(mapping_2d_scan_matching_real_time_correlative_scan_matcher
DEPENDS
common_lua_parameter_dictionary
common_math
kalman_filter_pose_tracker
mapping_2d_probability_grid
mapping_2d_scan_matching_correlative_scan_matcher
mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options

3
cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto
View File

@ -28,7 +28,4 @@ message RealTimeCorrelativeScanMatcherOptions {
// Weights applied to each part of the score.
optional double translation_delta_cost_weight = 3;
optional double rotation_delta_cost_weight = 4;
// Covariance estimate is multiplied by this scale.
optional double covariance_scale = 5;
}

22
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc
View File

@ -24,7 +24,6 @@
#include "Eigen/Geometry"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/math.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/transform.h"
@ -46,8 +45,6 @@ CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary->GetDouble("translation_delta_cost_weight"));
options.set_rotation_delta_cost_weight(
parameter_dictionary->GetDouble("rotation_delta_cost_weight"));
options.set_covariance_scale(
parameter_dictionary->GetDouble("covariance_scale"));
CHECK_GE(options.translation_delta_cost_weight(), 0.);
CHECK_GE(options.rotation_delta_cost_weight(), 0.);
return options;
@ -94,10 +91,9 @@ RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates(
double RealTimeCorrelativeScanMatcher::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud,
const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate,
kalman_filter::Pose2DCovariance* covariance) const {
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate) const {
CHECK_NOTNULL(pose_estimate);
CHECK_NOTNULL(covariance);
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
const sensor::PointCloud2D rotated_point_cloud =
@ -119,20 +115,6 @@ double RealTimeCorrelativeScanMatcher::Match(
ScoreCandidates(probability_grid, discrete_scans, search_parameters,
&candidates);
// Covariance computed as in "Real-Time Correlative Scan Matching" by Olson.
Eigen::Matrix3d K = Eigen::Matrix3d::Zero();
Eigen::Vector3d u = Eigen::Vector3d::Zero();
double s = 0.f;
for (const Candidate& candidate : candidates) {
const double p = candidate.score;
const Eigen::Vector3d xi(candidate.x, candidate.y, candidate.orientation);
K += (xi * xi.transpose()) * p;
u += xi * p;
s += p;
}
*covariance = (1. / s) * K - (1. / (s * s)) * (u * u.transpose());
*covariance *= options_.covariance_scale();
const Candidate& best_candidate =
*std::max_element(candidates.begin(), candidates.end());
*pose_estimate = transform::Rigid2d(

8
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h
View File

@ -41,7 +41,6 @@
#include <vector>
#include "Eigen/Core"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
@ -66,13 +65,12 @@ class RealTimeCorrelativeScanMatcher {
const RealTimeCorrelativeScanMatcher&) = delete;
// Aligns 'point_cloud' within the 'probability_grid' given an
// 'initial_pose_estimate' then updates 'pose_estimate' and 'covariance'
// with the result and returns the score.
// 'initial_pose_estimate' then updates 'pose_estimate' with the result and
// returns the score.
double Match(const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud2D& point_cloud,
const ProbabilityGrid& probability_grid,
transform::Rigid2d* pose_estimate,
kalman_filter::Pose2DCovariance* covariance) const;
transform::Rigid2d* pose_estimate) const;
// Computes the score for each Candidate in a collection. The cost is computed
// as the sum of probabilities, different from the Ceres CostFunctions:

1
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc
View File

@ -67,7 +67,6 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
"angular_search_window = 0.16, "
"translation_delta_cost_weight = 0., "
"rotation_delta_cost_weight = 0., "
"covariance_scale = 1., "
"}");
real_time_correlative_scan_matcher_ =
common::make_unique<RealTimeCorrelativeScanMatcher>(

1
cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc
View File

@ -102,7 +102,6 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
angular_search_window = math.rad(1.),
translation_delta_cost_weight = 1e-1,
rotation_delta_cost_weight = 1.,
covariance_scale = 1.,
},
pose_tracker = {
orientation_model_variance = 5e-4,

1
cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc
View File

@ -56,7 +56,6 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
angular_search_window = math.rad(1.),
translation_delta_cost_weight = 1e-1,
rotation_delta_cost_weight = 1.,
covariance_scale = 1.,
})text");
real_time_correlative_scan_matcher_.reset(
new RealTimeCorrelativeScanMatcher(

1
configuration_files/trajectory_builder_2d.lua
View File

@ -30,7 +30,6 @@ TRAJECTORY_BUILDER_2D = {
angular_search_window = math.rad(20.),
translation_delta_cost_weight = 1e-1,
rotation_delta_cost_weight = 1e-1,
covariance_scale = 1.,
},
ceres_scan_matcher = {

1
configuration_files/trajectory_builder_3d.lua
View File

@ -82,7 +82,6 @@ TRAJECTORY_BUILDER_3D = {
angular_search_window = math.rad(1.),
translation_delta_cost_weight = 1e-1,
rotation_delta_cost_weight = 1e-1,
covariance_scale = 1.,
},
},

9
scripts/update_cmakelists.py
View File

@ -14,7 +14,6 @@
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""A dumb CMakeLists.txt generator that relies on source name conventions."""
import os
@ -131,8 +130,8 @@ def ReadFileWithoutGoogleTargets(filename):
def main():
root_directory = os.path.realpath(os.path.join(
os.path.dirname(__file__), os.path.pardir, "cartographer"))
root_directory = os.path.realpath(
os.path.join(os.path.dirname(__file__), os.path.pardir, "cartographer"))
targets_by_src = {}
targets = []
@ -198,8 +197,8 @@ def main():
if lines:
outfile.write("\n".join(lines))
outfile.write("\n")
outfile.write("\n\n".join(t.Format(directory) for t in
targets_in_directory))
outfile.write("\n\n".join(
t.Format(directory) for t in targets_in_directory))
outfile.write("\n")