From c0aa9a2631f98e1a8609cbbe997b82eb644de656 Mon Sep 17 00:00:00 2001 From: Johannes Beck Date: Tue, 16 Apr 2019 21:12:31 +0200 Subject: [PATCH] Add checks in rotation.h for inplace operations. Some rotation functions like UnitQuaternionRotatePoint, QuaternionRotatePoint, etc. will calculate bad results if the input point and output point points to the same memory (inplace operation). This CL adds checks in debug mode to guard against it. Change-Id: Id0a30e9a0286b340757f0790d417d9f9a3409810 --- include/ceres/rotation.h | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h index a0530dde7..71d64e212 100644 --- a/include/ceres/rotation.h +++ b/include/ceres/rotation.h @@ -49,6 +49,8 @@ #include #include +#include "glog/logging.h" + namespace ceres { // Trivial wrapper to index linear arrays as matrices, given a fixed @@ -185,19 +187,33 @@ void QuaternionToRotation( // write the transform as (something)*pt + pt, as is clear from the // formula below. If you pass in a quaternion with |q|^2 = 2 then you // WILL NOT get back 2 times the result you get for a unit quaternion. +// +// Inplace rotation is not supported. pt and result must point to different +// memory locations, otherwise the result will be undefined. template inline void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); // With this function you do not need to assume that q has unit norm. // It does assume that the norm is non-zero. +// +// Inplace rotation is not supported. pt and result must point to different +// memory locations, otherwise the result will be undefined. template inline void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); // zw = z * w, where * is the Quaternion product between 4 vectors. +// +// Inplace quaternion product is not supported. The resulting quaternion zw must +// not share the memory with the input quaternion z and w, otherwise the result +// will be undefined. template inline void QuaternionProduct(const T z[4], const T w[4], T zw[4]); // xy = x cross y; +// +// Inplace cross product is not support. The resulting vector x_cross_y must not +// share the memory with the input vectors x and y, otherwise the result will be +// undefined. template inline void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); @@ -205,6 +221,9 @@ template inline T DotProduct(const T x[3], const T y[3]); // y = R(angle_axis) * x; +// +// Inplace rotation is not supported. pt and result must point to different +// memory locations, otherwise the result will be undefined. template inline void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]); @@ -505,6 +524,8 @@ void QuaternionToRotation(const T q[4], template inline void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + DCHECK_NE(pt, result) << "Inplace rotation is not supported."; + const T t2 = q[0] * q[1]; const T t3 = q[0] * q[2]; const T t4 = q[0] * q[3]; @@ -521,6 +542,8 @@ void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { template inline void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + DCHECK_NE(pt, result) << "Inplace rotation is not supported."; + // 'scale' is 1 / norm(q). const T scale = T(1) / sqrt(q[0] * q[0] + q[1] * q[1] + @@ -540,6 +563,9 @@ void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { template inline void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { + DCHECK_NE(z, zw) << "Inplace quaternion product is not supported."; + DCHECK_NE(w, zw) << "Inplace quaternion product is not supported."; + 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]; @@ -549,6 +575,9 @@ void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { // xy = x cross y; template inline void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) { + DCHECK_NE(x, x_cross_y) << "Inplace cross product is not supported."; + DCHECK_NE(y, x_cross_y) << "Inplace cross product is not supported."; + x_cross_y[0] = x[1] * y[2] - x[2] * y[1]; x_cross_y[1] = x[2] * y[0] - x[0] * y[2]; x_cross_y[2] = x[0] * y[1] - x[1] * y[0]; @@ -561,6 +590,8 @@ T DotProduct(const T x[3], const T y[3]) { template inline void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { + DCHECK_NE(pt, result) << "Inplace rotation is not supported."; + const T theta2 = DotProduct(angle_axis, angle_axis); if (theta2 > T(std::numeric_limits::epsilon())) { // Away from zero, use the rodriguez formula