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 8bcf0c6..7b33751 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 @@ -64,7 +64,7 @@ class LandmarkCostFunction2D { 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_)); + translation_weight_, rotation_weight_); std::copy(std::begin(error), std::end(error), e); return true; } diff --git a/cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h b/cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h index 81d3bf8..97296a2 100644 --- a/cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h @@ -50,7 +50,7 @@ class SpaCostFunction2D { const std::array error = ScaleError( 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); 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 4072844..e82b4ac 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 @@ -69,7 +69,7 @@ class LandmarkCostFunction3D { 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_)); + translation_weight_, rotation_weight_); std::copy(std::begin(error), std::end(error), e); return true; } diff --git a/cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h b/cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h index 98bacf8..d791bce 100644 --- a/cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h @@ -50,7 +50,7 @@ class SpaCostFunction3D { const std::array 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)); + pose_.translation_weight, pose_.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 e8c34bc..7b93942 100644 --- a/cartographer/mapping/internal/pose_graph/cost_helpers.h +++ b/cartographer/mapping/internal/pose_graph/cost_helpers.h @@ -35,8 +35,8 @@ static std::array ComputeUnscaledError( const transform::Rigid2d& relative_pose, const T* const start, const T* const end); template -std::array ScaleError(std::array error, T translation_weight, - T rotation_weight); +std::array ScaleError(std::array error, double translation_weight, + double 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. @@ -50,15 +50,15 @@ static std::array ComputeUnscaledError( const T* const end_translation); template -std::array ScaleError(std::array error, T translation_weight, - T rotation_weight); +std::array ScaleError(std::array error, double translation_weight, + double 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 std::array SlerpQuaternions(const T* const start, const T* const end, - T factor); + double factor); // Interpolates 3D poses. Linear interpolation is performed for translation and // spherical-linear one for rotation. diff --git a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h b/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h index d7073dd..551ec33 100644 --- a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h +++ b/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h @@ -39,8 +39,8 @@ static std::array ComputeUnscaledError( } template -std::array ScaleError(std::array error, T translation_weight, - T rotation_weight) { +std::array ScaleError(std::array error, double translation_weight, + double rotation_weight) { std::array scaled_error(std::move(error)); scaled_error[0] *= translation_weight; scaled_error[1] *= translation_weight; @@ -80,8 +80,8 @@ static std::array ComputeUnscaledError( } template -std::array ScaleError(std::array error, T translation_weight, - T rotation_weight) { +std::array ScaleError(std::array error, double translation_weight, + double rotation_weight) { std::array scaled_error(std::move(error)); scaled_error[0] *= translation_weight; scaled_error[1] *= translation_weight; @@ -96,7 +96,7 @@ std::array ScaleError(std::array error, T translation_weight, // platforms. Our own implementation is used instead. template std::array 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 // as the arccosine of their dot product. const T cos_theta = start[0] * end[0] + start[1] * end[1] + @@ -105,8 +105,8 @@ std::array SlerpQuaternions(const T* const start, const T* const end, const T abs_cos_theta = ceres::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; + T prev_scale(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); @@ -129,7 +129,7 @@ InterpolateNodes3D(const T* const prev_node_rotation, const double interpolation_parameter) { return std::make_tuple( SlerpQuaternions(prev_node_rotation, next_node_rotation, - T(interpolation_parameter)), + interpolation_parameter), std::array{ {prev_node_translation[0] + interpolation_parameter * @@ -171,7 +171,7 @@ InterpolateNodes2D(const T* const prev_node_pose, return std::make_tuple( SlerpQuaternions(prev_node_rotation.data(), next_node_rotation.data(), - T(interpolation_parameter)), + interpolation_parameter), std::array{ {prev_node_pose[0] + interpolation_parameter * (next_node_pose[0] - prev_node_pose[0]),