FixedFramePose in optimization 2d (#1580)

master
Jihoon Lee 2020-07-07 20:18:53 +09:00 committed by GitHub
parent d7016fdbbc
commit 5899bff425
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 143 additions and 7 deletions

View File

@ -229,7 +229,14 @@ void PoseGraph2D::AddOdometryData(const int trajectory_id,
void PoseGraph2D::AddFixedFramePoseData( void PoseGraph2D::AddFixedFramePoseData(
const int trajectory_id, const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) { 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, void PoseGraph2D::AddLandmarkData(int trajectory_id,
@ -753,7 +760,24 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
void PoseGraph2D::SetTrajectoryDataFromProto( void PoseGraph2D::SetTrajectoryDataFromProto(
const proto::TrajectoryData& data) { 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, void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id,
@ -984,15 +1008,14 @@ PoseGraph2D::GetLandmarkNodes() const {
std::map<int, PoseGraphInterface::TrajectoryData> std::map<int, PoseGraphInterface::TrajectoryData>
PoseGraph2D::GetTrajectoryData() const { PoseGraph2D::GetTrajectoryData() const {
// The 2D optimization problem does not have any 'TrajectoryData'. absl::MutexLock locker(&mutex_);
return {}; return optimization_problem_->trajectory_data();
} }
sensor::MapByTime<sensor::FixedFramePoseData> sensor::MapByTime<sensor::FixedFramePoseData>
PoseGraph2D::GetFixedFramePoseData() const { PoseGraph2D::GetFixedFramePoseData() const {
// FixedFramePoseData is not yet implemented for 2D. We need to return empty absl::MutexLock locker(&mutex_);
// so serialization works. return optimization_problem_->fixed_frame_pose_data();
return {};
} }
std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() const { std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() const {

View File

@ -42,6 +42,35 @@ namespace {
using ::cartographer::mapping::optimization::CeresPose; using ::cartographer::mapping::optimization::CeresPose;
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
using TrajectoryData =
::cartographer::mapping::PoseGraphInterface::TrajectoryData;
// For fixed frame pose.
std::unique_ptr<transform::Rigid3d> Interpolate(
const sensor::MapByTime<sensor::FixedFramePoseData>& 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<transform::Rigid3d>(it->pose.value());
}
return nullptr;
}
const auto prev_it = std::prev(it);
if (prev_it->pose.has_value()) {
return absl::make_unique<transform::Rigid3d>(
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: // Converts a pose into the 3 optimization variable format used for Ceres:
// translation in x and y, followed by the rotation angle representing the // 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); 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, void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
const NodeSpec2D& node_data) { const NodeSpec2D& node_data) {
node_data_.Append(trajectory_id, 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, void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id,
const NodeSpec2D& node_data) { const NodeSpec2D& node_data) {
node_data_.Insert(node_id, node_data); node_data_.Insert(node_id, node_data);
trajectory_data_[node_id.trajectory_id];
} }
void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) { void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) {
imu_data_.Trim(node_data_, node_id); imu_data_.Trim(node_data_, node_id);
odometry_data_.Trim(node_data_, node_id); odometry_data_.Trim(node_data_, node_id);
node_data_.Trim(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( void OptimizationProblem2D::AddSubmap(
@ -301,6 +346,53 @@ void OptimizationProblem2D::Solve(
} }
} }
std::map<int, std::array<double, 3>> 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<transform::Rigid3d> 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. // Solve.
ceres::Solver::Summary summary; ceres::Solver::Summary summary;
ceres::Solve( ceres::Solve(
@ -319,6 +411,10 @@ void OptimizationProblem2D::Solve(
node_data_.at(C_node_id_data.id).global_pose_2d = node_data_.at(C_node_id_data.id).global_pose_2d =
ToPose(C_node_id_data.data); 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) { for (const auto& C_landmark : C_landmarks) {
landmark_data_[C_landmark.first] = C_landmark.second.ToRigid(); landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
} }

View File

@ -101,6 +101,21 @@ class OptimizationProblem2D
return odometry_data_; 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<sensor::FixedFramePoseData>& fixed_frame_pose_data()
const {
return fixed_frame_pose_data_;
}
const std::map<int, PoseGraphInterface::TrajectoryData>& trajectory_data()
const {
return trajectory_data_;
}
private: private:
std::unique_ptr<transform::Rigid3d> InterpolateOdometry( std::unique_ptr<transform::Rigid3d> InterpolateOdometry(
int trajectory_id, common::Time time) const; int trajectory_id, common::Time time) const;
@ -115,6 +130,8 @@ class OptimizationProblem2D
std::map<std::string, transform::Rigid3d> landmark_data_; std::map<std::string, transform::Rigid3d> landmark_data_;
sensor::MapByTime<sensor::ImuData> imu_data_; sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_; sensor::MapByTime<sensor::OdometryData> odometry_data_;
sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;
}; };
} // namespace optimization } // namespace optimization