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

Move slerp and scaling of error to 'cost_helpers'. (#864) 

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)
master
Alexander Belyaev 2018-01-31 17:06:49 +01:00 committed by GitHub
parent ad4dc3c4d3
commit 0440761474
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 105 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cartographer/mapping/pose_graph/cost_helpers.h
View File

@ -31,9 +31,11 @@ namespace pose_graph {
//
// 'start' and 'end' poses have the format [x, y, rotation].
template <typename T>
static std::array<T, 3> ComputeUnscaledError2d(
const transform::Rigid2d& relative_pose, const T* const start,
const T* const end);
std::array<T, 3> ComputeUnscaledError(const transform::Rigid2d& relative_pose,
const T* const start, const T* const end);
template <typename T>
std::array<T, 3> ScaleError(std::array<T, 3> error, T translation_weight,
T rotation_weight);
// Computes the error between the given relative pose and the difference of
// poses 'start' and 'end' which are both in an arbitrary common frame.
@ -41,11 +43,22 @@ static std::array<T, 3> ComputeUnscaledError2d(
// 'start' and 'end' translation has the format [x, y, z].
// 'start' and 'end' rotation are quaternions in the format [w, n_1, n_2, n_3].
template <typename T>
static std::array<T, 6> ComputeUnscaledError3d(
const transform::Rigid3d& relative_pose, const T* const start_rotation,
const T* const start_translation, const T* const end_rotation,
const T* const end_translation);
std::array<T, 6> ComputeUnscaledError(const transform::Rigid3d& relative_pose,
const T* const start_rotation,
const T* const start_translation,
const T* const end_rotation,
const T* const end_translation);
template <typename T>
std::array<T, 6> ScaleError(std::array<T, 6> error, T translation_weight,
T rotation_weight);
// Computes spherical linear interpolation of unit quaternions.
//
// 'start' and 'end' are quaternions in the format [w, n_1, n_2, n_3].
template <typename T>
std::array<T, 4> SlerpQuaternions(const T* const start, const T* const end,
T factor);
} // namespace pose_graph
} // namespace mapping
} // namespace cartographer

54
cartographer/mapping/pose_graph/cost_helpers_impl.h
View File

