214 lines
7.0 KiB
C++
214 lines
7.0 KiB
C++
// This file is part of Eigen, a lightweight C++ template library
|
|
// for linear algebra.
|
|
//
|
|
// This Source Code Form is subject to the terms of the Mozilla
|
|
// Public License v. 2.0. If a copy of the MPL was not distributed
|
|
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
|
|
|
#include "main.h"
|
|
#include <Eigen/LU>
|
|
|
|
namespace {
|
|
template <typename Scalar>
|
|
void constructors() {
|
|
typedef Matrix<Scalar, 3, 1> Vector;
|
|
const Vector v = Vector::Random();
|
|
// l-value
|
|
const SkewSymmetricMatrix3<Scalar> s1(v);
|
|
const Vector& v1 = s1.vector();
|
|
VERIFY_IS_APPROX(v1, v);
|
|
VERIFY(s1.cols() == 3);
|
|
VERIFY(s1.rows() == 3);
|
|
|
|
// r-value
|
|
const SkewSymmetricMatrix3<Scalar> s2(std::move(v));
|
|
VERIFY_IS_APPROX(v1, s2.vector());
|
|
VERIFY_IS_APPROX(s1.toDenseMatrix(), s2.toDenseMatrix());
|
|
|
|
// from scalars
|
|
SkewSymmetricMatrix3<Scalar> s4(v1(0), v1(1), v1(2));
|
|
VERIFY_IS_APPROX(v1, s4.vector());
|
|
|
|
// constructors with four vectors do not compile
|
|
// Matrix<Scalar, 4, 1> vector4 = Matrix<Scalar, 4, 1>::Random();
|
|
// SkewSymmetricMatrix3<Scalar> s5(vector4);
|
|
}
|
|
|
|
template <typename Scalar>
|
|
void assignments() {
|
|
typedef Matrix<Scalar, 3, 1> Vector;
|
|
typedef Matrix<Scalar, 3, 3> SquareMatrix;
|
|
|
|
const Vector v = Vector::Random();
|
|
|
|
// assign to square matrix
|
|
SquareMatrix sq;
|
|
sq = v.asSkewSymmetric();
|
|
VERIFY(sq.isSkewSymmetric());
|
|
|
|
// assign to skew symmetric matrix
|
|
SkewSymmetricMatrix3<Scalar> sk;
|
|
sk = v.asSkewSymmetric();
|
|
VERIFY_IS_APPROX(v, sk.vector());
|
|
}
|
|
|
|
template <typename Scalar>
|
|
void plusMinus() {
|
|
typedef Matrix<Scalar, 3, 1> Vector;
|
|
typedef Matrix<Scalar, 3, 3> SquareMatrix;
|
|
|
|
const Vector v1 = Vector::Random();
|
|
const Vector v2 = Vector::Random();
|
|
|
|
SquareMatrix sq1;
|
|
sq1 = v1.asSkewSymmetric();
|
|
SquareMatrix sq2;
|
|
sq2 = v2.asSkewSymmetric();
|
|
|
|
SkewSymmetricMatrix3<Scalar> sk1;
|
|
sk1 = v1.asSkewSymmetric();
|
|
SkewSymmetricMatrix3<Scalar> sk2;
|
|
sk2 = v2.asSkewSymmetric();
|
|
|
|
VERIFY_IS_APPROX((sk1 + sk2).toDenseMatrix(), sq1 + sq2);
|
|
VERIFY_IS_APPROX((sk1 - sk2).toDenseMatrix(), sq1 - sq2);
|
|
|
|
SquareMatrix sq3 = v1.asSkewSymmetric();
|
|
VERIFY_IS_APPROX(sq3 = v1.asSkewSymmetric() + v2.asSkewSymmetric(), sq1 + sq2);
|
|
VERIFY_IS_APPROX(sq3 = v1.asSkewSymmetric() - v2.asSkewSymmetric(), sq1 - sq2);
|
|
VERIFY_IS_APPROX(sq3 = v1.asSkewSymmetric() - 2 * v2.asSkewSymmetric() + v1.asSkewSymmetric(), sq1 - 2 * sq2 + sq1);
|
|
|
|
VERIFY_IS_APPROX((sk1 + sk1).vector(), 2 * v1);
|
|
VERIFY((sk1 - sk1).vector().isZero());
|
|
VERIFY((sk1 - sk1).toDenseMatrix().isZero());
|
|
}
|
|
|
|
template <typename Scalar>
|
|
void multiplyScale() {
|
|
typedef Matrix<Scalar, 3, 1> Vector;
|
|
typedef Matrix<Scalar, 3, 3> SquareMatrix;
|
|
|
|
const Vector v1 = Vector::Random();
|
|
SquareMatrix sq1;
|
|
sq1 = v1.asSkewSymmetric();
|
|
SkewSymmetricMatrix3<Scalar> sk1;
|
|
sk1 = v1.asSkewSymmetric();
|
|
|
|
const Scalar s1 = internal::random<Scalar>();
|
|
VERIFY_IS_APPROX(SkewSymmetricMatrix3<Scalar>(sk1 * s1).vector(), sk1.vector() * s1);
|
|
VERIFY_IS_APPROX(SkewSymmetricMatrix3<Scalar>(s1 * sk1).vector(), s1 * sk1.vector());
|
|
VERIFY_IS_APPROX(sq1 * (sk1 * s1), (sq1 * sk1) * s1);
|
|
|
|
const Vector v2 = Vector::Random();
|
|
SquareMatrix sq2;
|
|
sq2 = v2.asSkewSymmetric();
|
|
SkewSymmetricMatrix3<Scalar> sk2;
|
|
sk2 = v2.asSkewSymmetric();
|
|
VERIFY_IS_APPROX(sk1 * sk2, sq1 * sq2);
|
|
|
|
// null space
|
|
VERIFY((sk1 * v1).isZero());
|
|
VERIFY((sk2 * v2).isZero());
|
|
}
|
|
|
|
template <typename Matrix>
|
|
void skewSymmetricMultiplication(const Matrix& m) {
|
|
typedef Eigen::Matrix<typename Matrix::Scalar, 3, 1> Vector;
|
|
const Vector v = Vector::Random();
|
|
const Matrix m1 = Matrix::Random(m.rows(), m.cols());
|
|
const SkewSymmetricMatrix3<typename Matrix::Scalar> sk = v.asSkewSymmetric();
|
|
VERIFY_IS_APPROX(m1.transpose() * (sk * m1), (m1.transpose() * sk) * m1);
|
|
VERIFY((m1.transpose() * (sk * m1)).isSkewSymmetric());
|
|
}
|
|
|
|
template <typename Scalar>
|
|
void traceAndDet() {
|
|
typedef Matrix<Scalar, 3, 1> Vector;
|
|
const Vector v = Vector::Random();
|
|
// this does not work, values larger than 1.e-08 can be seen
|
|
// VERIFY_IS_APPROX(sq.determinant(), static_cast<Scalar>(0));
|
|
VERIFY_IS_APPROX(v.asSkewSymmetric().determinant(), static_cast<Scalar>(0));
|
|
VERIFY_IS_APPROX(v.asSkewSymmetric().toDenseMatrix().trace(), static_cast<Scalar>(0));
|
|
}
|
|
|
|
template <typename Scalar>
|
|
void transpose() {
|
|
typedef Matrix<Scalar, 3, 1> Vector;
|
|
const Vector v = Vector::Random();
|
|
// By definition of a skew symmetric matrix: A^T = -A
|
|
VERIFY_IS_APPROX(v.asSkewSymmetric().toDenseMatrix().transpose(), v.asSkewSymmetric().transpose().toDenseMatrix());
|
|
VERIFY_IS_APPROX(v.asSkewSymmetric().transpose().vector(), (-v).asSkewSymmetric().vector());
|
|
}
|
|
|
|
template <typename Scalar>
|
|
void exponentialIdentity() {
|
|
typedef Matrix<Scalar, 3, 1> Vector;
|
|
const Vector v1 = Vector::Zero();
|
|
VERIFY(v1.asSkewSymmetric().exponential().isIdentity());
|
|
|
|
Vector v2 = Vector::Random();
|
|
v2.normalize();
|
|
VERIFY((2 * EIGEN_PI * v2).asSkewSymmetric().exponential().isIdentity());
|
|
|
|
Vector v3;
|
|
const auto precision = static_cast<Scalar>(1.1) * NumTraits<Scalar>::dummy_precision();
|
|
v3 << 0, 0, precision;
|
|
VERIFY(v3.asSkewSymmetric().exponential().isIdentity(precision));
|
|
}
|
|
|
|
template <typename Scalar>
|
|
void exponentialOrthogonality() {
|
|
typedef Matrix<Scalar, 3, 1> Vector;
|
|
typedef Matrix<Scalar, 3, 3> SquareMatrix;
|
|
const Vector v = Vector::Random();
|
|
SquareMatrix sq = v.asSkewSymmetric().exponential();
|
|
VERIFY(sq.isUnitary());
|
|
}
|
|
|
|
template <typename Scalar>
|
|
void exponentialRotation() {
|
|
typedef Matrix<Scalar, 3, 1> Vector;
|
|
typedef Matrix<Scalar, 3, 3> SquareMatrix;
|
|
|
|
// rotation axis is invariant
|
|
const Vector v1 = Vector::Random();
|
|
const SquareMatrix r1 = v1.asSkewSymmetric().exponential();
|
|
VERIFY_IS_APPROX(r1 * v1, v1);
|
|
|
|
// rotate around z-axis
|
|
Vector v2;
|
|
v2 << 0, 0, EIGEN_PI;
|
|
const SquareMatrix r2 = v2.asSkewSymmetric().exponential();
|
|
VERIFY_IS_APPROX(r2 * (Vector() << 1, 0, 0).finished(), (Vector() << -1, 0, 0).finished());
|
|
VERIFY_IS_APPROX(r2 * (Vector() << 0, 1, 0).finished(), (Vector() << 0, -1, 0).finished());
|
|
}
|
|
|
|
} // namespace
|
|
|
|
EIGEN_DECLARE_TEST(skew_symmetric_matrix3) {
|
|
for (int i = 0; i < g_repeat; i++) {
|
|
CALL_SUBTEST_1(constructors<float>());
|
|
CALL_SUBTEST_1(constructors<double>());
|
|
CALL_SUBTEST_1(assignments<float>());
|
|
CALL_SUBTEST_1(assignments<double>());
|
|
|
|
CALL_SUBTEST_2(plusMinus<float>());
|
|
CALL_SUBTEST_2(plusMinus<double>());
|
|
CALL_SUBTEST_2(multiplyScale<float>());
|
|
CALL_SUBTEST_2(multiplyScale<double>());
|
|
CALL_SUBTEST_2(skewSymmetricMultiplication(MatrixXf(3, internal::random<int>(1, EIGEN_TEST_MAX_SIZE))));
|
|
CALL_SUBTEST_2(skewSymmetricMultiplication(MatrixXd(3, internal::random<int>(1, EIGEN_TEST_MAX_SIZE))));
|
|
CALL_SUBTEST_2(traceAndDet<float>());
|
|
CALL_SUBTEST_2(traceAndDet<double>());
|
|
CALL_SUBTEST_2(transpose<float>());
|
|
CALL_SUBTEST_2(transpose<double>());
|
|
|
|
CALL_SUBTEST_3(exponentialIdentity<float>());
|
|
CALL_SUBTEST_3(exponentialIdentity<double>());
|
|
CALL_SUBTEST_3(exponentialOrthogonality<float>());
|
|
CALL_SUBTEST_3(exponentialOrthogonality<double>());
|
|
CALL_SUBTEST_3(exponentialRotation<float>());
|
|
CALL_SUBTEST_3(exponentialRotation<double>());
|
|
}
|
|
}
|