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

Move interpolation of poses to 'cost_helpers'. (#992)

master
Alexander Belyaev 2018-03-15 10:33:02 +01:00 committed by gaschler
parent 1e4d558ac4
commit e6c4ee4b8b
4 changed files with 99 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h
View File

@ -55,44 +55,16 @@ class LandmarkCostFunction2D {
bool operator()(const T* const prev_node_pose, const T* const next_node_pose,
const T* const landmark_rotation,
const T* const landmark_translation, T* const e) const {
const std::array<T, 3> interpolated_pose_translation{
{prev_node_pose[0] +
interpolation_parameter_ * (next_node_pose[0] - prev_node_pose[0]),
prev_node_pose[1] +
interpolation_parameter_ * (next_node_pose[1] - prev_node_pose[1]),
T(0)}};
// The following is equivalent to (Embed3D(prev_pose) *
// Rigid3d::Rotation(prev_pose_gravity_alignment)).rotation().
const Eigen::Quaternion<T> prev_quaternion(
(Eigen::AngleAxis<T>(prev_node_pose[2],
Eigen::Matrix<T, 3, 1>::UnitZ()) *
prev_node_gravity_alignment_.cast<T>())
.normalized());
const std::array<T, 4> prev_node_rotation = {
{prev_quaternion.w(), prev_quaternion.x(), prev_quaternion.y(),
prev_quaternion.z()}};
// The following is equivalent to (Embed3D(next_pose) *
// Rigid3d::Rotation(next_pose_gravity_alignment)).rotation().
const Eigen::Quaternion<T> next_quaternion(
(Eigen::AngleAxis<T>(next_node_pose[2],
Eigen::Matrix<T, 3, 1>::UnitZ()) *
next_node_gravity_alignment_.cast<T>())
.normalized());
const std::array<T, 4> next_node_rotation = {
{next_quaternion.w(), next_quaternion.x(), next_quaternion.y(),
next_quaternion.z()}};
const std::array<T, 4> interpolated_pose_rotation =
SlerpQuaternions(prev_node_rotation.data(), next_node_rotation.data(),
T(interpolation_parameter_));
const std::tuple<std::array<T, 4>, std::array<T, 3>>
interpolated_rotation_and_translation = InterpolateNodes2D(
prev_node_pose, prev_node_gravity_alignment_, next_node_pose,
next_node_gravity_alignment_, interpolation_parameter_);
const std::array<T, 6> error = ScaleError(
ComputeUnscaledError(landmark_to_tracking_transform_,
interpolated_pose_rotation.data(),
interpolated_pose_translation.data(),
landmark_rotation, landmark_translation),
ComputeUnscaledError(
landmark_to_tracking_transform_,
std::get<0>(interpolated_rotation_and_translation).data(),
std::get<1>(interpolated_rotation_and_translation).data(),
landmark_rotation, landmark_translation),
T(translation_weight_), T(rotation_weight_));
std::copy(std::begin(error), std::end(error), e);
return true;

26
cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h
View File

@ -60,24 +60,16 @@ class LandmarkCostFunction3D {
const T* const next_node_translation,
const T* const landmark_rotation,
const T* const landmark_translation, T* const e) const {
const std::array<T, 3> interpolated_pose_translation{
{prev_node_translation[0] +
interpolation_parameter_ *
(next_node_translation[0] - prev_node_translation[0]),
prev_node_translation[1] +
interpolation_parameter_ *
(next_node_translation[1] - prev_node_translation[1]),
prev_node_translation[2] +
interpolation_parameter_ *
(next_node_translation[2] - prev_node_translation[2])}};
const std::array<T, 4> interpolated_pose_rotation = SlerpQuaternions(
prev_node_rotation, next_node_rotation, T(interpolation_parameter_));
const std::tuple<std::array<T, 4>, std::array<T, 3>>
interpolated_rotation_and_translation = InterpolateNodes3D(
prev_node_rotation, prev_node_translation, next_node_rotation,
next_node_translation, interpolation_parameter_);
const std::array<T, 6> error = ScaleError(
ComputeUnscaledError(landmark_to_tracking_transform_,
interpolated_pose_rotation.data(),
interpolated_pose_translation.data(),
landmark_rotation, landmark_translation),
ComputeUnscaledError(
landmark_to_tracking_transform_,
std::get<0>(interpolated_rotation_and_translation).data(),
std::get<1>(interpolated_rotation_and_translation).data(),
landmark_rotation, landmark_translation),
T(translation_weight_), T(rotation_weight_));
std::copy(std::begin(error), std::end(error), e);
return true;

21
cartographer/mapping/internal/pose_graph/cost_helpers.h
View File

@ -59,6 +59,27 @@ std::array<T, 6> ScaleError(std::array<T, 6> error, T translation_weight,
template <typename T>
std::array<T, 4> SlerpQuaternions(const T* const start, const T* const end,
T factor);
// Interpolates 3D poses. Linear interpolation is performed for translation and
// spherical-linear one for rotation.
template <typename T>
std::tuple<std::array<T, 4> /* rotation */, std::array<T, 3> /* translation */>
InterpolateNodes3D(const T* const prev_node_rotation,
const T* const prev_node_translation,
const T* const next_node_rotation,
const T* const next_node_translation,
const double interpolation_parameter);
// Embeds 2D poses into 3D and interpolates them. Linear interpolation is
// performed for translation and spherical-linear one for rotation.
template <typename T>
std::tuple<std::array<T, 4> /* rotation */, std::array<T, 3> /* translation */>
InterpolateNodes2D(const T* const prev_node_pose,
const Eigen::Quaterniond& prev_node_gravity_alignment,
const T* const next_node_pose,
const Eigen::Quaterniond& next_node_gravity_alignment,
const double interpolation_parameter);
} // namespace pose_graph
} // namespace mapping
} // namespace cartographer

60
cartographer/mapping/internal/pose_graph/cost_helpers_impl.h
View File

@ -120,6 +120,66 @@ std::array<T, 4> SlerpQuaternions(const T* const start, const T* const end,
prev_scale * start[3] + next_scale * end[3]}};
}
template <typename T>
std::tuple<std::array<T, 4> /* rotation */, std::array<T, 3> /* translation */>
InterpolateNodes3D(const T* const prev_node_rotation,
const T* const prev_node_translation,
const T* const next_node_rotation,
const T* const next_node_translation,
const double interpolation_parameter) {
return std::make_tuple(
SlerpQuaternions(prev_node_rotation, next_node_rotation,
T(interpolation_parameter)),
std::array<T, 3>{
{prev_node_translation[0] +
interpolation_parameter *
(next_node_translation[0] - prev_node_translation[0]),
prev_node_translation[1] +
interpolation_parameter *
(next_node_translation[1] - prev_node_translation[1]),
prev_node_translation[2] +
interpolation_parameter *
(next_node_translation[2] - prev_node_translation[2])}});
}
template <typename T>
std::tuple<std::array<T, 4> /* rotation */, std::array<T, 3> /* translation */>
InterpolateNodes2D(const T* const prev_node_pose,
const Eigen::Quaterniond& prev_node_gravity_alignment,
const T* const next_node_pose,
const Eigen::Quaterniond& next_node_gravity_alignment,
const double interpolation_parameter) {
// The following is equivalent to (Embed3D(prev_node_pose) *
// Rigid3d::Rotation(prev_node_gravity_alignment)).rotation().
const Eigen::Quaternion<T> prev_quaternion(
(Eigen::AngleAxis<T>(prev_node_pose[2], Eigen::Matrix<T, 3, 1>::UnitZ()) *
prev_node_gravity_alignment.cast<T>())
.normalized());
const std::array<T, 4> prev_node_rotation = {
{prev_quaternion.w(), prev_quaternion.x(), prev_quaternion.y(),
prev_quaternion.z()}};
// The following is equivalent to (Embed3D(next_node_pose) *
// Rigid3d::Rotation(next_node_gravity_alignment)).rotation().
const Eigen::Quaternion<T> next_quaternion(
(Eigen::AngleAxis<T>(next_node_pose[2], Eigen::Matrix<T, 3, 1>::UnitZ()) *
next_node_gravity_alignment.cast<T>())
.normalized());
const std::array<T, 4> next_node_rotation = {
{next_quaternion.w(), next_quaternion.x(), next_quaternion.y(),
next_quaternion.z()}};
return std::make_tuple(
SlerpQuaternions(prev_node_rotation.data(), next_node_rotation.data(),
T(interpolation_parameter)),
std::array<T, 3>{
{prev_node_pose[0] + interpolation_parameter *
(next_node_pose[0] - prev_node_pose[0]),
prev_node_pose[1] + interpolation_parameter *
(next_node_pose[1] - prev_node_pose[1]),
T(0)}});
}
} // namespace pose_graph
} // namespace mapping
} // namespace cartographer