12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc
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/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<double, 3>& 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<double, 3>& prev_node_pose,
const std::array<double, 3>& 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<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;
}
@ -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<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)) {
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());
}
}

31
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc
View File

@ -107,12 +107,20 @@ std::unique_ptr<transform::Rigid3d> 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<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(
@ -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());
}

5
cartographer/mapping/internal/pose_graph/ceres_pose.cc
View File

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

3
cartographer/mapping/internal/pose_graph/ceres_pose.h
View File

@ -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 {

7
cartographer/transform/rigid_transform.h
View File

@ -144,6 +144,13 @@ class Rigid3 {
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>(); }
template <typename OtherType>