Add support for multi-trajectory to 3D SLAM. (#146)
parent
318607ccd1
commit
51a0ec06a1
|
@ -168,7 +168,7 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
|
|||
pose_estimate_ * odometry_correction_;
|
||||
const transform::Rigid3d model_prediction = pose_estimate_;
|
||||
// TODO(whess): Prefer IMU over odom orientation if configured?
|
||||
const transform::Rigid3d pose_prediction = odometry_prediction;
|
||||
const transform::Rigid3d& pose_prediction = odometry_prediction;
|
||||
|
||||
// Computes the rotation without yaw, as defined by GetYaw().
|
||||
const transform::Rigid3d tracking_to_tracking_2d =
|
||||
|
|
|
@ -38,7 +38,8 @@ void GlobalTrajectoryBuilder::AddImuData(
|
|||
const Eigen::Vector3d& angular_velocity) {
|
||||
local_trajectory_builder_->AddImuData(time, linear_acceleration,
|
||||
angular_velocity);
|
||||
sparse_pose_graph_->AddImuData(time, linear_acceleration, angular_velocity);
|
||||
sparse_pose_graph_->AddImuData(local_trajectory_builder_->submaps(), time,
|
||||
linear_acceleration, angular_velocity);
|
||||
}
|
||||
|
||||
void GlobalTrajectoryBuilder::AddRangefinderData(
|
||||
|
|
|
@ -136,12 +136,13 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
|
|||
}
|
||||
}
|
||||
|
||||
void SparsePoseGraph::AddImuData(common::Time time,
|
||||
void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
|
||||
common::Time time,
|
||||
const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
AddWorkItem([=]() REQUIRES(mutex_) {
|
||||
optimization_problem_.AddImuData(time, linear_acceleration,
|
||||
optimization_problem_.AddImuData(trajectory, time, linear_acceleration,
|
||||
angular_velocity);
|
||||
});
|
||||
}
|
||||
|
@ -202,7 +203,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
|||
submap_transforms_[matching_index] *
|
||||
matching_submap->local_pose().inverse() * pose;
|
||||
CHECK_EQ(scan_index, optimization_problem_.node_data().size());
|
||||
optimization_problem_.AddTrajectoryNode(time, optimized_pose);
|
||||
const mapping::Submaps* const scan_trajectory =
|
||||
trajectory_nodes_[scan_index].constant_data->trajectory;
|
||||
optimization_problem_.AddTrajectoryNode(scan_trajectory, time,
|
||||
optimized_pose);
|
||||
for (const Submap* submap : insertion_submaps) {
|
||||
const int submap_index = GetSubmapIndex(submap);
|
||||
CHECK(!submap_states_[submap_index].finished);
|
||||
|
@ -319,17 +323,13 @@ void SparsePoseGraph::RunFinalOptimization() {
|
|||
void SparsePoseGraph::RunOptimization() {
|
||||
if (!submap_transforms_.empty()) {
|
||||
transform::Rigid3d submap_0_pose;
|
||||
std::vector<const mapping::Submaps*> trajectories;
|
||||
{
|
||||
common::MutexLocker locker(&mutex_);
|
||||
CHECK(!submap_states_.empty());
|
||||
submap_0_pose = submap_states_.front().submap->local_pose();
|
||||
for (const auto& trajectory_node : trajectory_nodes_) {
|
||||
trajectories.push_back(trajectory_node.constant_data->trajectory);
|
||||
}
|
||||
}
|
||||
|
||||
optimization_problem_.Solve(constraints_, submap_0_pose, trajectories,
|
||||
optimization_problem_.Solve(constraints_, submap_0_pose,
|
||||
&submap_transforms_);
|
||||
common::MutexLocker locker(&mutex_);
|
||||
has_new_optimized_poses_ = true;
|
||||
|
|
|
@ -79,7 +79,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
|||
EXCLUDES(mutex_);
|
||||
|
||||
// Adds new IMU data to be used in the optimization.
|
||||
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
|
||||
void AddImuData(const mapping::Submaps* trajectory, common::Time time,
|
||||
const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity);
|
||||
|
||||
void RunFinalOptimization() override;
|
||||
|
|
|
@ -222,16 +222,25 @@ void ConstraintBuilder::ComputeConstraint(
|
|||
float score = 0.;
|
||||
transform::Rigid3d pose_estimate = transform::Rigid3d::Identity();
|
||||
|
||||
CHECK(!match_full_submap) << "match_full_submap not supported for 3D.";
|
||||
|
||||
if (match_full_submap) {
|
||||
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
|
||||
initial_pose.rotation(), filtered_point_cloud, point_cloud,
|
||||
options_.global_localization_min_score(), &score, &pose_estimate)) {
|
||||
CHECK_GT(score, options_.global_localization_min_score());
|
||||
trajectory_connectivity->Connect(scan_trajectory, submap_trajectory);
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
|
||||
initial_pose, filtered_point_cloud, point_cloud, options_.min_score(),
|
||||
&score, &pose_estimate)) {
|
||||
initial_pose, filtered_point_cloud, point_cloud,
|
||||
options_.min_score(), &score, &pose_estimate)) {
|
||||
// We've reported a successful local match.
|
||||
CHECK_GT(score, options_.min_score());
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
}
|
||||
{
|
||||
common::MutexLocker locker(&mutex_);
|
||||
score_histogram_.Add(score);
|
||||
|
|
|
@ -77,15 +77,18 @@ OptimizationProblem::OptimizationProblem(
|
|||
|
||||
OptimizationProblem::~OptimizationProblem() {}
|
||||
|
||||
void OptimizationProblem::AddImuData(common::Time time,
|
||||
void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory,
|
||||
const common::Time time,
|
||||
const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity) {
|
||||
imu_data_.push_back(ImuData{time, linear_acceleration, angular_velocity});
|
||||
imu_data_[trajectory].push_back(
|
||||
ImuData{time, linear_acceleration, angular_velocity});
|
||||
}
|
||||
|
||||
void OptimizationProblem::AddTrajectoryNode(
|
||||
common::Time time, const transform::Rigid3d& point_cloud_pose) {
|
||||
node_data_.push_back(NodeData{time, point_cloud_pose});
|
||||
const mapping::Submaps* const trajectory, const common::Time time,
|
||||
const transform::Rigid3d& point_cloud_pose) {
|
||||
node_data_.push_back(NodeData{trajectory, time, point_cloud_pose});
|
||||
}
|
||||
|
||||
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
|
||||
|
@ -96,13 +99,17 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
|
|||
void OptimizationProblem::Solve(
|
||||
const std::vector<Constraint>& constraints,
|
||||
const transform::Rigid3d& submap_0_transform,
|
||||
const std::vector<const mapping::Submaps*>& trajectories,
|
||||
std::vector<transform::Rigid3d>* submap_transforms) {
|
||||
std::vector<transform::Rigid3d>* const submap_transforms) {
|
||||
if (node_data_.empty()) {
|
||||
// Nothing to optimize.
|
||||
return;
|
||||
}
|
||||
CHECK(!imu_data_.empty());
|
||||
|
||||
// Compute the indices of consecutive nodes for each trajectory.
|
||||
std::map<const mapping::Submaps*, std::vector<size_t>> nodes_per_trajectory;
|
||||
for (size_t j = 0; j != node_data_.size(); ++j) {
|
||||
nodes_per_trajectory[node_data_[j].trajectory].push_back(j);
|
||||
}
|
||||
|
||||
ceres::Problem::Options problem_options;
|
||||
ceres::Problem problem(problem_options);
|
||||
|
@ -146,33 +153,39 @@ void OptimizationProblem::Solve(
|
|||
C_point_clouds[constraint.j].translation());
|
||||
}
|
||||
|
||||
CHECK(!node_data_.empty());
|
||||
CHECK_GE(trajectories.size(), node_data_.size());
|
||||
// Add constraints based on IMU observations of angular velocities and
|
||||
// linear acceleration.
|
||||
for (const auto& trajectory_nodes_pair : nodes_per_trajectory) {
|
||||
const mapping::Submaps* const trajectory = trajectory_nodes_pair.first;
|
||||
const std::vector<size_t>& node_indices = trajectory_nodes_pair.second;
|
||||
const std::deque<ImuData>& imu_data = imu_data_.at(trajectory);
|
||||
CHECK(!node_indices.empty());
|
||||
CHECK(!imu_data.empty());
|
||||
|
||||
// Add constraints for IMU observed data: angular velocities and
|
||||
// accelerations.
|
||||
auto it = imu_data_.cbegin();
|
||||
while ((it + 1) != imu_data_.cend() && (it + 1)->time <= node_data_[0].time) {
|
||||
// Skip IMU data before the first node of this trajectory.
|
||||
auto it = imu_data.cbegin();
|
||||
while ((it + 1) != imu_data.cend() &&
|
||||
(it + 1)->time <= node_data_[node_indices[0]].time) {
|
||||
++it;
|
||||
}
|
||||
|
||||
for (size_t j = 1; j < node_data_.size(); ++j) {
|
||||
for (size_t j = 1; j < node_indices.size(); ++j) {
|
||||
auto it2 = it;
|
||||
const IntegrateImuResult<double> result = IntegrateImu(
|
||||
imu_data_, node_data_[j - 1].time, node_data_[j].time, &it);
|
||||
if (j + 1 < node_data_.size()) {
|
||||
const common::Duration first_delta_time =
|
||||
node_data_[j].time - node_data_[j - 1].time;
|
||||
const common::Duration second_delta_time =
|
||||
node_data_[j + 1].time - node_data_[j].time;
|
||||
const common::Time first_center =
|
||||
node_data_[j - 1].time + first_delta_time / 2;
|
||||
const common::Time second_center =
|
||||
node_data_[j].time + second_delta_time / 2;
|
||||
const IntegrateImuResult<double> result =
|
||||
IntegrateImu(imu_data, node_data_[node_indices[j - 1]].time,
|
||||
node_data_[node_indices[j]].time, &it);
|
||||
if (j + 1 < node_indices.size()) {
|
||||
const common::Time first_time = node_data_[node_indices[j - 1]].time;
|
||||
const common::Time second_time = node_data_[node_indices[j]].time;
|
||||
const common::Time third_time = node_data_[node_indices[j + 1]].time;
|
||||
const common::Duration first_duration = second_time - first_time;
|
||||
const common::Duration second_duration = third_time - second_time;
|
||||
const common::Time first_center = first_time + first_duration / 2;
|
||||
const common::Time second_center = second_time + second_duration / 2;
|
||||
const IntegrateImuResult<double> result_to_first_center =
|
||||
IntegrateImu(imu_data_, node_data_[j - 1].time, first_center, &it2);
|
||||
IntegrateImu(imu_data, first_time, first_center, &it2);
|
||||
const IntegrateImuResult<double> result_center_to_center =
|
||||
IntegrateImu(imu_data_, first_center, second_center, &it2);
|
||||
IntegrateImu(imu_data, first_center, second_center, &it2);
|
||||
// 'delta_velocity' is the change in velocity from the point in time
|
||||
// halfway between the first and second poses to halfway between second
|
||||
// and third pose. It is computed from IMU data and still contains a
|
||||
|
@ -183,21 +196,25 @@ void OptimizationProblem::Solve(
|
|||
result_to_first_center.delta_rotation) *
|
||||
result_center_to_center.delta_velocity;
|
||||
problem.AddResidualBlock(
|
||||
new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3, 3,
|
||||
3, 1>(new AccelerationCostFunction(
|
||||
new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3,
|
||||
3, 3, 1>(
|
||||
new AccelerationCostFunction(
|
||||
options_.acceleration_weight(), delta_velocity,
|
||||
common::ToSeconds(first_delta_time),
|
||||
common::ToSeconds(second_delta_time))),
|
||||
nullptr, C_point_clouds[j].rotation(),
|
||||
C_point_clouds[j - 1].translation(), C_point_clouds[j].translation(),
|
||||
C_point_clouds[j + 1].translation(), &gravity_constant_);
|
||||
common::ToSeconds(first_duration),
|
||||
common::ToSeconds(second_duration))),
|
||||
nullptr, C_point_clouds[node_indices[j]].rotation(),
|
||||
C_point_clouds[node_indices[j - 1]].translation(),
|
||||
C_point_clouds[node_indices[j]].translation(),
|
||||
C_point_clouds[node_indices[j + 1]].translation(),
|
||||
&gravity_constant_);
|
||||
}
|
||||
problem.AddResidualBlock(
|
||||
new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>(
|
||||
new RotationCostFunction(options_.rotation_weight(),
|
||||
result.delta_rotation)),
|
||||
nullptr, C_point_clouds[j - 1].rotation(),
|
||||
C_point_clouds[j].rotation());
|
||||
nullptr, C_point_clouds[node_indices[j - 1]].rotation(),
|
||||
C_point_clouds[node_indices[j]].rotation());
|
||||
}
|
||||
}
|
||||
|
||||
// Solve.
|
||||
|
|
|
@ -36,6 +36,8 @@ namespace mapping_3d {
|
|||
namespace sparse_pose_graph {
|
||||
|
||||
struct NodeData {
|
||||
// TODO(whess): Keep nodes per trajectory instead.
|
||||
const mapping::Submaps* trajectory;
|
||||
common::Time time;
|
||||
transform::Rigid3d point_cloud_pose;
|
||||
};
|
||||
|
@ -53,26 +55,24 @@ class OptimizationProblem {
|
|||
OptimizationProblem(const OptimizationProblem&) = delete;
|
||||
OptimizationProblem& operator=(const OptimizationProblem&) = delete;
|
||||
|
||||
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
|
||||
void AddImuData(const mapping::Submaps* trajectory, common::Time time,
|
||||
const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity);
|
||||
void AddTrajectoryNode(common::Time time,
|
||||
void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
|
||||
const transform::Rigid3d& point_cloud_pose);
|
||||
|
||||
void SetMaxNumIterations(int32 max_num_iterations);
|
||||
|
||||
// Computes the optimized poses. The point cloud at 'point_cloud_poses[i]'
|
||||
// belongs to 'trajectories[i]'. Within a given trajectory, scans are expected
|
||||
// to be contiguous.
|
||||
// Computes the optimized poses.
|
||||
void Solve(const std::vector<Constraint>& constraints,
|
||||
const transform::Rigid3d& submap_0_transform,
|
||||
const std::vector<const mapping::Submaps*>& trajectories,
|
||||
std::vector<transform::Rigid3d>* submap_transforms);
|
||||
|
||||
const std::vector<NodeData>& node_data() const;
|
||||
|
||||
private:
|
||||
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
|
||||
std::deque<ImuData> imu_data_;
|
||||
std::map<const mapping::Submaps*, std::deque<ImuData>> imu_data_;
|
||||
std::vector<NodeData> node_data_;
|
||||
double gravity_constant_ = 9.8;
|
||||
};
|
||||
|
|
|
@ -118,15 +118,14 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
|
|||
NoisyNode{RandomTransform(10., 3.), RandomYawOnlyTransform(0.2, 0.3)});
|
||||
}
|
||||
|
||||
std::vector<const mapping::Submaps*> trajectories;
|
||||
common::Time now = common::FromUniversal(0);
|
||||
for (const NoisyNode& node : test_data) {
|
||||
trajectories.push_back(kTrajectory);
|
||||
const transform::Rigid3d pose =
|
||||
AddNoise(node.ground_truth_pose, node.noise);
|
||||
optimization_problem_.AddImuData(now, Eigen::Vector3d::UnitZ() * 9.81,
|
||||
optimization_problem_.AddImuData(kTrajectory, now,
|
||||
Eigen::Vector3d::UnitZ() * 9.81,
|
||||
Eigen::Vector3d::Zero());
|
||||
optimization_problem_.AddTrajectoryNode(now, pose);
|
||||
optimization_problem_.AddTrajectoryNode(kTrajectory, now, pose);
|
||||
now += common::FromSeconds(0.01);
|
||||
}
|
||||
|
||||
|
@ -166,7 +165,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
|
|||
node_data[j].point_cloud_pose);
|
||||
}
|
||||
|
||||
optimization_problem_.Solve(constraints, kSubmap0Transform, trajectories,
|
||||
optimization_problem_.Solve(constraints, kSubmap0Transform,
|
||||
&submap_transforms);
|
||||
|
||||
double translation_error_after = 0.;
|
||||
|
|
Loading…
Reference in New Issue