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

Remove float-to-jet casts where possible. (#998)

master
Alexander Belyaev 2018-03-16 14:53:10 +01:00 committed by GitHub
parent 6102f374b1
commit 1f9c78a82b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -64,7 +64,7 @@ class LandmarkCostFunction2D {
std::get<0>(interpolated_rotation_and_translation).data(), std::get<0>(interpolated_rotation_and_translation).data(),
std::get<1>(interpolated_rotation_and_translation).data(), std::get<1>(interpolated_rotation_and_translation).data(),
landmark_rotation, landmark_translation), landmark_rotation, landmark_translation),
T(translation_weight_), T(rotation_weight_)); translation_weight_, rotation_weight_);
std::copy(std::begin(error), std::end(error), e); std::copy(std::begin(error), std::end(error), e);
return true; return true;
} }

2
cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h
View File

@ -50,7 +50,7 @@ class SpaCostFunction2D {
const std::array<T, 3> error = ScaleError( const std::array<T, 3> error = ScaleError(
ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j), ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j),
T(pose_.translation_weight), T(pose_.rotation_weight)); pose_.translation_weight, pose_.rotation_weight);
std::copy(std::begin(error), std::end(error), e); std::copy(std::begin(error), std::end(error), e);
return true; return true;
} }

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

@ -69,7 +69,7 @@ class LandmarkCostFunction3D {
std::get<0>(interpolated_rotation_and_translation).data(), std::get<0>(interpolated_rotation_and_translation).data(),
std::get<1>(interpolated_rotation_and_translation).data(), std::get<1>(interpolated_rotation_and_translation).data(),
landmark_rotation, landmark_translation), landmark_rotation, landmark_translation),
T(translation_weight_), T(rotation_weight_)); translation_weight_, rotation_weight_);
std::copy(std::begin(error), std::end(error), e); std::copy(std::begin(error), std::end(error), e);
return true; return true;
} }

2
cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h
View File

@ -50,7 +50,7 @@ class SpaCostFunction3D {
const std::array<T, 6> error = ScaleError( const std::array<T, 6> error = ScaleError(
ComputeUnscaledError(pose_.zbar_ij, c_i_rotation, c_i_translation, ComputeUnscaledError(pose_.zbar_ij, c_i_rotation, c_i_translation,
c_j_rotation, c_j_translation), c_j_rotation, c_j_translation),
T(pose_.translation_weight), T(pose_.rotation_weight)); pose_.translation_weight, pose_.rotation_weight);
std::copy(std::begin(error), std::end(error), e); std::copy(std::begin(error), std::end(error), e);
return true; return true;
} }

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

@ -35,8 +35,8 @@ 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);
template <typename T> template <typename T>
std::array<T, 3> ScaleError(std::array<T, 3> error, T translation_weight, std::array<T, 3> ScaleError(std::array<T, 3> error, double translation_weight,
T rotation_weight); double 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.
@ -50,15 +50,15 @@ static std::array<T, 6> ComputeUnscaledError(
const T* const end_translation); const T* const end_translation);
template <typename T> template <typename T>
std::array<T, 6> ScaleError(std::array<T, 6> error, T translation_weight, std::array<T, 6> ScaleError(std::array<T, 6> error, double translation_weight,
T rotation_weight); double rotation_weight);
// Computes spherical linear interpolation of unit quaternions. // Computes spherical linear interpolation of unit quaternions.
// //
// 'start' and 'end' are quaternions in the format [w, n_1, n_2, n_3]. // 'start' and 'end' are quaternions in the format [w, n_1, n_2, n_3].
template <typename T> template <typename T>
std::array<T, 4> SlerpQuaternions(const T* const start, const T* const end, std::array<T, 4> SlerpQuaternions(const T* const start, const T* const end,
T factor); double factor);
// Interpolates 3D poses. Linear interpolation is performed for translation and // Interpolates 3D poses. Linear interpolation is performed for translation and
// spherical-linear one for rotation. // spherical-linear one for rotation.

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

