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

View File

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

View File

@ -35,8 +35,8 @@ static std::array<T, 3> ComputeUnscaledError(
const transform::Rigid2d& relative_pose, const T* const start,
const T* const end);
template <typename T>
std::array<T, 3> ScaleError(std::array<T, 3> error, double translation_weight,
double rotation_weight);
std::array<T, 3> ScaleError(const std::array<T, 3>& 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,8 +50,8 @@ static std::array<T, 6> ComputeUnscaledError(
const T* const end_translation);
template <typename T>
std::array<T, 6> ScaleError(std::array<T, 6> error, double translation_weight,
double rotation_weight);
std::array<T, 6> ScaleError(const std::array<T, 6>& error,
double translation_weight, double rotation_weight);
// Computes spherical linear interpolation of unit quaternions.
//

View File

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

View File

@ -64,10 +64,10 @@ Eigen::Matrix<T, 3, 1> 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() *= -1.;
normalized_quaternion.x() *= -1.;
normalized_quaternion.y() *= -1.;
normalized_quaternion.z() *= -1.;
normalized_quaternion.w() = -1.* normalized_quaternion.w();
normalized_quaternion.x() = -1.* normalized_quaternion.x();
normalized_quaternion.y() = -1.* normalized_quaternion.y();
normalized_quaternion.z() = -1.* normalized_quaternion.z();
}
// We convert the normalized_quaternion into a vector along the rotation axis
// with length of the rotation angle.