diff --git a/cartographer/common/math.h b/cartographer/common/math.h index f0ae9e2..f35bbad 100644 --- a/cartographer/common/math.h +++ b/cartographer/common/math.h @@ -61,10 +61,10 @@ constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; } template T NormalizeAngleDifference(T difference) { while (difference > M_PI) { - difference -= T(2. * M_PI); + difference -= 2. * M_PI; } while (difference < -M_PI) { - difference += T(2. * M_PI); + difference += 2. * M_PI; } return difference; } @@ -74,6 +74,15 @@ T atan2(const Eigen::Matrix& vector) { return ceres::atan2(vector.y(), vector.x()); } +template +inline void QuaternionProduct(const double* const z, const T* const w, + T* const zw) { + zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; + zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; + zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; + zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; +} + } // namespace common } // namespace cartographer diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h index ee7fccb..7ca59f9 100644 --- a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h @@ -64,9 +64,9 @@ class OccupiedSpaceCostFunction2D { const Eigen::Matrix world = transform * point; interpolator.Evaluate( (limits.max().x() - world[0]) / limits.resolution() - 0.5 + - T(kPadding), + double(kPadding), (limits.max().y() - world[1]) / limits.resolution() - 0.5 + - T(kPadding), + double(kPadding), &residual[i]); residual[i] = scaling_factor_ * (1. - residual[i]); } diff --git a/cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h b/cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h index cf32060..301f45b 100644 --- a/cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h +++ b/cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h @@ -20,6 +20,7 @@ #include #include "Eigen/Core" +#include "cartographer/common/math.h" #include "ceres/ceres.h" #include "ceres/rotation.h" @@ -41,12 +42,9 @@ class RotationDeltaCostFunctor3D { template bool operator()(const T* const rotation_quaternion, T* residual) const { - T delta[4]; - T target_rotation_inverse[4] = { - T(target_rotation_inverse_[0]), T(target_rotation_inverse_[1]), - T(target_rotation_inverse_[2]), T(target_rotation_inverse_[3])}; - ceres::QuaternionProduct(target_rotation_inverse, rotation_quaternion, - delta); + std::array delta; + common::QuaternionProduct(target_rotation_inverse_, rotation_quaternion, + delta.data()); // Will compute the squared norm of the imaginary component of the delta // quaternion which is sin(phi/2)^2. residual[0] = scaling_factor_ * delta[1]; diff --git a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h b/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h index 551ec33..e46c90b 100644 --- a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h +++ b/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h @@ -32,10 +32,10 @@ static std::array ComputeUnscaledError( const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y, -sin_theta_i * delta_x + cos_theta_i * delta_y, end[2] - start[2]}; - return {{T(relative_pose.translation().x()) - h[0], - T(relative_pose.translation().y()) - h[1], - common::NormalizeAngleDifference( - T(relative_pose.rotation().angle()) - h[2])}}; + return {{relative_pose.translation().x() - h[0], + relative_pose.translation().y() - h[1], + common::NormalizeAngleDifference(relative_pose.rotation().angle() - + h[2])}}; } template @@ -72,9 +72,9 @@ static std::array ComputeUnscaledError( transform::RotationQuaternionToAngleAxisVector( h_rotation_inverse * relative_pose.rotation().cast()); - return {{T(relative_pose.translation().x()) - h_translation[0], - T(relative_pose.translation().y()) - h_translation[1], - T(relative_pose.translation().z()) - h_translation[2], + return {{relative_pose.translation().x() - h_translation[0], + relative_pose.translation().y() - h_translation[1], + relative_pose.translation().z() - h_translation[2], angle_axis_difference[0], angle_axis_difference[1], angle_axis_difference[2]}}; } @@ -107,13 +107,15 @@ std::array SlerpQuaternions(const T* const start, const T* const end, // interval, then the quaternions are likely to be collinear. T prev_scale(1. - factor); T next_scale(factor); - if (abs_cos_theta < T(1. - 1e-5)) { + if (abs_cos_theta < 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; + prev_scale = sin((1. - factor) * theta) / sin_theta; + next_scale = sin(factor * theta) / sin_theta; + } + if (cos_theta < 0.) { + next_scale = -next_scale; } - 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], diff --git a/cartographer/transform/transform.h b/cartographer/transform/transform.h index 2fd9b05..3d4b4c0 100644 --- a/cartographer/transform/transform.h +++ b/cartographer/transform/transform.h @@ -64,17 +64,17 @@ Eigen::Matrix RotationQuaternionToAngleAxisVector( // angle that represents this orientation. if (normalized_quaternion.w() < 0.) { // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560 - normalized_quaternion.w() *= T(-1.); - normalized_quaternion.x() *= T(-1.); - normalized_quaternion.y() *= T(-1.); - normalized_quaternion.z() *= T(-1.); + normalized_quaternion.w() *= -1.; + normalized_quaternion.x() *= -1.; + normalized_quaternion.y() *= -1.; + normalized_quaternion.z() *= -1.; } // We convert the normalized_quaternion into a vector along the rotation axis // with length of the rotation angle. - const T angle = T(2.) * atan2(normalized_quaternion.vec().norm(), - normalized_quaternion.w()); + const T angle = + 2. * atan2(normalized_quaternion.vec().norm(), normalized_quaternion.w()); constexpr double kCutoffAngle = 1e-7; // We linearize below this angle. - const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / T(2.)); + const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / 2.); return Eigen::Matrix(scale * normalized_quaternion.x(), scale * normalized_quaternion.y(), scale * normalized_quaternion.z());