Interpolate trajectory node to compute starting point for landmarks. (#997)

[Landmarks RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)
master
Alexander Belyaev 2018-03-16 12:01:55 +01:00 committed by GitHub
parent 37a68cd7f4
commit 06a9d3544d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 53 additions and 24 deletions

View File

@ -30,6 +30,7 @@
#include "cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h" #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/2d/pose_graph/spa_cost_function_2d.h"
#include "cartographer/mapping/internal/pose_graph/ceres_pose.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/sensor/odometry_data.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "ceres/ceres.h" #include "ceres/ceres.h"
@ -62,13 +63,20 @@ transform::Rigid2d ToPose(const std::array<double, 3>& values) {
// applies a relative transform from it. // applies a relative transform from it.
transform::Rigid3d GetInitialLandmarkPose( transform::Rigid3d GetInitialLandmarkPose(
const LandmarkNode::LandmarkObservation& observation, const LandmarkNode::LandmarkObservation& observation,
const NodeData& prev_node, const NodeData& next_node) { const NodeData& prev_node, const NodeData& next_node,
const NodeData& closest_node = const std::array<double, 3>& prev_node_pose,
observation.time - prev_node.time < next_node.time - observation.time const std::array<double, 3>& next_node_pose) {
? prev_node const double interpolation_parameter =
: next_node; common::ToSeconds(observation.time - prev_node.time) /
return transform::Embed3D(closest_node.pose) * common::ToSeconds(next_node.time - prev_node.time);
transform::Rigid3d::Rotation(closest_node.gravity_alignment) *
const std::tuple<std::array<double, 4>, std::array<double, 3>>
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; observation.landmark_to_tracking_transform;
} }
@ -104,11 +112,14 @@ void AddLandmarkCostFunctions(
} }
auto prev = std::prev(next); auto prev = std::prev(next);
// Add parameter blocks for the landmark ID if they were not added before. // Add parameter blocks for the landmark ID if they were not added before.
std::array<double, 3>* prev_node_pose = &C_nodes->at(prev->id);
std::array<double, 3>* next_node_pose = &C_nodes->at(next->id);
if (!C_landmarks->count(landmark_id)) { if (!C_landmarks->count(landmark_id)) {
const transform::Rigid3d starting_point = const transform::Rigid3d starting_point =
landmark_node.second.global_landmark_pose.has_value() landmark_node.second.global_landmark_pose.has_value()
? landmark_node.second.global_landmark_pose.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( C_landmarks->emplace(
landmark_id, landmark_id,
CeresPose(starting_point, nullptr /* translation_parametrization */, CeresPose(starting_point, nullptr /* translation_parametrization */,
@ -124,8 +135,8 @@ void AddLandmarkCostFunctions(
problem->AddResidualBlock( problem->AddResidualBlock(
LandmarkCostFunction2D::CreateAutoDiffCostFunction( LandmarkCostFunction2D::CreateAutoDiffCostFunction(
observation, prev->data, next->data), observation, prev->data, next->data),
nullptr /* loss function */, C_nodes->at(prev->id).data(), nullptr /* loss function */, prev_node_pose->data(),
C_nodes->at(next->id).data(), C_landmarks->at(landmark_id).rotation(), next_node_pose->data(), C_landmarks->at(landmark_id).rotation(),
C_landmarks->at(landmark_id).translation()); C_landmarks->at(landmark_id).translation());
} }
} }

View File

@ -107,12 +107,20 @@ std::unique_ptr<transform::Rigid3d> Interpolate(
// applies a relative transform from it. // applies a relative transform from it.
transform::Rigid3d GetInitialLandmarkPose( transform::Rigid3d GetInitialLandmarkPose(
const LandmarkNode::LandmarkObservation& observation, const LandmarkNode::LandmarkObservation& observation,
const NodeData& prev_node, const NodeData& next_node) { const NodeData& prev_node, const NodeData& next_node,
const NodeData& closest_node = const CeresPose& prev_node_pose, const CeresPose& next_node_pose) {
observation.time - prev_node.time < next_node.time - observation.time const double interpolation_parameter =
? prev_node common::ToSeconds(observation.time - prev_node.time) /
: next_node; common::ToSeconds(next_node.time - prev_node.time);
return closest_node.global_pose * observation.landmark_to_tracking_transform;
const std::tuple<std::array<double, 4>, std::array<double, 3>>
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( void AddLandmarkCostFunctions(
@ -147,11 +155,14 @@ void AddLandmarkCostFunctions(
} }
auto prev = std::prev(next); auto prev = std::prev(next);
// Add parameter blocks for the landmark ID if they were not added before. // 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)) { if (!C_landmarks->count(landmark_id)) {
const transform::Rigid3d starting_point = const transform::Rigid3d starting_point =
landmark_node.second.global_landmark_pose.has_value() landmark_node.second.global_landmark_pose.has_value()
? landmark_node.second.global_landmark_pose.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( C_landmarks->emplace(
landmark_id, landmark_id,
CeresPose(starting_point, nullptr /* translation_parametrization */, CeresPose(starting_point, nullptr /* translation_parametrization */,
@ -167,9 +178,9 @@ void AddLandmarkCostFunctions(
problem->AddResidualBlock( problem->AddResidualBlock(
LandmarkCostFunction3D::CreateAutoDiffCostFunction( LandmarkCostFunction3D::CreateAutoDiffCostFunction(
observation, prev->data, next->data), observation, prev->data, next->data),
nullptr /* loss function */, C_nodes->at(prev->id).rotation(), nullptr /* loss function */, prev_node_pose->rotation(),
C_nodes->at(prev->id).translation(), C_nodes->at(next->id).rotation(), prev_node_pose->translation(), next_node_pose->rotation(),
C_nodes->at(next->id).translation(), next_node_pose->translation(),
C_landmarks->at(landmark_id).rotation(), C_landmarks->at(landmark_id).rotation(),
C_landmarks->at(landmark_id).translation()); C_landmarks->at(landmark_id).translation());
} }

View File

@ -37,10 +37,7 @@ CeresPose::CeresPose(
} }
const transform::Rigid3d CeresPose::ToRigid() const { const transform::Rigid3d CeresPose::ToRigid() const {
const auto& rotation = data_->rotation; return transform::Rigid3d::FromArrays(data_->rotation, data_->translation);
return transform::Rigid3d(
Eigen::Map<const Eigen::Vector3d>(data_->translation.data()),
Eigen::Quaterniond(rotation[0], rotation[1], rotation[2], rotation[3]));
} }
} // namespace pose_graph } // namespace pose_graph

View File

@ -39,7 +39,10 @@ class CeresPose {
const transform::Rigid3d ToRigid() const; const transform::Rigid3d ToRigid() const;
double* translation() { return data_->translation.data(); } double* translation() { return data_->translation.data(); }
const double* translation() const { return data_->translation.data(); }
double* rotation() { return data_->rotation.data(); } double* rotation() { return data_->rotation.data(); }
const double* rotation() const { return data_->rotation.data(); }
private: private:
struct Data { struct Data {

View File

@ -144,6 +144,13 @@ class Rigid3 {
return Rigid3(vector, Quaternion::Identity()); return Rigid3(vector, Quaternion::Identity());
} }
static Rigid3 FromArrays(const std::array<FloatType, 4>& rotation,
const std::array<FloatType, 3>& translation) {
return Rigid3(Eigen::Map<const Vector>(translation.data()),
Eigen::Quaternion<FloatType>(rotation[0], rotation[1],
rotation[2], rotation[3]));
}
static Rigid3<FloatType> Identity() { return Rigid3<FloatType>(); } static Rigid3<FloatType> Identity() { return Rigid3<FloatType>(); }
template <typename OtherType> template <typename OtherType>