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

6
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -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;

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();
}
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<LocalTrajectoryBuilder::InsertionResult> insertion_result =
local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan);
if (insertion_result != nullptr) {

8
cartographer/mapping_2d/global_trajectory_builder.h
View File

@ -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_;

6
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);
}
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;

8
cartographer/mapping_3d/global_trajectory_builder.h
View File

@ -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<LocalTrajectoryBuilderInterface> local_trajectory_builder_;

4
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -74,8 +74,8 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
}
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
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;

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,
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;
void AddOdometerPose(
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;
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));

2
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

@ -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<InsertionResult> AddLaserFan3D(
virtual std::unique_ptr<InsertionResult> AddLaserFan(
common::Time time, const sensor::LaserFan& laser_fan) = 0;
virtual void AddOdometerPose(
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>
OptimizingLocalTrajectoryBuilder::AddLaserFan3D(
OptimizingLocalTrajectoryBuilder::AddLaserFan(
const common::Time time, const sensor::LaserFan& laser_fan_in_tracking) {
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,
const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddLaserFan3D(
std::unique_ptr<InsertionResult> AddLaserFan(
common::Time time,
const sensor::LaserFan& laser_fan_in_tracking) override;

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

@ -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<T> 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<T, 3, 1> 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<T, 3, 1> 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<T, 3, 1> h_translation = R_i_inverse * delta;
const Eigen::Quaternion<T> h_rotation_inverse =
Eigen::Quaternion<T>(c_j_rotation[0], -c_j_rotation[1],
-c_j_rotation[2], -c_j_rotation[3]) *
Eigen::Quaternion<T>(c_i_rotation[0], c_i_rotation[1],
c_i_rotation[2], c_i_rotation[3]);
Eigen::Quaternion<T>(c_i_rotation[0], c_i_rotation[1], c_i_rotation[2],
c_i_rotation[3]);
const Eigen::Matrix<T, 3, 1> angle_axis_difference =
transform::RotationQuaternionToAngleAxisVector(