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

Cleaning up dependencies for constraint builder & optimization problem. (#877)

master
Alexander Belyaev 2018-02-01 20:59:22 +01:00 committed by Wally B. Feed
parent 9ca06bd06b
commit eed51b8bb6
7 changed files with 16 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping_2d/pose_graph/constraint_builder.h
View File

@ -30,8 +30,8 @@
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/common/mutex.h" #include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h" #include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/submaps.h" #include "cartographer/mapping_2d/submaps.h"
@ -58,7 +58,7 @@ transform::Rigid2d ComputeSubmapPose(const Submap& submap);
// This class is thread-safe. // This class is thread-safe.
class ConstraintBuilder { class ConstraintBuilder {
public: public:
using Constraint = mapping::PoseGraph::Constraint; using Constraint = mapping::PoseGraphInterface::Constraint;
using Result = std::vector<Constraint>; using Result = std::vector<Constraint>;
ConstraintBuilder( ConstraintBuilder(

3
cartographer/mapping_2d/pose_graph/optimization_problem.cc
View File

@ -160,7 +160,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
C_submaps.at(constraint.submap_id).data(), C_submaps.at(constraint.submap_id).data(),
C_nodes.at(constraint.node_id).data()); C_nodes.at(constraint.node_id).data());
} }
// Add penalties for violating odometry or changes between consecutive nodes // Add penalties for violating odometry or changes between consecutive nodes
// if odometry is not available. // if odometry is not available.
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
@ -211,7 +210,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
for (const auto& C_node_id_data : C_nodes) { for (const auto& C_node_id_data : C_nodes) {
node_data_.at(C_node_id_data.id).pose = ToPose(C_node_id_data.data); node_data_.at(C_node_id_data.id).pose = ToPose(C_node_id_data.data);
} }
} } // namespace pose_graph
const mapping::MapById<mapping::NodeId, NodeData>& const mapping::MapById<mapping::NodeId, NodeData>&
OptimizationProblem::node_data() const { OptimizationProblem::node_data() const {

4
cartographer/mapping_2d/pose_graph/optimization_problem.h
View File

@ -28,8 +28,8 @@
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/map_by_time.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
@ -53,7 +53,7 @@ struct SubmapData {
// Implements the SPA loop closure method. // Implements the SPA loop closure method.
class OptimizationProblem { class OptimizationProblem {
public: public:
using Constraint = mapping::PoseGraph::Constraint; using Constraint = mapping::PoseGraphInterface::Constraint;
explicit OptimizationProblem( explicit OptimizationProblem(
const mapping::pose_graph::proto::OptimizationProblemOptions& options); const mapping::pose_graph::proto::OptimizationProblemOptions& options);

9
cartographer/mapping_3d/pose_graph/constraint_builder.cc
View File

@ -172,7 +172,7 @@ void ConstraintBuilder::ComputeConstraint(
const mapping::TrajectoryNode::Data* const constant_data, const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid3d& global_node_pose, const transform::Rigid3d& global_node_pose,
const transform::Rigid3d& global_submap_pose, const transform::Rigid3d& global_submap_pose,
std::unique_ptr<OptimizationProblem::Constraint>* constraint) { std::unique_ptr<Constraint>* constraint) {
const SubmapScanMatcher* const submap_scan_matcher = const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_id); GetSubmapScanMatcher(submap_id);
@ -229,12 +229,12 @@ void ConstraintBuilder::ComputeConstraint(
submap_scan_matcher->low_resolution_hybrid_grid}}, submap_scan_matcher->low_resolution_hybrid_grid}},
&constraint_transform, &unused_summary); &constraint_transform, &unused_summary);
constraint->reset(new OptimizationProblem::Constraint{ constraint->reset(new Constraint{
submap_id, submap_id,
node_id, node_id,
{constraint_transform, options_.loop_closure_translation_weight(), {constraint_transform, options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight()}, options_.loop_closure_rotation_weight()},
OptimizationProblem::Constraint::INTER_SUBMAP}); Constraint::INTER_SUBMAP});
if (options_.log_matches()) { if (options_.log_matches()) {
std::ostringstream info; std::ostringstream info;
@ -270,8 +270,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
if (pending_computations_.empty()) { if (pending_computations_.empty()) {
CHECK_EQ(submap_queued_work_items_.size(), 0); CHECK_EQ(submap_queued_work_items_.size(), 0);
if (when_done_ != nullptr) { if (when_done_ != nullptr) {
for (const std::unique_ptr<OptimizationProblem::Constraint>& for (const std::unique_ptr<Constraint>& constraint : constraints_) {
constraint : constraints_) {
if (constraint != nullptr) { if (constraint != nullptr) {
result.push_back(*constraint); result.push_back(*constraint);
} }

5
cartographer/mapping_3d/pose_graph/constraint_builder.h
View File

@ -31,8 +31,9 @@
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/common/mutex.h" #include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping/trajectory_node.h"
#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/submaps.h" #include "cartographer/mapping_3d/submaps.h"
@ -54,7 +55,7 @@ namespace pose_graph {
// This class is thread-safe. // This class is thread-safe.
class ConstraintBuilder { class ConstraintBuilder {
public: public:
using Constraint = mapping::PoseGraph::Constraint; using Constraint = mapping::PoseGraphInterface::Constraint;
using Result = std::vector<Constraint>; using Result = std::vector<Constraint>;
ConstraintBuilder( ConstraintBuilder(

6
cartographer/mapping_3d/pose_graph/optimization_problem.cc
View File

@ -251,7 +251,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
C_nodes.at(constraint.node_id).rotation(), C_nodes.at(constraint.node_id).rotation(),
C_nodes.at(constraint.node_id).translation()); C_nodes.at(constraint.node_id).translation());
} }
// Add constraints based on IMU observations of angular velocities and // Add constraints based on IMU observations of angular velocities and
// linear acceleration. // linear acceleration.
if (fix_z_ == FixZ::kNo) { if (fix_z_ == FixZ::kNo) {
@ -400,7 +399,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
continue; continue;
} }
const mapping::PoseGraph::Constraint::Pose constraint_pose{ const Constraint::Pose constraint_pose{
*fixed_frame_pose, options_.fixed_frame_pose_translation_weight(), *fixed_frame_pose, options_.fixed_frame_pose_translation_weight(),
options_.fixed_frame_pose_rotation_weight()}; options_.fixed_frame_pose_rotation_weight()};
@ -435,7 +434,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation()); C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation());
} }
} }
// Solve. // Solve.
ceres::Solver::Summary summary; ceres::Solver::Summary summary;
ceres::Solve( ceres::Solve(
@ -472,7 +470,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
trajectory_data_.at(C_fixed_frame.first).fixed_frame = trajectory_data_.at(C_fixed_frame.first).fixed_frame =
C_fixed_frame.second.ToRigid(); C_fixed_frame.second.ToRigid();
} }
} } // namespace pose_graph
const mapping::MapById<mapping::NodeId, NodeData>& const mapping::MapById<mapping::NodeId, NodeData>&
OptimizationProblem::node_data() const { OptimizationProblem::node_data() const {

4
cartographer/mapping_3d/pose_graph/optimization_problem.h
View File

@ -28,8 +28,8 @@
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/map_by_time.h"
@ -53,7 +53,7 @@ struct SubmapData {
// Implements the SPA loop closure method. // Implements the SPA loop closure method.
class OptimizationProblem { class OptimizationProblem {
public: public:
using Constraint = mapping::PoseGraph::Constraint; using Constraint = mapping::PoseGraphInterface::Constraint;
enum class FixZ { kYes, kNo }; enum class FixZ { kYes, kNo };