From e6c4ee4b8b6a405c9959da00dc35c198afe13207 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Thu, 15 Mar 2018 10:33:02 +0100 Subject: [PATCH] Move interpolation of poses to 'cost_helpers'. (#992) --- .../2d/pose_graph/landmark_cost_function_2d.h | 46 +++----------- .../3d/pose_graph/landmark_cost_function_3d.h | 26 +++----- .../internal/pose_graph/cost_helpers.h | 21 +++++++ .../internal/pose_graph/cost_helpers_impl.h | 60 +++++++++++++++++++ 4 files changed, 99 insertions(+), 54 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h b/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h index 997e4f2..75f9345 100644 --- a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h @@ -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 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 prev_quaternion( - (Eigen::AngleAxis(prev_node_pose[2], - Eigen::Matrix::UnitZ()) * - prev_node_gravity_alignment_.cast()) - .normalized()); - const std::array 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 next_quaternion( - (Eigen::AngleAxis(next_node_pose[2], - Eigen::Matrix::UnitZ()) * - next_node_gravity_alignment_.cast()) - .normalized()); - const std::array next_node_rotation = { - {next_quaternion.w(), next_quaternion.x(), next_quaternion.y(), - next_quaternion.z()}}; - - const std::array interpolated_pose_rotation = - SlerpQuaternions(prev_node_rotation.data(), next_node_rotation.data(), - T(interpolation_parameter_)); - + const std::tuple, std::array> + interpolated_rotation_and_translation = InterpolateNodes2D( + prev_node_pose, prev_node_gravity_alignment_, next_node_pose, + next_node_gravity_alignment_, interpolation_parameter_); const std::array 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; diff --git a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h b/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h index 88fee28..d2cca35 100644 --- a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h @@ -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 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 interpolated_pose_rotation = SlerpQuaternions( - prev_node_rotation, next_node_rotation, T(interpolation_parameter_)); - + const std::tuple, std::array> + interpolated_rotation_and_translation = InterpolateNodes3D( + prev_node_rotation, prev_node_translation, next_node_rotation, + next_node_translation, interpolation_parameter_); const std::array 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; diff --git a/cartographer/mapping/internal/pose_graph/cost_helpers.h b/cartographer/mapping/internal/pose_graph/cost_helpers.h index a9052a5..e8c34bc 100644 --- a/cartographer/mapping/internal/pose_graph/cost_helpers.h +++ b/cartographer/mapping/internal/pose_graph/cost_helpers.h @@ -59,6 +59,27 @@ std::array ScaleError(std::array error, T translation_weight, template std::array 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 +std::tuple /* rotation */, std::array /* 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 +std::tuple /* rotation */, std::array /* 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 diff --git a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h b/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h index bb0ab71..d7073dd 100644 --- a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h +++ b/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h @@ -120,6 +120,66 @@ std::array SlerpQuaternions(const T* const start, const T* const end, prev_scale * start[3] + next_scale * end[3]}}; } +template +std::tuple /* rotation */, std::array /* 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{ + {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 +std::tuple /* rotation */, std::array /* 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 prev_quaternion( + (Eigen::AngleAxis(prev_node_pose[2], Eigen::Matrix::UnitZ()) * + prev_node_gravity_alignment.cast()) + .normalized()); + const std::array 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 next_quaternion( + (Eigen::AngleAxis(next_node_pose[2], Eigen::Matrix::UnitZ()) * + next_node_gravity_alignment.cast()) + .normalized()); + const std::array 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{ + {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