diff options
Diffstat (limited to 'intern/libmv/libmv/numeric/numeric.h')
| -rw-r--r-- | intern/libmv/libmv/numeric/numeric.h | 393 |
1 files changed, 192 insertions, 201 deletions
diff --git a/intern/libmv/libmv/numeric/numeric.h b/intern/libmv/libmv/numeric/numeric.h index f5478bee6ab..e3d44226338 100644 --- a/intern/libmv/libmv/numeric/numeric.h +++ b/intern/libmv/libmv/numeric/numeric.h @@ -34,10 +34,9 @@ #include <Eigen/SVD> #if !defined(__MINGW64__) -# if defined(_WIN32) || defined(__APPLE__) || \ - defined(__FreeBSD__) || defined(__NetBSD__) || \ - defined(__HAIKU__) -inline void sincos(double x, double *sinx, double *cosx) { +# if defined(_WIN32) || defined(__APPLE__) || defined(__FreeBSD__) || \ + defined(__NetBSD__) || defined(__HAIKU__) +inline void sincos(double x, double* sinx, double* cosx) { *sinx = sin(x); *cosx = cos(x); } @@ -46,11 +45,11 @@ inline void sincos(double x, double *sinx, double *cosx) { #if (defined(WIN32) || defined(WIN64)) && !defined(__MINGW32__) inline long lround(double d) { - return (long)(d>0 ? d+0.5 : ceil(d-0.5)); + return (long)(d > 0 ? d + 0.5 : ceil(d - 0.5)); } # if _MSC_VER < 1800 inline int round(double d) { - return (d>0) ? int(d+0.5) : int(d-0.5); + return (d > 0) ? int(d + 0.5) : int(d - 0.5); } # endif // _MSC_VER < 1800 typedef unsigned int uint; @@ -92,25 +91,25 @@ typedef Eigen::Matrix<double, 4, 4, Eigen::RowMajor> RMat4; typedef Eigen::Matrix<double, 2, Eigen::Dynamic> Mat2X; typedef Eigen::Matrix<double, 3, Eigen::Dynamic> Mat3X; typedef Eigen::Matrix<double, 4, Eigen::Dynamic> Mat4X; -typedef Eigen::Matrix<double, Eigen::Dynamic, 2> MatX2; -typedef Eigen::Matrix<double, Eigen::Dynamic, 3> MatX3; -typedef Eigen::Matrix<double, Eigen::Dynamic, 4> MatX4; -typedef Eigen::Matrix<double, Eigen::Dynamic, 5> MatX5; -typedef Eigen::Matrix<double, Eigen::Dynamic, 6> MatX6; -typedef Eigen::Matrix<double, Eigen::Dynamic, 7> MatX7; -typedef Eigen::Matrix<double, Eigen::Dynamic, 8> MatX8; -typedef Eigen::Matrix<double, Eigen::Dynamic, 9> MatX9; +typedef Eigen::Matrix<double, Eigen::Dynamic, 2> MatX2; +typedef Eigen::Matrix<double, Eigen::Dynamic, 3> MatX3; +typedef Eigen::Matrix<double, Eigen::Dynamic, 4> MatX4; +typedef Eigen::Matrix<double, Eigen::Dynamic, 5> MatX5; +typedef Eigen::Matrix<double, Eigen::Dynamic, 6> MatX6; +typedef Eigen::Matrix<double, Eigen::Dynamic, 7> MatX7; +typedef Eigen::Matrix<double, Eigen::Dynamic, 8> MatX8; +typedef Eigen::Matrix<double, Eigen::Dynamic, 9> MatX9; typedef Eigen::Matrix<double, Eigen::Dynamic, 15> MatX15; typedef Eigen::Matrix<double, Eigen::Dynamic, 16> MatX16; typedef Eigen::Vector2d Vec2; typedef Eigen::Vector3d Vec3; typedef Eigen::Vector4d Vec4; -typedef Eigen::Matrix<double, 5, 1> Vec5; -typedef Eigen::Matrix<double, 6, 1> Vec6; -typedef Eigen::Matrix<double, 7, 1> Vec7; -typedef Eigen::Matrix<double, 8, 1> Vec8; -typedef Eigen::Matrix<double, 9, 1> Vec9; +typedef Eigen::Matrix<double, 5, 1> Vec5; +typedef Eigen::Matrix<double, 6, 1> Vec6; +typedef Eigen::Matrix<double, 7, 1> Vec7; +typedef Eigen::Matrix<double, 8, 1> Vec8; +typedef Eigen::Matrix<double, 9, 1> Vec9; typedef Eigen::Matrix<double, 10, 1> Vec10; typedef Eigen::Matrix<double, 11, 1> Vec11; typedef Eigen::Matrix<double, 12, 1> Vec12; @@ -133,15 +132,13 @@ typedef Eigen::Vector2i Vec2i; typedef Eigen::Vector3i Vec3i; typedef Eigen::Vector4i Vec4i; -typedef Eigen::Matrix<float, - Eigen::Dynamic, - Eigen::Dynamic, - Eigen::RowMajor> RMatf; +typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> + RMatf; typedef Eigen::NumTraits<double> EigenDouble; -using Eigen::Map; using Eigen::Dynamic; +using Eigen::Map; using Eigen::Matrix; // Find U, s, and VT such that @@ -149,7 +146,7 @@ using Eigen::Matrix; // A = U * diag(s) * VT // template <typename TMat, typename TVec> -inline void SVD(TMat * /*A*/, Vec * /*s*/, Mat * /*U*/, Mat * /*VT*/) { +inline void SVD(TMat* /*A*/, Vec* /*s*/, Mat* /*U*/, Mat* /*VT*/) { assert(0); } @@ -158,11 +155,11 @@ inline void SVD(TMat * /*A*/, Vec * /*s*/, Mat * /*U*/, Mat * /*VT*/) { // Destroys A and resizes x if necessary. // TODO(maclean): Take the SVD of the transpose instead of this zero padding. template <typename TMat, typename TVec> -double Nullspace(TMat *A, TVec *nullspace) { +double Nullspace(TMat* A, TVec* nullspace) { Eigen::JacobiSVD<TMat> svd(*A, Eigen::ComputeFullV); - (*nullspace) = svd.matrixV().col(A->cols()-1); + (*nullspace) = svd.matrixV().col(A->cols() - 1); if (A->rows() >= A->cols()) - return svd.singularValues()(A->cols()-1); + return svd.singularValues()(A->cols() - 1); else return 0.0; } @@ -173,55 +170,55 @@ double Nullspace(TMat *A, TVec *nullspace) { // the singluar value corresponding to the solution x1. Destroys A and resizes // x if necessary. template <typename TMat, typename TVec1, typename TVec2> -double Nullspace2(TMat *A, TVec1 *x1, TVec2 *x2) { +double Nullspace2(TMat* A, TVec1* x1, TVec2* x2) { Eigen::JacobiSVD<TMat> svd(*A, Eigen::ComputeFullV); *x1 = svd.matrixV().col(A->cols() - 1); *x2 = svd.matrixV().col(A->cols() - 2); if (A->rows() >= A->cols()) - return svd.singularValues()(A->cols()-1); + return svd.singularValues()(A->cols() - 1); else return 0.0; } // In place transpose for square matrices. -template<class TA> -inline void TransposeInPlace(TA *A) { +template <class TA> +inline void TransposeInPlace(TA* A) { *A = A->transpose().eval(); } -template<typename TVec> -inline double NormL1(const TVec &x) { +template <typename TVec> +inline double NormL1(const TVec& x) { return x.array().abs().sum(); } -template<typename TVec> -inline double NormL2(const TVec &x) { +template <typename TVec> +inline double NormL2(const TVec& x) { return x.norm(); } -template<typename TVec> -inline double NormLInfinity(const TVec &x) { +template <typename TVec> +inline double NormLInfinity(const TVec& x) { return x.array().abs().maxCoeff(); } -template<typename TVec> -inline double DistanceL1(const TVec &x, const TVec &y) { +template <typename TVec> +inline double DistanceL1(const TVec& x, const TVec& y) { return (x - y).array().abs().sum(); } -template<typename TVec> -inline double DistanceL2(const TVec &x, const TVec &y) { +template <typename TVec> +inline double DistanceL2(const TVec& x, const TVec& y) { return (x - y).norm(); } -template<typename TVec> -inline double DistanceLInfinity(const TVec &x, const TVec &y) { +template <typename TVec> +inline double DistanceLInfinity(const TVec& x, const TVec& y) { return (x - y).array().abs().maxCoeff(); } // Normalize a vector with the L1 norm, and return the norm before it was // normalized. -template<typename TVec> -inline double NormalizeL1(TVec *x) { +template <typename TVec> +inline double NormalizeL1(TVec* x) { double norm = NormL1(*x); *x /= norm; return norm; @@ -229,8 +226,8 @@ inline double NormalizeL1(TVec *x) { // Normalize a vector with the L2 norm, and return the norm before it was // normalized. -template<typename TVec> -inline double NormalizeL2(TVec *x) { +template <typename TVec> +inline double NormalizeL2(TVec* x) { double norm = NormL2(*x); *x /= norm; return norm; @@ -238,15 +235,15 @@ inline double NormalizeL2(TVec *x) { // Normalize a vector with the L^Infinity norm, and return the norm before it // was normalized. -template<typename TVec> -inline double NormalizeLInfinity(TVec *x) { +template <typename TVec> +inline double NormalizeLInfinity(TVec* x) { double norm = NormLInfinity(*x); *x /= norm; return norm; } // Return the square of a number. -template<typename T> +template <typename T> inline T Square(T x) { return x * x; } @@ -258,7 +255,7 @@ Mat3 RotationAroundZ(double angle); // Returns the rotation matrix of a rotation of angle |axis| around axis. // This is computed using the Rodrigues formula, see: // http://mathworld.wolfram.com/RodriguesRotationFormula.html -Mat3 RotationRodrigues(const Vec3 &axis); +Mat3 RotationRodrigues(const Vec3& axis); // Make a rotation matrix such that center becomes the direction of the // positive z-axis, and y is oriented close to up. @@ -266,177 +263,173 @@ Mat3 LookAt(Vec3 center); // Return a diagonal matrix from a vector containg the diagonal values. template <typename TVec> -inline Mat Diag(const TVec &x) { +inline Mat Diag(const TVec& x) { return x.asDiagonal(); } -template<typename TMat> -inline double FrobeniusNorm(const TMat &A) { +template <typename TMat> +inline double FrobeniusNorm(const TMat& A) { return sqrt(A.array().abs2().sum()); } -template<typename TMat> -inline double FrobeniusDistance(const TMat &A, const TMat &B) { +template <typename TMat> +inline double FrobeniusDistance(const TMat& A, const TMat& B) { return FrobeniusNorm(A - B); } -inline Vec3 CrossProduct(const Vec3 &x, const Vec3 &y) { +inline Vec3 CrossProduct(const Vec3& x, const Vec3& y) { return x.cross(y); } -Mat3 CrossProductMatrix(const Vec3 &x); +Mat3 CrossProductMatrix(const Vec3& x); -void MeanAndVarianceAlongRows(const Mat &A, - Vec *mean_pointer, - Vec *variance_pointer); +void MeanAndVarianceAlongRows(const Mat& A, + Vec* mean_pointer, + Vec* variance_pointer); #if defined(_WIN32) - // TODO(bomboze): un-#if this for both platforms once tested under Windows - /* This solution was extensively discussed here - http://forum.kde.org/viewtopic.php?f=74&t=61940 */ - #define SUM_OR_DYNAMIC(x, y) (x == Eigen::Dynamic || y == Eigen::Dynamic) ? Eigen::Dynamic : (x+y) - - template<typename Derived1, typename Derived2> - struct hstack_return { - typedef typename Derived1::Scalar Scalar; - enum { - RowsAtCompileTime = Derived1::RowsAtCompileTime, - ColsAtCompileTime = SUM_OR_DYNAMIC(Derived1::ColsAtCompileTime, - Derived2::ColsAtCompileTime), - Options = Derived1::Flags&Eigen::RowMajorBit ? Eigen::RowMajor : 0, - MaxRowsAtCompileTime = Derived1::MaxRowsAtCompileTime, - MaxColsAtCompileTime = SUM_OR_DYNAMIC(Derived1::MaxColsAtCompileTime, - Derived2::MaxColsAtCompileTime) - }; - typedef Eigen::Matrix<Scalar, - RowsAtCompileTime, - ColsAtCompileTime, - Options, - MaxRowsAtCompileTime, - MaxColsAtCompileTime> type; +// TODO(bomboze): un-#if this for both platforms once tested under Windows +/* This solution was extensively discussed here + http://forum.kde.org/viewtopic.php?f=74&t=61940 */ +# define SUM_OR_DYNAMIC(x, y) \ + (x == Eigen::Dynamic || y == Eigen::Dynamic) ? Eigen::Dynamic : (x + y) + +template <typename Derived1, typename Derived2> +struct hstack_return { + typedef typename Derived1::Scalar Scalar; + enum { + RowsAtCompileTime = Derived1::RowsAtCompileTime, + ColsAtCompileTime = SUM_OR_DYNAMIC(Derived1::ColsAtCompileTime, + Derived2::ColsAtCompileTime), + Options = Derived1::Flags & Eigen::RowMajorBit ? Eigen::RowMajor : 0, + MaxRowsAtCompileTime = Derived1::MaxRowsAtCompileTime, + MaxColsAtCompileTime = SUM_OR_DYNAMIC(Derived1::MaxColsAtCompileTime, + Derived2::MaxColsAtCompileTime) }; - - template<typename Derived1, typename Derived2> - typename hstack_return<Derived1, Derived2>::type - HStack(const Eigen::MatrixBase<Derived1>& lhs, - const Eigen::MatrixBase<Derived2>& rhs) { - typename hstack_return<Derived1, Derived2>::type res; - res.resize(lhs.rows(), lhs.cols()+rhs.cols()); - res << lhs, rhs; - return res; - }; - - - template<typename Derived1, typename Derived2> - struct vstack_return { - typedef typename Derived1::Scalar Scalar; - enum { - RowsAtCompileTime = SUM_OR_DYNAMIC(Derived1::RowsAtCompileTime, - Derived2::RowsAtCompileTime), - ColsAtCompileTime = Derived1::ColsAtCompileTime, - Options = Derived1::Flags&Eigen::RowMajorBit ? Eigen::RowMajor : 0, - MaxRowsAtCompileTime = SUM_OR_DYNAMIC(Derived1::MaxRowsAtCompileTime, - Derived2::MaxRowsAtCompileTime), - MaxColsAtCompileTime = Derived1::MaxColsAtCompileTime - }; - typedef Eigen::Matrix<Scalar, - RowsAtCompileTime, - ColsAtCompileTime, - Options, - MaxRowsAtCompileTime, - MaxColsAtCompileTime> type; + typedef Eigen::Matrix<Scalar, + RowsAtCompileTime, + ColsAtCompileTime, + Options, + MaxRowsAtCompileTime, + MaxColsAtCompileTime> + type; +}; + +template <typename Derived1, typename Derived2> +typename hstack_return<Derived1, Derived2>::type HStack( + const Eigen::MatrixBase<Derived1>& lhs, + const Eigen::MatrixBase<Derived2>& rhs) { + typename hstack_return<Derived1, Derived2>::type res; + res.resize(lhs.rows(), lhs.cols() + rhs.cols()); + res << lhs, rhs; + return res; +}; + +template <typename Derived1, typename Derived2> +struct vstack_return { + typedef typename Derived1::Scalar Scalar; + enum { + RowsAtCompileTime = SUM_OR_DYNAMIC(Derived1::RowsAtCompileTime, + Derived2::RowsAtCompileTime), + ColsAtCompileTime = Derived1::ColsAtCompileTime, + Options = Derived1::Flags & Eigen::RowMajorBit ? Eigen::RowMajor : 0, + MaxRowsAtCompileTime = SUM_OR_DYNAMIC(Derived1::MaxRowsAtCompileTime, + Derived2::MaxRowsAtCompileTime), + MaxColsAtCompileTime = Derived1::MaxColsAtCompileTime }; - - template<typename Derived1, typename Derived2> - typename vstack_return<Derived1, Derived2>::type - VStack(const Eigen::MatrixBase<Derived1>& lhs, - const Eigen::MatrixBase<Derived2>& rhs) { - typename vstack_return<Derived1, Derived2>::type res; - res.resize(lhs.rows()+rhs.rows(), lhs.cols()); - res << lhs, rhs; - return res; - }; - + typedef Eigen::Matrix<Scalar, + RowsAtCompileTime, + ColsAtCompileTime, + Options, + MaxRowsAtCompileTime, + MaxColsAtCompileTime> + type; +}; + +template <typename Derived1, typename Derived2> +typename vstack_return<Derived1, Derived2>::type VStack( + const Eigen::MatrixBase<Derived1>& lhs, + const Eigen::MatrixBase<Derived2>& rhs) { + typename vstack_return<Derived1, Derived2>::type res; + res.resize(lhs.rows() + rhs.rows(), lhs.cols()); + res << lhs, rhs; + return res; +}; #else // _WIN32 - // Since it is not possible to typedef privately here, use a macro. - // Always take dynamic columns if either side is dynamic. - #define COLS \ - ((ColsLeft == Eigen::Dynamic || ColsRight == Eigen::Dynamic) \ - ? Eigen::Dynamic : (ColsLeft + ColsRight)) - - // Same as above, except that prefer fixed size if either is fixed. - #define ROWS \ - ((RowsLeft == Eigen::Dynamic && RowsRight == Eigen::Dynamic) \ - ? Eigen::Dynamic \ - : ((RowsLeft == Eigen::Dynamic) \ - ? RowsRight \ - : RowsLeft \ - ) \ - ) - - // TODO(keir): Add a static assert if both rows are at compiletime. - template<typename T, int RowsLeft, int RowsRight, int ColsLeft, int ColsRight> - Eigen::Matrix<T, ROWS, COLS> - HStack(const Eigen::Matrix<T, RowsLeft, ColsLeft> &left, - const Eigen::Matrix<T, RowsRight, ColsRight> &right) { - assert(left.rows() == right.rows()); - int n = left.rows(); - int m1 = left.cols(); - int m2 = right.cols(); - - Eigen::Matrix<T, ROWS, COLS> stacked(n, m1 + m2); - stacked.block(0, 0, n, m1) = left; - stacked.block(0, m1, n, m2) = right; - return stacked; - } - - // Reuse the above macros by swapping the order of Rows and Cols. Nasty, but - // the duplication is worse. - // TODO(keir): Add a static assert if both rows are at compiletime. - // TODO(keir): Mail eigen list about making this work for general expressions - // rather than only matrix types. - template<typename T, int RowsLeft, int RowsRight, int ColsLeft, int ColsRight> - Eigen::Matrix<T, COLS, ROWS> - VStack(const Eigen::Matrix<T, ColsLeft, RowsLeft> &top, - const Eigen::Matrix<T, ColsRight, RowsRight> &bottom) { - assert(top.cols() == bottom.cols()); - int n1 = top.rows(); - int n2 = bottom.rows(); - int m = top.cols(); - - Eigen::Matrix<T, COLS, ROWS> stacked(n1 + n2, m); - stacked.block(0, 0, n1, m) = top; - stacked.block(n1, 0, n2, m) = bottom; - return stacked; - } - #undef COLS - #undef ROWS -#endif // _WIN32 +// Since it is not possible to typedef privately here, use a macro. +// Always take dynamic columns if either side is dynamic. +# define COLS \ + ((ColsLeft == Eigen::Dynamic || ColsRight == Eigen::Dynamic) \ + ? Eigen::Dynamic \ + : (ColsLeft + ColsRight)) + +// Same as above, except that prefer fixed size if either is fixed. +# define ROWS \ + ((RowsLeft == Eigen::Dynamic && RowsRight == Eigen::Dynamic) \ + ? Eigen::Dynamic \ + : ((RowsLeft == Eigen::Dynamic) ? RowsRight : RowsLeft)) + +// TODO(keir): Add a static assert if both rows are at compiletime. +template <typename T, int RowsLeft, int RowsRight, int ColsLeft, int ColsRight> +Eigen::Matrix<T, ROWS, COLS> HStack( + const Eigen::Matrix<T, RowsLeft, ColsLeft>& left, + const Eigen::Matrix<T, RowsRight, ColsRight>& right) { + assert(left.rows() == right.rows()); + int n = left.rows(); + int m1 = left.cols(); + int m2 = right.cols(); + + Eigen::Matrix<T, ROWS, COLS> stacked(n, m1 + m2); + stacked.block(0, 0, n, m1) = left; + stacked.block(0, m1, n, m2) = right; + return stacked; +} +// Reuse the above macros by swapping the order of Rows and Cols. Nasty, but +// the duplication is worse. +// TODO(keir): Add a static assert if both rows are at compiletime. +// TODO(keir): Mail eigen list about making this work for general expressions +// rather than only matrix types. +template <typename T, int RowsLeft, int RowsRight, int ColsLeft, int ColsRight> +Eigen::Matrix<T, COLS, ROWS> VStack( + const Eigen::Matrix<T, ColsLeft, RowsLeft>& top, + const Eigen::Matrix<T, ColsRight, RowsRight>& bottom) { + assert(top.cols() == bottom.cols()); + int n1 = top.rows(); + int n2 = bottom.rows(); + int m = top.cols(); + Eigen::Matrix<T, COLS, ROWS> stacked(n1 + n2, m); + stacked.block(0, 0, n1, m) = top; + stacked.block(n1, 0, n2, m) = bottom; + return stacked; +} +# undef COLS +# undef ROWS +#endif // _WIN32 -void HorizontalStack(const Mat &left, const Mat &right, Mat *stacked); +void HorizontalStack(const Mat& left, const Mat& right, Mat* stacked); -template<typename TTop, typename TBot, typename TStacked> -void VerticalStack(const TTop &top, const TBot &bottom, TStacked *stacked) { +template <typename TTop, typename TBot, typename TStacked> +void VerticalStack(const TTop& top, const TBot& bottom, TStacked* stacked) { assert(top.cols() == bottom.cols()); int n1 = top.rows(); int n2 = bottom.rows(); int m = top.cols(); stacked->resize(n1 + n2, m); - stacked->block(0, 0, n1, m) = top; + stacked->block(0, 0, n1, m) = top; stacked->block(n1, 0, n2, m) = bottom; } -void MatrixColumn(const Mat &A, int i, Vec2 *v); -void MatrixColumn(const Mat &A, int i, Vec3 *v); -void MatrixColumn(const Mat &A, int i, Vec4 *v); +void MatrixColumn(const Mat& A, int i, Vec2* v); +void MatrixColumn(const Mat& A, int i, Vec3* v); +void MatrixColumn(const Mat& A, int i, Vec4* v); template <typename TMat, typename TCols> -TMat ExtractColumns(const TMat &A, const TCols &columns) { +TMat ExtractColumns(const TMat& A, const TCols& columns) { TMat compressed(A.rows(), columns.size()); for (int i = 0; i < columns.size(); ++i) { compressed.col(i) = A.col(columns[i]); @@ -445,12 +438,12 @@ TMat ExtractColumns(const TMat &A, const TCols &columns) { } template <typename TMat, typename TDest> -void reshape(const TMat &a, int rows, int cols, TDest *b) { - assert(a.rows()*a.cols() == rows*cols); +void reshape(const TMat& a, int rows, int cols, TDest* b) { + assert(a.rows() * a.cols() == rows * cols); b->resize(rows, cols); for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { - (*b)(i, j) = a[cols*i + j]; + (*b)(i, j) = a[cols * i + j]; } } } @@ -467,24 +460,21 @@ inline bool isnan(double i) { /// and negative values template <typename FloatType> FloatType ceil0(const FloatType& value) { - FloatType result = std::ceil(std::fabs(value)); - return (value < 0.0) ? -result : result; + FloatType result = std::ceil(std::fabs(value)); + return (value < 0.0) ? -result : result; } /// Returns the skew anti-symmetric matrix of a vector -inline Mat3 SkewMat(const Vec3 &x) { +inline Mat3 SkewMat(const Vec3& x) { Mat3 skew; - skew << 0 , -x(2), x(1), - x(2), 0 , -x(0), - -x(1), x(0), 0; + skew << 0, -x(2), x(1), x(2), 0, -x(0), -x(1), x(0), 0; return skew; } /// Returns the skew anti-symmetric matrix of a vector with only /// the first two (independent) lines -inline Mat23 SkewMatMinimal(const Vec2 &x) { +inline Mat23 SkewMatMinimal(const Vec2& x) { Mat23 skew; - skew << 0, -1, x(1), - 1, 0, -x(0); + skew << 0, -1, x(1), 1, 0, -x(0); return skew; } @@ -496,7 +486,8 @@ inline Mat3 RotationFromEulerVector(Vec3 euler_vector) { } Vec3 w = euler_vector / theta; Mat3 w_hat = CrossProductMatrix(w); - return Mat3::Identity() + w_hat*sin(theta) + w_hat*w_hat*(1 - cos(theta)); + return Mat3::Identity() + w_hat * sin(theta) + + w_hat * w_hat * (1 - cos(theta)); } } // namespace libmv |