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

Unify to AddLaserFan for both 2D and 3D. (#81)

master
Wolfgang Hess 2016-10-19 15:02:31 +02:00 committed by GitHub
parent 9031f0533a
commit d4e04a3cda
12 changed files with 25 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -78,13 +78,11 @@ class GlobalTrajectoryBuilderInterface {
virtual kalman_filter::PoseTracker* pose_tracker() const = 0; virtual kalman_filter::PoseTracker* pose_tracker() const = 0;
virtual const PoseEstimate& pose_estimate() const = 0; virtual const PoseEstimate& pose_estimate() const = 0;
virtual void AddHorizontalLaserFan(common::Time time, virtual void AddLaserFan(common::Time time,
const sensor::LaserFan& laser_fan) = 0; const sensor::LaserFan& laser_fan) = 0;
virtual void AddImuData(common::Time time, virtual void AddImuData(common::Time time,
const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) = 0; const Eigen::Vector3d& angular_velocity) = 0;
virtual void AddLaserFan3D(common::Time time,
const sensor::LaserFan& laser_fan) = 0;
virtual void AddOdometerPose( virtual void AddOdometerPose(
common::Time time, const transform::Rigid3d& pose, common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) = 0; const kalman_filter::PoseCovariance& covariance) = 0;

4
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -40,8 +40,8 @@ kalman_filter::PoseTracker* GlobalTrajectoryBuilder::pose_tracker() const {
return local_trajectory_builder_.pose_tracker(); return local_trajectory_builder_.pose_tracker();
} }
void GlobalTrajectoryBuilder::AddHorizontalLaserFan( void GlobalTrajectoryBuilder::AddLaserFan(const common::Time time,
const common::Time time, const sensor::LaserFan& laser_fan) { const sensor::LaserFan& laser_fan) {
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result = std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan); local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan);
if (insertion_result != nullptr) { if (insertion_result != nullptr) {

6
cartographer/mapping_2d/global_trajectory_builder.h
View File

@ -40,16 +40,14 @@ class GlobalTrajectoryBuilder
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate() const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
const override; const override;
void AddHorizontalLaserFan(common::Time time, // Handles approximately horizontal laser fans.
void AddLaserFan(common::Time time,
const sensor::LaserFan& laser_fan) override; const sensor::LaserFan& laser_fan) override;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override; const Eigen::Vector3d& angular_velocity) override;
void AddOdometerPose( void AddOdometerPose(
common::Time time, const transform::Rigid3d& pose, common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) override; const kalman_filter::PoseCovariance& covariance) override;
void AddLaserFan3D(common::Time, const sensor::LaserFan&) override {
LOG(FATAL) << "Not implemented.";
};
private: private:
const proto::LocalTrajectoryBuilderOptions options_; const proto::LocalTrajectoryBuilderOptions options_;

4
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -49,10 +49,10 @@ void GlobalTrajectoryBuilder::AddImuData(
sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity); sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity);
} }
void GlobalTrajectoryBuilder::AddLaserFan3D(const common::Time time, void GlobalTrajectoryBuilder::AddLaserFan(const common::Time time,
const sensor::LaserFan& laser_fan) { const sensor::LaserFan& laser_fan) {
auto insertion_result = auto insertion_result =
local_trajectory_builder_->AddLaserFan3D(time, laser_fan); local_trajectory_builder_->AddLaserFan(time, laser_fan);
if (insertion_result == nullptr) { if (insertion_result == nullptr) {
return; return;

6
cartographer/mapping_3d/global_trajectory_builder.h
View File

@ -42,17 +42,13 @@ class GlobalTrajectoryBuilder
kalman_filter::PoseTracker* pose_tracker() const override; kalman_filter::PoseTracker* pose_tracker() const override;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override; const Eigen::Vector3d& angular_velocity) override;
void AddLaserFan3D(common::Time time, void AddLaserFan(common::Time time,
const sensor::LaserFan& laser_fan) override; const sensor::LaserFan& laser_fan) override;
void AddOdometerPose( void AddOdometerPose(
common::Time time, const transform::Rigid3d& pose, common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) override; const kalman_filter::PoseCovariance& covariance) override;
const PoseEstimate& pose_estimate() const override; const PoseEstimate& pose_estimate() const override;
void AddHorizontalLaserFan(common::Time, const sensor::LaserFan&) override {
LOG(FATAL) << "Not implemented.";
}
private: private:
mapping_3d::SparsePoseGraph* const sparse_pose_graph_; mapping_3d::SparsePoseGraph* const sparse_pose_graph_;
std::unique_ptr<LocalTrajectoryBuilderInterface> local_trajectory_builder_; std::unique_ptr<LocalTrajectoryBuilderInterface> local_trajectory_builder_;

2
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -74,7 +74,7 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
} }
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult> std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::AddLaserFan3D(const common::Time time, KalmanLocalTrajectoryBuilder::AddLaserFan(const common::Time time,
const sensor::LaserFan& laser_fan) { const sensor::LaserFan& laser_fan) {
if (!pose_tracker_) { if (!pose_tracker_) {
LOG(INFO) << "PoseTracker not yet initialized."; LOG(INFO) << "PoseTracker not yet initialized.";

2
cartographer/mapping_3d/kalman_local_trajectory_builder.h
View File

@ -48,7 +48,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override; const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddLaserFan3D( std::unique_ptr<InsertionResult> AddLaserFan(
common::Time time, const sensor::LaserFan& laser_fan) override; common::Time time, const sensor::LaserFan& laser_fan) override;
void AddOdometerPose( void AddOdometerPose(
common::Time time, const transform::Rigid3d& pose, common::Time time, const transform::Rigid3d& pose,

2
cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc
View File

@ -256,7 +256,7 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
int num_poses = 0; int num_poses = 0;
for (const TrajectoryNode& node : expected_trajectory) { for (const TrajectoryNode& node : expected_trajectory) {
AddLinearOnlyImuObservation(node.time, node.pose); AddLinearOnlyImuObservation(node.time, node.pose);
if (local_trajectory_builder_->AddLaserFan3D( if (local_trajectory_builder_->AddLaserFan(
node.time, GenerateLaserFan(node.pose)) != nullptr) { node.time, GenerateLaserFan(node.pose)) != nullptr) {
const auto pose_estimate = local_trajectory_builder_->pose_estimate(); const auto pose_estimate = local_trajectory_builder_->pose_estimate();
EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1)); EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));

2
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

@ -53,7 +53,7 @@ class LocalTrajectoryBuilderInterface {
virtual void AddImuData(common::Time time, virtual void AddImuData(common::Time time,
const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) = 0; const Eigen::Vector3d& angular_velocity) = 0;
virtual std::unique_ptr<InsertionResult> AddLaserFan3D( virtual std::unique_ptr<InsertionResult> AddLaserFan(
common::Time time, const sensor::LaserFan& laser_fan) = 0; common::Time time, const sensor::LaserFan& laser_fan) = 0;
virtual void AddOdometerPose( virtual void AddOdometerPose(
common::Time time, const transform::Rigid3d& pose, common::Time time, const transform::Rigid3d& pose,

2
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -133,7 +133,7 @@ void OptimizingLocalTrajectoryBuilder::AddOdometerPose(
} }
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult> std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::AddLaserFan3D( OptimizingLocalTrajectoryBuilder::AddLaserFan(
const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) { const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) {
CHECK_GT(laser_fan_in_tracking.returns.size(), 0); CHECK_GT(laser_fan_in_tracking.returns.size(), 0);

2
cartographer/mapping_3d/optimizing_local_trajectory_builder.h
View File

@ -52,7 +52,7 @@ class OptimizingLocalTrajectoryBuilder
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override; const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddLaserFan3D( std::unique_ptr<InsertionResult> AddLaserFan(
common::Time time, common::Time time,
const sensor::LaserFan& laser_fan_in_tracking) override; const sensor::LaserFan& laser_fan_in_tracking) override;

10
cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h
View File

@ -46,11 +46,9 @@ class SpaCostFunction {
const T* const c_i_translation, const T* const c_j_rotation, const T* const c_i_translation, const T* const c_j_rotation,
const T* const c_j_translation) { const T* const c_j_translation) {
const Eigen::Quaternion<T> R_i_inverse(c_i_rotation[0], -c_i_rotation[1], const Eigen::Quaternion<T> R_i_inverse(c_i_rotation[0], -c_i_rotation[1],
-c_i_rotation[2], -c_i_rotation[2], -c_i_rotation[3]);
-c_i_rotation[3]);
const Eigen::Matrix<T, 3, 1> delta( const Eigen::Matrix<T, 3, 1> delta(c_j_translation[0] - c_i_translation[0],
c_j_translation[0] - c_i_translation[0],
c_j_translation[1] - c_i_translation[1], c_j_translation[1] - c_i_translation[1],
c_j_translation[2] - c_i_translation[2]); c_j_translation[2] - c_i_translation[2]);
const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta; const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;
@ -58,8 +56,8 @@ class SpaCostFunction {
const Eigen::Quaternion<T> h_rotation_inverse = const Eigen::Quaternion<T> h_rotation_inverse =
Eigen::Quaternion<T>(c_j_rotation[0], -c_j_rotation[1], Eigen::Quaternion<T>(c_j_rotation[0], -c_j_rotation[1],
-c_j_rotation[2], -c_j_rotation[3]) * -c_j_rotation[2], -c_j_rotation[3]) *
Eigen::Quaternion<T>(c_i_rotation[0], c_i_rotation[1], Eigen::Quaternion<T>(c_i_rotation[0], c_i_rotation[1], c_i_rotation[2],
c_i_rotation[2], c_i_rotation[3]); c_i_rotation[3]);
const Eigen::Matrix<T, 3, 1> angle_axis_difference = const Eigen::Matrix<T, 3, 1> angle_axis_difference =
transform::RotationQuaternionToAngleAxisVector( transform::RotationQuaternionToAngleAxisVector(