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

Do not use operator*= with Jets. (#1018)

master
Alexander Belyaev 2018-03-26 15:55:50 +02:00 committed by GitHub
parent ae05658ff7
commit e0e1c081e7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 31 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h
View File

@ -64,9 +64,9 @@ class OccupiedSpaceCostFunction2D {
const Eigen::Matrix<T, 3, 1> world = transform * point; const Eigen::Matrix<T, 3, 1> world = transform * point;
interpolator.Evaluate( interpolator.Evaluate(
(limits.max().x() - world[0]) / limits.resolution() - 0.5 + (limits.max().x() - world[0]) / limits.resolution() - 0.5 +
double(kPadding), static_cast<double>(kPadding),
(limits.max().y() - world[1]) / limits.resolution() - 0.5 + (limits.max().y() - world[1]) / limits.resolution() - 0.5 +
double(kPadding), static_cast<double>(kPadding),
&residual[i]); &residual[i]);
residual[i] = scaling_factor_ * (1. - residual[i]); residual[i] = scaling_factor_ * (1. - residual[i]);
} }

8
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, double translation_weight, std::array<T, 3> ScaleError(const std::array<T, 3>& error,
double rotation_weight); double translation_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,8 +50,8 @@ 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, double translation_weight, std::array<T, 6> ScaleError(const std::array<T, 6>& error,
double rotation_weight); double translation_weight, double rotation_weight);
// Computes spherical linear interpolation of unit quaternions. // Computes spherical linear interpolation of unit quaternions.
// //

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

@ -39,13 +39,15 @@ static std::array<T, 3> ComputeUnscaledError(
} }
template <typename T> template <typename T>
std::array<T, 3> ScaleError(std::array<T, 3> error, double translation_weight, std::array<T, 3> ScaleError(const std::array<T, 3>& error,
double rotation_weight) { double translation_weight, double rotation_weight) {
std::array<T, 3> scaled_error(std::move(error)); // clang-format off
scaled_error[0] *= translation_weight; return {{
scaled_error[1] *= translation_weight; error[0] * translation_weight,
scaled_error[2] *= rotation_weight; error[1] * translation_weight,
return scaled_error; error[2] * rotation_weight
}};
// clang-format on
} }
template <typename T> template <typename T>
@ -80,16 +82,18 @@ static std::array<T, 6> ComputeUnscaledError(
} }
template <typename T> template <typename T>
std::array<T, 6> ScaleError(std::array<T, 6> error, double translation_weight, std::array<T, 6> ScaleError(const std::array<T, 6>& error,
double rotation_weight) { double translation_weight, double rotation_weight) {
std::array<T, 6> scaled_error(std::move(error)); // clang-format off
scaled_error[0] *= translation_weight; return {{
scaled_error[1] *= translation_weight; error[0] * translation_weight,
scaled_error[2] *= translation_weight; error[1] * translation_weight,
scaled_error[3] *= rotation_weight; error[2] * translation_weight,
scaled_error[4] *= rotation_weight; error[3] * rotation_weight,
scaled_error[5] *= rotation_weight; error[4] * rotation_weight,
return scaled_error; error[5] * rotation_weight
}};
// clang-format on
} }
// Eigen implementation of slerp is not compatible with Ceres on all supported // Eigen implementation of slerp is not compatible with Ceres on all supported

8
cartographer/transform/transform.h
View File

@ -64,10 +64,10 @@ Eigen::Matrix<T, 3, 1> RotationQuaternionToAngleAxisVector(
// angle that represents this orientation. // angle that represents this orientation.
if (normalized_quaternion.w() < 0.) { if (normalized_quaternion.w() < 0.) {
// Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560 // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560
normalized_quaternion.w() *= -1.; normalized_quaternion.w() = -1.* normalized_quaternion.w();
normalized_quaternion.x() *= -1.; normalized_quaternion.x() = -1.* normalized_quaternion.x();
normalized_quaternion.y() *= -1.; normalized_quaternion.y() = -1.* normalized_quaternion.y();
normalized_quaternion.z() *= -1.; normalized_quaternion.z() = -1.* normalized_quaternion.z();
} }
// We convert the normalized_quaternion into a vector along the rotation axis // We convert the normalized_quaternion into a vector along the rotation axis
// with length of the rotation angle. // with length of the rotation angle.