From 5899bff425bb8831e7cf2b84b610cc65fa0b4cd9 Mon Sep 17 00:00:00 2001 From: Jihoon Lee Date: Tue, 7 Jul 2020 20:18:53 +0900 Subject: [PATCH] FixedFramePose in optimization 2d (#1580) --- .../mapping/internal/2d/pose_graph_2d.cc | 37 +++++-- .../optimization/optimization_problem_2d.cc | 96 +++++++++++++++++++ .../optimization/optimization_problem_2d.h | 17 ++++ 3 files changed, 143 insertions(+), 7 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index b3620f5..6ccf43b 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -229,7 +229,14 @@ void PoseGraph2D::AddOdometryData(const int trajectory_id, void PoseGraph2D::AddFixedFramePoseData( const int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) { - LOG(FATAL) << "Not yet implemented for 2D."; + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->AddFixedFramePoseData(trajectory_id, + fixed_frame_pose_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); } void PoseGraph2D::AddLandmarkData(int trajectory_id, @@ -753,7 +760,24 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose, void PoseGraph2D::SetTrajectoryDataFromProto( const proto::TrajectoryData& data) { - LOG(ERROR) << "not implemented"; + TrajectoryData trajectory_data; + // gravity_constant and imu_calibration are omitted as its not used in 2d + + if (data.has_fixed_frame_origin_in_map()) { + trajectory_data.fixed_frame_origin_in_map = + transform::ToRigid3(data.fixed_frame_origin_in_map()); + + const int trajectory_id = data.trajectory_id(); + AddWorkItem([this, trajectory_id, trajectory_data]() + LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->SetTrajectoryData( + trajectory_id, trajectory_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); + } } void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id, @@ -984,15 +1008,14 @@ PoseGraph2D::GetLandmarkNodes() const { std::map PoseGraph2D::GetTrajectoryData() const { - // The 2D optimization problem does not have any 'TrajectoryData'. - return {}; + absl::MutexLock locker(&mutex_); + return optimization_problem_->trajectory_data(); } sensor::MapByTime PoseGraph2D::GetFixedFramePoseData() const { - // FixedFramePoseData is not yet implemented for 2D. We need to return empty - // so serialization works. - return {}; + absl::MutexLock locker(&mutex_); + return optimization_problem_->fixed_frame_pose_data(); } std::vector PoseGraph2D::constraints() const { diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc index cd0eafc..508cae7 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc @@ -42,6 +42,35 @@ namespace { using ::cartographer::mapping::optimization::CeresPose; using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; +using TrajectoryData = + ::cartographer::mapping::PoseGraphInterface::TrajectoryData; + +// For fixed frame pose. +std::unique_ptr Interpolate( + const sensor::MapByTime& map_by_time, + const int trajectory_id, const common::Time time) { + const auto it = map_by_time.lower_bound(trajectory_id, time); + if (it == map_by_time.EndOfTrajectory(trajectory_id) || + !it->pose.has_value()) { + return nullptr; + } + if (it == map_by_time.BeginOfTrajectory(trajectory_id)) { + if (it->time == time) { + return absl::make_unique(it->pose.value()); + } + return nullptr; + } + const auto prev_it = std::prev(it); + if (prev_it->pose.has_value()) { + return absl::make_unique( + Interpolate(transform::TimestampedTransform{prev_it->time, + prev_it->pose.value()}, + transform::TimestampedTransform{it->time, it->pose.value()}, + time) + .transform); + } + return nullptr; +} // Converts a pose into the 3 optimization variable format used for Ceres: // translation in x and y, followed by the rotation angle representing the @@ -154,20 +183,36 @@ void OptimizationProblem2D::AddOdometryData( odometry_data_.Append(trajectory_id, odometry_data); } +void OptimizationProblem2D::AddFixedFramePoseData( + const int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) { + fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data); +} + void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id, const NodeSpec2D& node_data) { node_data_.Append(trajectory_id, node_data); + trajectory_data_[trajectory_id]; +} + +void OptimizationProblem2D::SetTrajectoryData( + int trajectory_id, const TrajectoryData& trajectory_data) { + trajectory_data_[trajectory_id] = trajectory_data; } void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id, const NodeSpec2D& node_data) { node_data_.Insert(node_id, node_data); + trajectory_data_[node_id.trajectory_id]; } void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) { imu_data_.Trim(node_data_, node_id); odometry_data_.Trim(node_data_, node_id); node_data_.Trim(node_id); + if (node_data_.SizeOfTrajectoryOrZero(node_id.trajectory_id) == 0) { + trajectory_data_.erase(node_id.trajectory_id); + } } void OptimizationProblem2D::AddSubmap( @@ -301,6 +346,53 @@ void OptimizationProblem2D::Solve( } } + std::map> C_fixed_frames; + for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { + const int trajectory_id = node_it->id.trajectory_id; + const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); + if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) { + node_it = trajectory_end; + continue; + } + + const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id); + bool fixed_frame_pose_initialized = false; + for (; node_it != trajectory_end; ++node_it) { + const NodeId node_id = node_it->id; + const NodeSpec2D& node_data = node_it->data; + + const std::unique_ptr fixed_frame_pose = + Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time); + if (fixed_frame_pose == nullptr) { + continue; + } + + const Constraint::Pose constraint_pose{ + *fixed_frame_pose, options_.fixed_frame_pose_translation_weight(), + options_.fixed_frame_pose_rotation_weight()}; + + if (!fixed_frame_pose_initialized) { + transform::Rigid2d fixed_frame_pose_in_map; + if (trajectory_data.fixed_frame_origin_in_map.has_value()) { + fixed_frame_pose_in_map = transform::Project2D( + trajectory_data.fixed_frame_origin_in_map.value()); + } else { + fixed_frame_pose_in_map = node_data.global_pose_2d; + } + + C_fixed_frames.emplace( + std::piecewise_construct, std::forward_as_tuple(trajectory_id), + std::forward_as_tuple(FromPose(fixed_frame_pose_in_map))); + fixed_frame_pose_initialized = true; + } + + problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(constraint_pose), + nullptr /* loss function */, + C_fixed_frames.at(trajectory_id).data(), + C_nodes.at(node_id).data()); + } + } + // Solve. ceres::Solver::Summary summary; ceres::Solve( @@ -319,6 +411,10 @@ void OptimizationProblem2D::Solve( node_data_.at(C_node_id_data.id).global_pose_2d = ToPose(C_node_id_data.data); } + for (const auto& C_fixed_frame : C_fixed_frames) { + trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map = + transform::Embed3D(ToPose(C_fixed_frame.second)); + } for (const auto& C_landmark : C_landmarks) { landmark_data_[C_landmark.first] = C_landmark.second.ToRigid(); } diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.h b/cartographer/mapping/internal/optimization/optimization_problem_2d.h index 496f2d9..f831231 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.h +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.h @@ -101,6 +101,21 @@ class OptimizationProblem2D return odometry_data_; } + void AddFixedFramePoseData( + int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data); + void SetTrajectoryData( + int trajectory_id, + const PoseGraphInterface::TrajectoryData& trajectory_data); + const sensor::MapByTime& fixed_frame_pose_data() + const { + return fixed_frame_pose_data_; + } + const std::map& trajectory_data() + const { + return trajectory_data_; + } + private: std::unique_ptr InterpolateOdometry( int trajectory_id, common::Time time) const; @@ -115,6 +130,8 @@ class OptimizationProblem2D std::map landmark_data_; sensor::MapByTime imu_data_; sensor::MapByTime odometry_data_; + sensor::MapByTime fixed_frame_pose_data_; + std::map trajectory_data_; }; } // namespace optimization