Move slerp and scaling of error to 'cost_helpers'. (#864)
[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)master
parent
ad4dc3c4d3
commit
0440761474
|
@ -31,9 +31,11 @@ namespace pose_graph {
|
||||||
//
|
//
|
||||||
// 'start' and 'end' poses have the format [x, y, rotation].
|
// 'start' and 'end' poses have the format [x, y, rotation].
|
||||||
template <typename T>
|
template <typename T>
|
||||||
static std::array<T, 3> ComputeUnscaledError2d(
|
std::array<T, 3> ComputeUnscaledError(const transform::Rigid2d& relative_pose,
|
||||||
const transform::Rigid2d& relative_pose, const T* const start,
|
const T* const start, const T* const end);
|
||||||
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
|
// Computes the error between the given relative pose and the difference of
|
||||||
// poses 'start' and 'end' which are both in an arbitrary common frame.
|
// 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' translation has the format [x, y, z].
|
||||||
// 'start' and 'end' rotation are quaternions in the format [w, n_1, n_2, n_3].
|
// 'start' and 'end' rotation are quaternions in the format [w, n_1, n_2, n_3].
|
||||||
template <typename T>
|
template <typename T>
|
||||||
static std::array<T, 6> ComputeUnscaledError3d(
|
std::array<T, 6> ComputeUnscaledError(const transform::Rigid3d& relative_pose,
|
||||||
const transform::Rigid3d& relative_pose, const T* const start_rotation,
|
const T* const start_rotation,
|
||||||
const T* const start_translation, const T* const end_rotation,
|
const T* const start_translation,
|
||||||
|
const T* const end_rotation,
|
||||||
const T* const end_translation);
|
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 pose_graph
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -22,7 +22,7 @@ namespace mapping {
|
||||||
namespace pose_graph {
|
namespace pose_graph {
|
||||||
|
|
||||||
template <typename T>
|
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 transform::Rigid2d& relative_pose, const T* const start,
|
||||||
const T* const end) {
|
const T* const end) {
|
||||||
const T cos_theta_i = cos(start[2]);
|
const T cos_theta_i = cos(start[2]);
|
||||||
|
@ -39,7 +39,17 @@ static std::array<T, 3> ComputeUnscaledError2d(
|
||||||
}
|
}
|
||||||
|
|
||||||
template <typename T>
|
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 transform::Rigid3d& relative_pose, const T* const start_rotation,
|
||||||
const T* const start_translation, const T* const end_rotation,
|
const T* const start_translation, const T* const end_rotation,
|
||||||
const T* const end_translation) {
|
const T* const end_translation) {
|
||||||
|
@ -69,6 +79,46 @@ static std::array<T, 6> ComputeUnscaledError3d(
|
||||||
angle_axis_difference[2]}};
|
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 pose_graph
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -30,33 +30,6 @@ namespace cartographer {
|
||||||
namespace mapping {
|
namespace mapping {
|
||||||
namespace pose_graph {
|
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
|
// Cost function measuring the weighted error between the observed pose given by
|
||||||
// the landmark measurement and the linearly interpolated pose.
|
// the landmark measurement and the linearly interpolated pose.
|
||||||
class LandmarkCostFunction {
|
class LandmarkCostFunction {
|
||||||
|
@ -85,8 +58,8 @@ class LandmarkCostFunction {
|
||||||
const T* const next_node_translation,
|
const T* const next_node_translation,
|
||||||
const T* const landmark_rotation,
|
const T* const landmark_rotation,
|
||||||
const T* const landmark_translation, T* const e) const {
|
const T* const landmark_translation, T* const e) const {
|
||||||
const T interpolated_pose_translation[3] = {
|
const std::array<T, 3> interpolated_pose_translation{
|
||||||
prev_node_translation[0] +
|
{prev_node_translation[0] +
|
||||||
interpolation_parameter_ *
|
interpolation_parameter_ *
|
||||||
(next_node_translation[0] - prev_node_translation[0]),
|
(next_node_translation[0] - prev_node_translation[0]),
|
||||||
prev_node_translation[1] +
|
prev_node_translation[1] +
|
||||||
|
@ -94,21 +67,17 @@ class LandmarkCostFunction {
|
||||||
(next_node_translation[1] - prev_node_translation[1]),
|
(next_node_translation[1] - prev_node_translation[1]),
|
||||||
prev_node_translation[2] +
|
prev_node_translation[2] +
|
||||||
interpolation_parameter_ *
|
interpolation_parameter_ *
|
||||||
(next_node_translation[2] - prev_node_translation[2])};
|
(next_node_translation[2] - prev_node_translation[2])}};
|
||||||
|
const std::array<T, 4> interpolated_pose_rotation = SlerpQuaternions(
|
||||||
std::array<T, 4> interpolated_pose_rotation = SlerpQuaternions(
|
|
||||||
prev_node_rotation, next_node_rotation, T(interpolation_parameter_));
|
prev_node_rotation, next_node_rotation, T(interpolation_parameter_));
|
||||||
|
|
||||||
const std::array<T, 6> unscaled_error = ComputeUnscaledError3d(
|
const std::array<T, 6> error = ScaleError(
|
||||||
landmark_to_tracking_transform_, interpolated_pose_rotation.data(),
|
ComputeUnscaledError(landmark_to_tracking_transform_,
|
||||||
interpolated_pose_translation, landmark_rotation, landmark_translation);
|
interpolated_pose_rotation.data(),
|
||||||
|
interpolated_pose_translation.data(),
|
||||||
e[0] = T(translation_weight_) * unscaled_error[0];
|
landmark_rotation, landmark_translation),
|
||||||
e[1] = T(translation_weight_) * unscaled_error[1];
|
T(translation_weight_), T(rotation_weight_));
|
||||||
e[2] = T(translation_weight_) * unscaled_error[2];
|
std::copy(std::begin(error), std::end(error), e);
|
||||||
e[3] = T(rotation_weight_) * unscaled_error[3];
|
|
||||||
e[4] = T(rotation_weight_) * unscaled_error[4];
|
|
||||||
e[5] = T(rotation_weight_) * unscaled_error[5];
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -47,21 +47,14 @@ class SpaCostFunction {
|
||||||
|
|
||||||
template <typename T>
|
template <typename T>
|
||||||
bool operator()(const T* const c_i, const T* const c_j, T* e) const {
|
bool operator()(const T* const c_i, const T* const c_j, T* e) const {
|
||||||
ComputeScaledError(pose_, c_i, c_j, e);
|
using mapping::pose_graph::ComputeUnscaledError;
|
||||||
return true;
|
using mapping::pose_graph::ScaleError;
|
||||||
}
|
|
||||||
|
|
||||||
// Computes the error scaled by 'translation_weight' and 'rotation_weight',
|
const std::array<T, 3> error = ScaleError(
|
||||||
// storing it in 'e'.
|
ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j),
|
||||||
template <typename T>
|
T(pose_.translation_weight), T(pose_.rotation_weight));
|
||||||
static void ComputeScaledError(const Constraint::Pose& pose,
|
std::copy(std::begin(error), std::end(error), e);
|
||||||
const T* const c_i, const T* const c_j,
|
return true;
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
@ -49,28 +49,15 @@ class SpaCostFunction {
|
||||||
bool operator()(const T* const c_i_rotation, const T* const c_i_translation,
|
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,
|
const T* const c_j_rotation, const T* const c_j_translation,
|
||||||
T* const e) const {
|
T* const e) const {
|
||||||
ComputeScaledError(pose_, c_i_rotation, c_i_translation, c_j_rotation,
|
using mapping::pose_graph::ComputeUnscaledError;
|
||||||
c_j_translation, e);
|
using mapping::pose_graph::ScaleError;
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Computes the error scaled by 'translation_weight' and 'rotation_weight',
|
const std::array<T, 6> error = ScaleError(
|
||||||
// storing it in 'e'.
|
ComputeUnscaledError(pose_.zbar_ij, c_i_rotation, c_i_translation,
|
||||||
template <typename T>
|
c_j_rotation, c_j_translation),
|
||||||
static void ComputeScaledError(const Constraint::Pose& pose,
|
T(pose_.translation_weight), T(pose_.rotation_weight));
|
||||||
const T* const c_i_rotation,
|
std::copy(std::begin(error), std::end(error), e);
|
||||||
const T* const c_i_translation,
|
return true;
|
||||||
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);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
Loading…
Reference in New Issue