Do not use operator*= with Jets. (#1018)
parent
ae05658ff7
commit
e0e1c081e7
|
@ -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]);
|
||||
}
|
||||
|
|
|
@ -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.
|
||||
//
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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.
|
||||
|
|
Loading…
Reference in New Issue