From 06a9d3544d7a8628ddd00b0c36a4bf07d8c92529 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Fri, 16 Mar 2018 12:01:55 +0100 Subject: [PATCH] Interpolate trajectory node to compute starting point for landmarks. (#997) [Landmarks RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md) --- .../2d/pose_graph/optimization_problem_2d.cc | 31 +++++++++++++------ .../3d/pose_graph/optimization_problem_3d.cc | 31 +++++++++++++------ .../mapping/internal/pose_graph/ceres_pose.cc | 5 +-- .../mapping/internal/pose_graph/ceres_pose.h | 3 ++ cartographer/transform/rigid_transform.h | 7 +++++ 5 files changed, 53 insertions(+), 24 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc index 575383a..89544fa 100644 --- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc @@ -30,6 +30,7 @@ #include "cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h" #include "cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h" #include "cartographer/mapping/internal/pose_graph/ceres_pose.h" +#include "cartographer/mapping/internal/pose_graph/cost_helpers.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" @@ -62,13 +63,20 @@ transform::Rigid2d ToPose(const std::array& values) { // applies a relative transform from it. transform::Rigid3d GetInitialLandmarkPose( const LandmarkNode::LandmarkObservation& observation, - const NodeData& prev_node, const NodeData& next_node) { - const NodeData& closest_node = - observation.time - prev_node.time < next_node.time - observation.time - ? prev_node - : next_node; - return transform::Embed3D(closest_node.pose) * - transform::Rigid3d::Rotation(closest_node.gravity_alignment) * + const NodeData& prev_node, const NodeData& next_node, + const std::array& prev_node_pose, + const std::array& next_node_pose) { + const double interpolation_parameter = + common::ToSeconds(observation.time - prev_node.time) / + common::ToSeconds(next_node.time - prev_node.time); + + const std::tuple, std::array> + rotation_and_translation = + InterpolateNodes2D(prev_node_pose.data(), prev_node.gravity_alignment, + next_node_pose.data(), next_node.gravity_alignment, + interpolation_parameter); + return transform::Rigid3d::FromArrays(std::get<0>(rotation_and_translation), + std::get<1>(rotation_and_translation)) * observation.landmark_to_tracking_transform; } @@ -104,11 +112,14 @@ void AddLandmarkCostFunctions( } auto prev = std::prev(next); // Add parameter blocks for the landmark ID if they were not added before. + std::array* prev_node_pose = &C_nodes->at(prev->id); + std::array* next_node_pose = &C_nodes->at(next->id); if (!C_landmarks->count(landmark_id)) { const transform::Rigid3d starting_point = landmark_node.second.global_landmark_pose.has_value() ? landmark_node.second.global_landmark_pose.value() - : GetInitialLandmarkPose(observation, prev->data, next->data); + : GetInitialLandmarkPose(observation, prev->data, next->data, + *prev_node_pose, *next_node_pose); C_landmarks->emplace( landmark_id, CeresPose(starting_point, nullptr /* translation_parametrization */, @@ -124,8 +135,8 @@ void AddLandmarkCostFunctions( problem->AddResidualBlock( LandmarkCostFunction2D::CreateAutoDiffCostFunction( observation, prev->data, next->data), - nullptr /* loss function */, C_nodes->at(prev->id).data(), - C_nodes->at(next->id).data(), C_landmarks->at(landmark_id).rotation(), + nullptr /* loss function */, prev_node_pose->data(), + next_node_pose->data(), C_landmarks->at(landmark_id).rotation(), C_landmarks->at(landmark_id).translation()); } } diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc index a071fc7..dd1e43e 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc @@ -107,12 +107,20 @@ std::unique_ptr Interpolate( // applies a relative transform from it. transform::Rigid3d GetInitialLandmarkPose( const LandmarkNode::LandmarkObservation& observation, - const NodeData& prev_node, const NodeData& next_node) { - const NodeData& closest_node = - observation.time - prev_node.time < next_node.time - observation.time - ? prev_node - : next_node; - return closest_node.global_pose * observation.landmark_to_tracking_transform; + const NodeData& prev_node, const NodeData& next_node, + const CeresPose& prev_node_pose, const CeresPose& next_node_pose) { + const double interpolation_parameter = + common::ToSeconds(observation.time - prev_node.time) / + common::ToSeconds(next_node.time - prev_node.time); + + const std::tuple, std::array> + rotation_and_translation = InterpolateNodes3D( + prev_node_pose.rotation(), prev_node_pose.translation(), + next_node_pose.rotation(), next_node_pose.translation(), + interpolation_parameter); + return transform::Rigid3d::FromArrays(std::get<0>(rotation_and_translation), + std::get<1>(rotation_and_translation)) * + observation.landmark_to_tracking_transform; } void AddLandmarkCostFunctions( @@ -147,11 +155,14 @@ void AddLandmarkCostFunctions( } auto prev = std::prev(next); // Add parameter blocks for the landmark ID if they were not added before. + CeresPose* prev_node_pose = &C_nodes->at(prev->id); + CeresPose* next_node_pose = &C_nodes->at(next->id); if (!C_landmarks->count(landmark_id)) { const transform::Rigid3d starting_point = landmark_node.second.global_landmark_pose.has_value() ? landmark_node.second.global_landmark_pose.value() - : GetInitialLandmarkPose(observation, prev->data, next->data); + : GetInitialLandmarkPose(observation, prev->data, next->data, + *prev_node_pose, *next_node_pose); C_landmarks->emplace( landmark_id, CeresPose(starting_point, nullptr /* translation_parametrization */, @@ -167,9 +178,9 @@ void AddLandmarkCostFunctions( problem->AddResidualBlock( LandmarkCostFunction3D::CreateAutoDiffCostFunction( observation, prev->data, next->data), - nullptr /* loss function */, C_nodes->at(prev->id).rotation(), - C_nodes->at(prev->id).translation(), C_nodes->at(next->id).rotation(), - C_nodes->at(next->id).translation(), + nullptr /* loss function */, prev_node_pose->rotation(), + prev_node_pose->translation(), next_node_pose->rotation(), + next_node_pose->translation(), C_landmarks->at(landmark_id).rotation(), C_landmarks->at(landmark_id).translation()); } diff --git a/cartographer/mapping/internal/pose_graph/ceres_pose.cc b/cartographer/mapping/internal/pose_graph/ceres_pose.cc index fb993d6..7063a47 100644 --- a/cartographer/mapping/internal/pose_graph/ceres_pose.cc +++ b/cartographer/mapping/internal/pose_graph/ceres_pose.cc @@ -37,10 +37,7 @@ CeresPose::CeresPose( } const transform::Rigid3d CeresPose::ToRigid() const { - const auto& rotation = data_->rotation; - return transform::Rigid3d( - Eigen::Map(data_->translation.data()), - Eigen::Quaterniond(rotation[0], rotation[1], rotation[2], rotation[3])); + return transform::Rigid3d::FromArrays(data_->rotation, data_->translation); } } // namespace pose_graph diff --git a/cartographer/mapping/internal/pose_graph/ceres_pose.h b/cartographer/mapping/internal/pose_graph/ceres_pose.h index 31ed407..fdd6d2e 100644 --- a/cartographer/mapping/internal/pose_graph/ceres_pose.h +++ b/cartographer/mapping/internal/pose_graph/ceres_pose.h @@ -39,7 +39,10 @@ class CeresPose { const transform::Rigid3d ToRigid() const; double* translation() { return data_->translation.data(); } + const double* translation() const { return data_->translation.data(); } + double* rotation() { return data_->rotation.data(); } + const double* rotation() const { return data_->rotation.data(); } private: struct Data { diff --git a/cartographer/transform/rigid_transform.h b/cartographer/transform/rigid_transform.h index 29f0bbc..50df99f 100644 --- a/cartographer/transform/rigid_transform.h +++ b/cartographer/transform/rigid_transform.h @@ -144,6 +144,13 @@ class Rigid3 { return Rigid3(vector, Quaternion::Identity()); } + static Rigid3 FromArrays(const std::array& rotation, + const std::array& translation) { + return Rigid3(Eigen::Map(translation.data()), + Eigen::Quaternion(rotation[0], rotation[1], + rotation[2], rotation[3])); + } + static Rigid3 Identity() { return Rigid3(); } template