From d4e04a3cdaf5e8243f68c7fc37dc53b5a352358c Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 19 Oct 2016 15:02:31 +0200 Subject: [PATCH] Unify to AddLaserFan for both 2D and 3D. (#81) --- .../mapping/global_trajectory_builder_interface.h | 6 ++---- .../mapping_2d/global_trajectory_builder.cc | 4 ++-- .../mapping_2d/global_trajectory_builder.h | 8 +++----- .../mapping_3d/global_trajectory_builder.cc | 6 +++--- .../mapping_3d/global_trajectory_builder.h | 8 ++------ .../mapping_3d/kalman_local_trajectory_builder.cc | 4 ++-- .../mapping_3d/kalman_local_trajectory_builder.h | 2 +- .../kalman_local_trajectory_builder_test.cc | 2 +- .../local_trajectory_builder_interface.h | 2 +- .../optimizing_local_trajectory_builder.cc | 2 +- .../optimizing_local_trajectory_builder.h | 2 +- .../sparse_pose_graph/spa_cost_function.h | 14 ++++++-------- 12 files changed, 25 insertions(+), 35 deletions(-) diff --git a/cartographer/mapping/global_trajectory_builder_interface.h b/cartographer/mapping/global_trajectory_builder_interface.h index 502aa80..656595e 100644 --- a/cartographer/mapping/global_trajectory_builder_interface.h +++ b/cartographer/mapping/global_trajectory_builder_interface.h @@ -78,13 +78,11 @@ class GlobalTrajectoryBuilderInterface { virtual kalman_filter::PoseTracker* pose_tracker() const = 0; virtual const PoseEstimate& pose_estimate() const = 0; - virtual void AddHorizontalLaserFan(common::Time time, - const sensor::LaserFan& laser_fan) = 0; + virtual void AddLaserFan(common::Time time, + const sensor::LaserFan& laser_fan) = 0; virtual void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) = 0; - virtual void AddLaserFan3D(common::Time time, - const sensor::LaserFan& laser_fan) = 0; virtual void AddOdometerPose( common::Time time, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) = 0; diff --git a/cartographer/mapping_2d/global_trajectory_builder.cc b/cartographer/mapping_2d/global_trajectory_builder.cc index f32c575..ff6242b 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.cc +++ b/cartographer/mapping_2d/global_trajectory_builder.cc @@ -40,8 +40,8 @@ kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const { return local_trajectory_builder_.pose_tracker(); } -void GlobalTrajectoryBuilder::AddHorizontalLaserFan( - const common::Time time, const sensor::LaserFan& laser_fan) { +void GlobalTrajectoryBuilder::AddLaserFan(const common::Time time, + const sensor::LaserFan& laser_fan) { std::unique_ptr insertion_result = local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan); if (insertion_result != nullptr) { diff --git a/cartographer/mapping_2d/global_trajectory_builder.h b/cartographer/mapping_2d/global_trajectory_builder.h index 996cb99..ee3f4ce 100644 --- a/cartographer/mapping_2d/global_trajectory_builder.h +++ b/cartographer/mapping_2d/global_trajectory_builder.h @@ -40,16 +40,14 @@ class GlobalTrajectoryBuilder const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate() const override; - void AddHorizontalLaserFan(common::Time time, - const sensor::LaserFan& laser_fan) override; + // Handles approximately horizontal laser fans. + void AddLaserFan(common::Time time, + const sensor::LaserFan& laser_fan) override; void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; void AddOdometerPose( common::Time time, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) override; - void AddLaserFan3D(common::Time, const sensor::LaserFan&) override { - LOG(FATAL) << "Not implemented."; - }; private: const proto::LocalTrajectoryBuilderOptions options_; diff --git a/cartographer/mapping_3d/global_trajectory_builder.cc b/cartographer/mapping_3d/global_trajectory_builder.cc index a38b175..05e209c 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.cc +++ b/cartographer/mapping_3d/global_trajectory_builder.cc @@ -49,10 +49,10 @@ void GlobalTrajectoryBuilder::AddImuData( sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity); } -void GlobalTrajectoryBuilder::AddLaserFan3D(const common::Time time, - const sensor::LaserFan& laser_fan) { +void GlobalTrajectoryBuilder::AddLaserFan(const common::Time time, + const sensor::LaserFan& laser_fan) { auto insertion_result = - local_trajectory_builder_->AddLaserFan3D(time, laser_fan); + local_trajectory_builder_->AddLaserFan(time, laser_fan); if (insertion_result == nullptr) { return; diff --git a/cartographer/mapping_3d/global_trajectory_builder.h b/cartographer/mapping_3d/global_trajectory_builder.h index b96db59..3a15d2e 100644 --- a/cartographer/mapping_3d/global_trajectory_builder.h +++ b/cartographer/mapping_3d/global_trajectory_builder.h @@ -42,17 +42,13 @@ class GlobalTrajectoryBuilder kalman_filter::PoseTracker* pose_tracker() const override; void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; - void AddLaserFan3D(common::Time time, - const sensor::LaserFan& laser_fan) override; + void AddLaserFan(common::Time time, + const sensor::LaserFan& laser_fan) override; void AddOdometerPose( common::Time time, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) override; const PoseEstimate& pose_estimate() const override; - void AddHorizontalLaserFan(common::Time, const sensor::LaserFan&) override { - LOG(FATAL) << "Not implemented."; - } - private: mapping_3d::SparsePoseGraph* const sparse_pose_graph_; std::unique_ptr local_trajectory_builder_; diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 81e26ad..adebe96 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -74,8 +74,8 @@ void KalmanLocalTrajectoryBuilder::AddImuData( } std::unique_ptr -KalmanLocalTrajectoryBuilder::AddLaserFan3D(const common::Time time, - const sensor::LaserFan& laser_fan) { +KalmanLocalTrajectoryBuilder::AddLaserFan(const common::Time time, + const sensor::LaserFan& laser_fan) { if (!pose_tracker_) { LOG(INFO) << "PoseTracker not yet initialized."; return nullptr; diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.h b/cartographer/mapping_3d/kalman_local_trajectory_builder.h index 87ba388..7006063 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.h +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.h @@ -48,7 +48,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; - std::unique_ptr AddLaserFan3D( + std::unique_ptr AddLaserFan( common::Time time, const sensor::LaserFan& laser_fan) override; void AddOdometerPose( common::Time time, const transform::Rigid3d& pose, diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc index ffa99ce..2070501 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc @@ -256,7 +256,7 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { int num_poses = 0; for (const TrajectoryNode& node : expected_trajectory) { AddLinearOnlyImuObservation(node.time, node.pose); - if (local_trajectory_builder_->AddLaserFan3D( + if (local_trajectory_builder_->AddLaserFan( node.time, GenerateLaserFan(node.pose)) != nullptr) { const auto pose_estimate = local_trajectory_builder_->pose_estimate(); EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1)); diff --git a/cartographer/mapping_3d/local_trajectory_builder_interface.h b/cartographer/mapping_3d/local_trajectory_builder_interface.h index e8918ff..8755f96 100644 --- a/cartographer/mapping_3d/local_trajectory_builder_interface.h +++ b/cartographer/mapping_3d/local_trajectory_builder_interface.h @@ -53,7 +53,7 @@ class LocalTrajectoryBuilderInterface { virtual void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) = 0; - virtual std::unique_ptr AddLaserFan3D( + virtual std::unique_ptr AddLaserFan( common::Time time, const sensor::LaserFan& laser_fan) = 0; virtual void AddOdometerPose( common::Time time, const transform::Rigid3d& pose, diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index 9bd78eb..1cca04f 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -133,7 +133,7 @@ void OptimizingLocalTrajectoryBuilder::AddOdometerPose( } std::unique_ptr -OptimizingLocalTrajectoryBuilder::AddLaserFan3D( +OptimizingLocalTrajectoryBuilder::AddLaserFan( const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) { CHECK_GT(laser_fan_in_tracking.returns.size(), 0); diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h index 1d5d9bd..c20f4f0 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.h +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.h @@ -52,7 +52,7 @@ class OptimizingLocalTrajectoryBuilder void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; - std::unique_ptr AddLaserFan3D( + std::unique_ptr AddLaserFan( common::Time time, const sensor::LaserFan& laser_fan_in_tracking) override; diff --git a/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h b/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h index 14e022e..26bd20e 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h +++ b/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h @@ -46,20 +46,18 @@ class SpaCostFunction { const T* const c_i_translation, const T* const c_j_rotation, const T* const c_j_translation) { const Eigen::Quaternion R_i_inverse(c_i_rotation[0], -c_i_rotation[1], - -c_i_rotation[2], - -c_i_rotation[3]); + -c_i_rotation[2], -c_i_rotation[3]); - const Eigen::Matrix delta( - c_j_translation[0] - c_i_translation[0], - c_j_translation[1] - c_i_translation[1], - c_j_translation[2] - c_i_translation[2]); + const Eigen::Matrix delta(c_j_translation[0] - c_i_translation[0], + c_j_translation[1] - c_i_translation[1], + c_j_translation[2] - c_i_translation[2]); const Eigen::Matrix h_translation = R_i_inverse * delta; const Eigen::Quaternion h_rotation_inverse = Eigen::Quaternion(c_j_rotation[0], -c_j_rotation[1], -c_j_rotation[2], -c_j_rotation[3]) * - Eigen::Quaternion(c_i_rotation[0], c_i_rotation[1], - c_i_rotation[2], c_i_rotation[3]); + Eigen::Quaternion(c_i_rotation[0], c_i_rotation[1], c_i_rotation[2], + c_i_rotation[3]); const Eigen::Matrix angle_axis_difference = transform::RotationQuaternionToAngleAxisVector(