@ -39,8 +39,8 @@ static std::array<T, 3> ComputeUnscaledError(
} }
template <typename T> template <typename T>
std::array<T, 3> ScaleError(std::array<T, 3> error, T translation_weight, std::array<T, 3> ScaleError(std::array<T, 3> error, double translation_weight,
T rotation_weight) { double rotation_weight) {
std::array<T, 3> scaled_error(std::move(error)); std::array<T, 3> scaled_error(std::move(error));
scaled_error[0] *= translation_weight; scaled_error[0] *= translation_weight;
scaled_error[1] *= translation_weight; scaled_error[1] *= translation_weight;
@ -80,8 +80,8 @@ static std::array<T, 6> ComputeUnscaledError(
} }
template <typename T> template <typename T>
std::array<T, 6> ScaleError(std::array<T, 6> error, T translation_weight, std::array<T, 6> ScaleError(std::array<T, 6> error, double translation_weight,
T rotation_weight) { double rotation_weight) {
std::array<T, 6> scaled_error(std::move(error)); std::array<T, 6> scaled_error(std::move(error));
scaled_error[0] *= translation_weight; scaled_error[0] *= translation_weight;
scaled_error[1] *= translation_weight; scaled_error[1] *= translation_weight;
@ -96,7 +96,7 @@ std::array<T, 6> ScaleError(std::array<T, 6> error, T translation_weight,
// platforms. Our own implementation is used instead. // platforms. Our own implementation is used instead.
template <typename T> template <typename T>
std::array<T, 4> SlerpQuaternions(const T* const start, const T* const end, std::array<T, 4> SlerpQuaternions(const T* const start, const T* const end,
T factor) { double factor) {
// Angle 'theta' is the half-angle "between" quaternions. It can be computed // Angle 'theta' is the half-angle "between" quaternions. It can be computed
// as the arccosine of their dot product. // as the arccosine of their dot product.
const T cos_theta = start[0] * end[0] + start[1] * end[1] + const T cos_theta = start[0] * end[0] + start[1] * end[1] +
@ -105,8 +105,8 @@ std::array<T, 4> SlerpQuaternions(const T* const start, const T* const end,
const T abs_cos_theta = ceres::abs(cos_theta); const T abs_cos_theta = ceres::abs(cos_theta);
// If numerical error brings 'cos_theta' outside [-1 + epsilon, 1 - epsilon] // If numerical error brings 'cos_theta' outside [-1 + epsilon, 1 - epsilon]
// interval, then the quaternions are likely to be collinear. // interval, then the quaternions are likely to be collinear.
T prev_scale = T(1.) - factor; T prev_scale(1. - factor);
T next_scale = factor; T next_scale(factor);
if (abs_cos_theta < T(1. - 1e-5)) { if (abs_cos_theta < T(1. - 1e-5)) {
const T theta = acos(abs_cos_theta); const T theta = acos(abs_cos_theta);
const T sin_theta = sin(theta); const T sin_theta = sin(theta);
@ -129,7 +129,7 @@ InterpolateNodes3D(const T* const prev_node_rotation,
const double interpolation_parameter) { const double interpolation_parameter) {
return std::make_tuple( return std::make_tuple(
SlerpQuaternions(prev_node_rotation, next_node_rotation, SlerpQuaternions(prev_node_rotation, next_node_rotation,
T(interpolation_parameter)), interpolation_parameter),
std::array<T, 3>{ std::array<T, 3>{
{prev_node_translation[0] + {prev_node_translation[0] +
interpolation_parameter * interpolation_parameter *
@ -171,7 +171,7 @@ InterpolateNodes2D(const T* const prev_node_pose,
return std::make_tuple( return std::make_tuple(
SlerpQuaternions(prev_node_rotation.data(), next_node_rotation.data(), SlerpQuaternions(prev_node_rotation.data(), next_node_rotation.data(),
T(interpolation_parameter)), interpolation_parameter),
std::array<T, 3>{ std::array<T, 3>{
{prev_node_pose[0] + interpolation_parameter * {prev_node_pose[0] + interpolation_parameter *
(next_node_pose[0] - prev_node_pose[0]), (next_node_pose[0] - prev_node_pose[0]),