@ -22,7 +22,7 @@ namespace mapping {
namespace pose_graph {
template <typename T>
static std::array<T, 3> ComputeUnscaledError2d(
static std::array<T, 3> ComputeUnscaledError(
const transform::Rigid2d& relative_pose, const T* const start,
const T* const end) {
const T cos_theta_i = cos(start[2]);
@ -39,7 +39,17 @@ static std::array<T, 3> ComputeUnscaledError2d(
}
template <typename T>
static std::array<T, 6> ComputeUnscaledError3d(
std::array<T, 3> ScaleError(std::array<T, 3> error, T translation_weight,
T rotation_weight) {
std::array<T, 3> scaled_error(std::move(error));
scaled_error[0] *= translation_weight;
scaled_error[1] *= translation_weight;
scaled_error[2] *= rotation_weight;
return scaled_error;
}
template <typename T>
static std::array<T, 6> ComputeUnscaledError(
const transform::Rigid3d& relative_pose, const T* const start_rotation,
const T* const start_translation, const T* const end_rotation,
const T* const end_translation) {
@ -69,6 +79,46 @@ static std::array<T, 6> ComputeUnscaledError3d(
angle_axis_difference[2]}};
}
template <typename T>
std::array<T, 6> ScaleError(std::array<T, 6> error, T translation_weight,
T rotation_weight) {
std::array<T, 6> scaled_error(std::move(error));
scaled_error[0] *= translation_weight;
scaled_error[1] *= translation_weight;
scaled_error[2] *= translation_weight;
scaled_error[3] *= rotation_weight;
scaled_error[4] *= rotation_weight;
scaled_error[5] *= rotation_weight;
return scaled_error;
}
// Eigen implementation of slerp is not compatible with Ceres on all supported
// platforms. Our own implementation is used instead.
template <typename T>
std::array<T, 4> SlerpQuaternions(const T* const start, const T* const end,
T factor) {
// Angle 'theta' is the half-angle "between" quaternions. It can be computed
// as the arccosine of their dot product.
const T cos_theta = start[0] * end[0] + start[1] * end[1] +
start[2] * end[2] + start[3] * end[3];
const T abs_cos_theta = abs(cos_theta);
// If numerical error brings 'cos_theta' outside [-1 + epsilon, 1 - epsilon]
// interval, then the quaternions are likely to be collinear.
T prev_scale = T(1.) - factor;
T next_scale = factor;
if (abs_cos_theta < T(1. - 1e-5)) {
const T theta = acos(abs_cos_theta);
const T sin_theta = sin(theta);
prev_scale = sin(prev_scale * theta) / sin_theta;
next_scale = sin(next_scale * theta) / sin_theta;
}
if (cos_theta < T(0.)) next_scale = -next_scale;
return {{prev_scale * start[0] + next_scale * end[0],
prev_scale * start[1] + next_scale * end[1],
prev_scale * start[2] + next_scale * end[2],
prev_scale * start[3] + next_scale * end[3]}};
}
} // namespace pose_graph
} // namespace mapping
} // namespace cartographer

67
cartographer/mapping/pose_graph/landmark_cost_function.h
View File

@ -30,33 +30,6 @@ namespace cartographer {
namespace mapping {
namespace pose_graph {
template <typename T>
std::array<T, 4> SlerpQuaternions(const T* const prev_rotation,
const T* const next_rotation, T factor) {
// Angle 'theta' is the half-angle "between" quaternions. It can be computed
// as the arccosine of their dot product.
const T cos_theta = prev_rotation[0] * next_rotation[0] +
prev_rotation[1] * next_rotation[1] +
prev_rotation[2] * next_rotation[2] +
prev_rotation[3] * next_rotation[3];
// If numerical error brings 'cos_theta' outside of [-1., 1.] interval, then
// the quaternions are likely to be collinear.
if (cos_theta >= T(1.0) || cos_theta <= T(-1.0)) {
return {{next_rotation[0], next_rotation[1], next_rotation[2],
next_rotation[3]}};
}
const T theta = acos(abs(cos_theta));
const T sin_theta = sin(theta);
const T prev_scale = sin((T(1.0) - factor) * theta) / sin_theta;
const T next_scale =
sin(factor * theta) * (cos_theta < T(0) ? T(-1.0) : T(1.0)) / sin_theta;
return {{prev_scale * prev_rotation[0] + next_scale * next_rotation[0],
prev_scale * prev_rotation[1] + next_scale * next_rotation[1],
prev_scale * prev_rotation[2] + next_scale * next_rotation[2],
prev_scale * prev_rotation[3] + next_scale * next_rotation[3]}};
}
// Cost function measuring the weighted error between the observed pose given by
// the landmark measurement and the linearly interpolated pose.
class LandmarkCostFunction {
@ -85,30 +58,26 @@ class LandmarkCostFunction {
const T* const next_node_translation,
const T* const landmark_rotation,
const T* const landmark_translation, T* const e) const {
const T interpolated_pose_translation[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])};
std::array<T, 4> interpolated_pose_rotation = SlerpQuaternions(
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::array<T, 6> unscaled_error = ComputeUnscaledError3d(
landmark_to_tracking_transform_, interpolated_pose_rotation.data(),
interpolated_pose_translation, landmark_rotation, landmark_translation);
e[0] = T(translation_weight_) * unscaled_error[0];
e[1] = T(translation_weight_) * unscaled_error[1];
e[2] = T(translation_weight_) * unscaled_error[2];
e[3] = T(rotation_weight_) * unscaled_error[3];
e[4] = T(rotation_weight_) * unscaled_error[4];
e[5] = T(rotation_weight_) * unscaled_error[5];
const std::array<T, 6> error = ScaleError(
ComputeUnscaledError(landmark_to_tracking_transform_,
interpolated_pose_rotation.data(),
interpolated_pose_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_2d/pose_graph/spa_cost_function.h
View File

@ -47,21 +47,14 @@ class SpaCostFunction {
template <typename T>
bool operator()(const T* const c_i, const T* const c_j, T* e) const {
ComputeScaledError(pose_, c_i, c_j, e);
return true;
}
using mapping::pose_graph::ComputeUnscaledError;
using mapping::pose_graph::ScaleError;
// Computes the error scaled by 'translation_weight' and 'rotation_weight',
// storing it in 'e'.
template <typename T>
static void ComputeScaledError(const Constraint::Pose& pose,
const T* const c_i, const T* const c_j,
T* const e) {
const std::array<T, 3> e_ij = mapping::pose_graph::ComputeUnscaledError2d(
transform::Project2D(pose.zbar_ij), c_i, c_j);
e[0] = e_ij[0] * T(pose.translation_weight);
e[1] = e_ij[1] * T(pose.translation_weight);
e[2] = e_ij[2] * T(pose.rotation_weight);
const std::array<T, 3> error = ScaleError(
ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j),
T(pose_.translation_weight), T(pose_.rotation_weight));
std::copy(std::begin(error), std::end(error), e);
return true;
}
private:

29
cartographer/mapping_3d/pose_graph/spa_cost_function.h
View File

@ -49,28 +49,15 @@ class SpaCostFunction {
bool operator()(const T* const c_i_rotation, const T* const c_i_translation,
const T* const c_j_rotation, const T* const c_j_translation,
T* const e) const {
ComputeScaledError(pose_, c_i_rotation, c_i_translation, c_j_rotation,
c_j_translation, e);
return true;
}
using mapping::pose_graph::ComputeUnscaledError;
using mapping::pose_graph::ScaleError;
// Computes the error scaled by 'translation_weight' and 'rotation_weight',
// storing it in 'e'.
template <typename T>
static void ComputeScaledError(const Constraint::Pose& pose,
const T* const c_i_rotation,
const T* const c_i_translation,
const T* const c_j_rotation,
const T* const c_j_translation, T* const e) {
const std::array<T, 6> e_ij = mapping::pose_graph::ComputeUnscaledError3d(
pose.zbar_ij, c_i_rotation, c_i_translation, c_j_rotation,
c_j_translation);
for (int ij : {0, 1, 2}) {
e[ij] = e_ij[ij] * T(pose.translation_weight);
}
for (int ij : {3, 4, 5}) {
e[ij] = e_ij[ij] * T(pose.rotation_weight);
}
const std::array<T, 6> error = ScaleError(
ComputeUnscaledError(pose_.zbar_ij, c_i_rotation, c_i_translation,
c_j_rotation, c_j_translation),
T(pose_.translation_weight), T(pose_.rotation_weight));
std::copy(std::begin(error), std::end(error), e);
return true;
}
private: