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

New fixes for Jets. (#1023) 

Issue #1015
master
Alexander Belyaev 2018-03-27 14:26:21 +02:00 committed by gaschler
parent 94f564d871
commit 962393074a
3 changed files with 16 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cartographer/common/math.h
View File

@ -60,12 +60,9 @@ constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; }
// Bring the 'difference' between two angles into [-pi; pi]. // Bring the 'difference' between two angles into [-pi; pi].
template <typename T> template <typename T>
T NormalizeAngleDifference(T difference) { T NormalizeAngleDifference(T difference) {
while (difference > M_PI) { const T kPi = T(M_PI);
difference -= 2. * M_PI; while (difference > kPi) difference -= 2. * kPi;
} while (difference < -kPi) difference += 2. * kPi;
while (difference < -M_PI) {
difference += 2. * M_PI;
}
return difference; return difference;
} }

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

@ -32,10 +32,10 @@ static std::array<T, 3> ComputeUnscaledError(
const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y, const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y,
-sin_theta_i * delta_x + cos_theta_i * delta_y, -sin_theta_i * delta_x + cos_theta_i * delta_y,
end[2] - start[2]}; end[2] - start[2]};
return {{relative_pose.translation().x() - h[0], return {{T(relative_pose.translation().x()) - h[0],
relative_pose.translation().y() - h[1], T(relative_pose.translation().y()) - h[1],
common::NormalizeAngleDifference(relative_pose.rotation().angle() - common::NormalizeAngleDifference(
h[2])}}; T(relative_pose.rotation().angle()) - h[2])}};
} }
template <typename T> template <typename T>
@ -74,9 +74,9 @@ static std::array<T, 6> ComputeUnscaledError(
transform::RotationQuaternionToAngleAxisVector( transform::RotationQuaternionToAngleAxisVector(
h_rotation_inverse * relative_pose.rotation().cast<T>()); h_rotation_inverse * relative_pose.rotation().cast<T>());
return {{relative_pose.translation().x() - h_translation[0], return {{T(relative_pose.translation().x()) - h_translation[0],
relative_pose.translation().y() - h_translation[1], T(relative_pose.translation().y()) - h_translation[1],
relative_pose.translation().z() - h_translation[2], T(relative_pose.translation().z()) - h_translation[2],
angle_axis_difference[0], angle_axis_difference[1], angle_axis_difference[0], angle_axis_difference[1],
angle_axis_difference[2]}}; angle_axis_difference[2]}};
} }
@ -111,13 +111,13 @@ std::array<T, 4> SlerpQuaternions(const T* const start, const T* const end,
// interval, then the quaternions are likely to be collinear. // interval, then the quaternions are likely to be collinear.
T prev_scale(1. - factor); T prev_scale(1. - factor);
T next_scale(factor); T next_scale(factor);
if (abs_cos_theta < 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);
prev_scale = sin((1. - factor) * theta) / sin_theta; prev_scale = sin((1. - factor) * theta) / sin_theta;
next_scale = sin(factor * theta) / sin_theta; next_scale = sin(factor * theta) / sin_theta;
} }
if (cos_theta < 0.) { if (cos_theta < T(0.)) {
next_scale = -next_scale; next_scale = -next_scale;
} }
return {{prev_scale * start[0] + next_scale * end[0], return {{prev_scale * start[0] + next_scale * end[0],