You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
560 lines
19 KiB
560 lines
19 KiB
// This file is part of Eigen, a lightweight C++ template library
|
|||||||||||||
// for linear algebra.
|
|||||||||||||
//
|
|||||||||||||
// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
|
|||||||||||||
//
|
|||||||||||||
// 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/Geometry>
|
|||||||||||||
#include <Eigen/LU>
|
|||||||||||||
#include <Eigen/SVD>
|
|||||||||||||
|
|||||||||||||
template<typename T>
|
|||||||||||||
Matrix<T,2,1> angleToVec(T a)
|
|||||||||||||
{
|
|||||||||||||
return Matrix<T,2,1>(std::cos(a), std::sin(a));
|
|||||||||||||
}
|
|||||||||||||
|
|||||||||||||
template<typename Scalar, int Mode, int Options> void non_projective_only()
|
|||||||||||||
{
|
|||||||||||||
/* this test covers the following files:
|
|||||||||||||
Cross.h Quaternion.h, Transform.cpp
|
|||||||||||||
*/
|
|||||||||||||
typedef Matrix<Scalar,3,1> Vector3;
|
|||||||||||||
typedef Quaternion<Scalar> Quaternionx;
|
|||||||||||||
typedef AngleAxis<Scalar> AngleAxisx;
|
|||||||||||||
typedef Transform<Scalar,3,Mode,Options> Transform3;
|
|||||||||||||
typedef DiagonalMatrix<Scalar,3> AlignedScaling3;
|
|||||||||||||
typedef Translation<Scalar,3> Translation3;
|
|||||||||||||
|
|||||||||||||
Vector3 v0 = Vector3::Random(),
|
|||||||||||||
v1 = Vector3::Random();
|
|||||||||||||
|
|||||||||||||
Transform3 t0, t1, t2;
|
|||||||||||||
|
|||||||||||||
Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
|
|||||||||||||
|
|||||||||||||
Quaternionx q1, q2;
|
|||||||||||||
|
|||||||||||||
q1 = AngleAxisx(a, v0.normalized());
|
|||||||||||||
|
|||||||||||||
t0 = Transform3::Identity();
|
|||||||||||||
VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());
|
|||||||||||||
|
|||||||||||||
t0.linear() = q1.toRotationMatrix();
|
|||||||||||||
|
|||||||||||||
v0 << 50, 2, 1;
|
|||||||||||||
t0.scale(v0);
|
|||||||||||||
|
|||||||||||||
VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).template head<3>().norm(), v0.x());
|
|||||||||||||
|
|||||||||||||
t0.setIdentity();
|
|||||||||||||
t1.setIdentity();
|
|||||||||||||
v1 << 1, 2, 3;
|
|||||||||||||
t0.linear() = q1.toRotationMatrix();
|
|||||||||||||
t0.pretranslate(v0);
|
|||||||||||||
t0.scale(v1);
|
|||||||||||||
t1.linear() = q1.conjugate().toRotationMatrix();
|
|||||||||||||
t1.prescale(v1.cwiseInverse());
|
|||||||||||||
t1.translate(-v0);
|
|||||||||||||
|
|||||||||||||
VERIFY((t0 * t1).matrix().isIdentity(test_precision<Scalar>()));
|
|||||||||||||
|
|||||||||||||
t1.fromPositionOrientationScale(v0, q1, v1);
|
|||||||||||||
VERIFY_IS_APPROX(t1.matrix(), t0.matrix());
|
|||||||||||||
VERIFY_IS_APPROX(t1*v1, t0*v1);
|
|||||||||||||
|
|||||||||||||
// translation * vector
|
|||||||||||||
t0.setIdentity();
|
|||||||||||||
t0.translate(v0);
|
|||||||||||||
VERIFY_IS_APPROX((t0 * v1).template head<3>(), Translation3(v0) * v1);
|
|||||||||||||
|
|||||||||||||
// AlignedScaling * vector
|
|||||||||||||
t0.setIdentity();
|
|||||||||||||
t0.scale(v0);
|
|||||||||||||
VERIFY_IS_APPROX((t0 * v1).template head<3>(), AlignedScaling3(v0) * v1);
|
|||||||||||||
}
|
|||||||||||||
|
|||||||||||||
template<typename Scalar, int Mode, int Options> void transformations()
|
|||||||||||||
{
|
|||||||||||||
/* this test covers the following files:
|
|||||||||||||
Cross.h Quaternion.h, Transform.cpp
|
|||||||||||||
*/
|
|||||||||||||
using std::cos;
|
|||||||||||||
using std::abs;
|
|||||||||||||
typedef Matrix<Scalar,3,3> Matrix3;
|
|||||||||||||
typedef Matrix<Scalar,4,4> Matrix4;
|
|||||||||||||
typedef Matrix<Scalar,2,1> Vector2;
|
|||||||||||||
typedef Matrix<Scalar,3,1> Vector3;
|
|||||||||||||
typedef Matrix<Scalar,4,1> Vector4;
|
|||||||||||||
typedef Quaternion<Scalar> Quaternionx;
|
|||||||||||||
typedef AngleAxis<Scalar> AngleAxisx;
|
|||||||||||||
typedef Transform<Scalar,2,Mode,Options> Transform2;
|
|||||||||||||
typedef Transform<Scalar,3,Mode,Options> Transform3;
|
|||||||||||||
typedef typename Transform3::MatrixType MatrixType;
|
|||||||||||||
typedef DiagonalMatrix<Scalar,3> AlignedScaling3;
|
|||||||||||||
typedef Translation<Scalar,2> Translation2;
|
|||||||||||||
typedef Translation<Scalar,3> Translation3;
|
|||||||||||||
|
|||||||||||||
Vector3 v0 = Vector3::Random(),
|
|||||||||||||
v1 = Vector3::Random();
|
|||||||||||||
Matrix3 matrot1, m;
|
|||||||||||||
|
|||||||||||||
Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
|
|||||||||||||
Scalar s0 = internal::random<Scalar>(), s1 = internal::random<Scalar>();
|
|||||||||||||
|
|||||||||||||
while(v0.norm() < test_precision<Scalar>()) v0 = Vector3::Random();
|
|||||||||||||
while(v1.norm() < test_precision<Scalar>()) v1 = Vector3::Random();
|
|||||||||||||
|
|||||||||||||
VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0);
|
|||||||||||||
VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(EIGEN_PI), v0.unitOrthogonal()) * v0);
|
|||||||||||||
if(abs(cos(a)) > test_precision<Scalar>())
|
|||||||||||||
{
|
|||||||||||||
VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0));
|
|||||||||||||
}
|
|||||||||||||
m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint();
|
|||||||||||||
VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized()));
|
|||||||||||||
VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m);
|
|||||||||||||
|
|||||||||||||
Quaternionx q1, q2;
|
|||||||||||||
q1 = AngleAxisx(a, v0.normalized());
|
|||||||||||||
q2 = AngleAxisx(a, v1.normalized());
|
|||||||||||||
|
|||||||||||||
// rotation matrix conversion
|
|||||||||||||
matrot1 = AngleAxisx(Scalar(0.1), Vector3::UnitX())
|
|||||||||||||
* AngleAxisx(Scalar(0.2), Vector3::UnitY())
|
|||||||||||||
* AngleAxisx(Scalar(0.3), Vector3::UnitZ());
|
|||||||||||||
VERIFY_IS_APPROX(matrot1 * v1,
|
|||||||||||||
AngleAxisx(Scalar(0.1), Vector3(1,0,0)).toRotationMatrix()
|
|||||||||||||
* (AngleAxisx(Scalar(0.2), Vector3(0,1,0)).toRotationMatrix()
|
|||||||||||||
* (AngleAxisx(Scalar(0.3), Vector3(0,0,1)).toRotationMatrix() * v1)));
|
|||||||||||||
|
|||||||||||||
// angle-axis conversion
|
|||||||||||||
AngleAxisx aa = AngleAxisx(q1);
|
|||||||||||||
VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
|
|||||||||||||
|
|||||||||||||
// The following test is stable only if 2*angle != angle and v1 is not colinear with axis
|
|||||||||||||
if( (abs(aa.angle()) > test_precision<Scalar>()) && (abs(aa.axis().dot(v1.normalized()))<(Scalar(1)-Scalar(4)*test_precision<Scalar>())) )
|
|||||||||||||
{
|
|||||||||||||
VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) );
|
|||||||||||||
}
|
|||||||||||||
|
|||||||||||||
aa.fromRotationMatrix(aa.toRotationMatrix());
|
|||||||||||||
VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
|
|||||||||||||
// The following test is stable only if 2*angle != angle and v1 is not colinear with axis
|
|||||||||||||
if( (abs(aa.angle()) > test_precision<Scalar>()) && (abs(aa.axis().dot(v1.normalized()))<(Scalar(1)-Scalar(4)*test_precision<Scalar>())) )
|
|||||||||||||
{
|
|||||||||||||
VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) );
|
|||||||||||||
}
|
|||||||||||||
|
|||||||||||||
// AngleAxis
|
|||||||||||||
VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(),
|
|||||||||||||
Quaternionx(AngleAxisx(a,v1.normalized())).toRotationMatrix());
|
|||||||||||||
|
|||||||||||||
AngleAxisx aa1;
|
|||||||||||||
m = q1.toRotationMatrix();
|
|||||||||||||
aa1 = m;
|
|||||||||||||
VERIFY_IS_APPROX(AngleAxisx(m).toRotationMatrix(),
|
|||||||||||||
Quaternionx(m).toRotationMatrix());
|
|||||||||||||
|
|||||||||||||
// Transform
|
|||||||||||||
// TODO complete the tests !
|
|||||||||||||
a = 0;
|
|||||||||||||
while (abs(a)<Scalar(0.1))
|
|||||||||||||
a = internal::random<Scalar>(-Scalar(0.4)*Scalar(EIGEN_PI), Scalar(0.4)*Scalar(EIGEN_PI));
|
|||||||||||||
q1 = AngleAxisx(a, v0.normalized());
|
|||||||||||||
Transform3 t0, t1, t2;
|
|||||||||||||
|
|||||||||||||
// first test setIdentity() and Identity()
|
|||||||||||||
t0.setIdentity();
|
|||||||||||||
VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());
|
|||||||||||||
t0.matrix().setZero();
|
|||||||||||||
t0 = Transform3::Identity();
|
|||||||||||||
VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());
|
|||||||||||||
|
|||||||||||||
t0.setIdentity();
|
|||||||||||||
t1.setIdentity();
|
|||||||||||||
v1 << 1, 2, 3;
|
|||||||||||||
t0.linear() = q1.toRotationMatrix();
|
|||||||||||||
t0.pretranslate(v0);
|
|||||||||||||
t0.scale(v1);
|
|||||||||||||
t1.linear() = q1.conjugate().toRotationMatrix();
|
|||||||||||||
t1.prescale(v1.cwiseInverse());
|
|||||||||||||
t1.translate(-v0);
|
|||||||||||||
|
|||||||||||||
VERIFY((t0 * t1).matrix().isIdentity(test_precision<Scalar>()));
|
|||||||||||||
|
|||||||||||||
t1.fromPositionOrientationScale(v0, q1, v1);
|
|||||||||||||
VERIFY_IS_APPROX(t1.matrix(), t0.matrix());
|
|||||||||||||
|
|||||||||||||
t0.setIdentity(); t0.scale(v0).rotate(q1.toRotationMatrix());
|
|||||||||||||
t1.setIdentity(); t1.scale(v0).rotate(q1);
|
|||||||||||||
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
|||||||||||||
|
|||||||||||||
t0.setIdentity(); t0.scale(v0).rotate(AngleAxisx(q1));
|
|||||||||||||
VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
|||||||||||||
|
|||||||||||||
VERIFY_IS_APPROX(t0.scale(a).matrix(), t1.scale(Vector3::Constant(a)).matrix());
|
|||||||||||||
VERIFY_IS_APPROX(t0.prescale(a).matrix(), t1.prescale(Vector3::Constant(a)).matrix());
|
|||||||||||||
|
|||||||||||||
// More transform constructors, operator=, operator*=
|
|||||||||||||
|
|||||||||||||
Matrix3 mat3 = Matrix3::Random();
|
|||||||||||||
Matrix4 mat4;
|
|||||||||||||
mat4 << mat3 , Vector3::Zero() , Vector4::Zero().transpose();
|
|||||||||||||
Transform3 tmat3(mat3), tmat4(mat4);
|
|||||||||||||
if(Mode!=int(AffineCompact))
|
|||||||||||||
tmat4.matrix()(3,3) = Scalar(1);
|
|||||||||||||
VERIFY_IS_APPROX(tmat3.matrix(), tmat4.matrix());
|
|||||||||||||
|
|||||||||||||
Scalar a3 = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
|
|||||||||||||
Vector3 v3 = Vector3::Random().normalized();
|
|||||||||||||
AngleAxisx aa3(a3, v3);
|
|||||||||||||
Transform3 t3(aa3);
|
|||||||||||||
Transform3 t4;
|
|||||||||||||
t4 = aa3;
|
|||||||||||||
VERIFY_IS_APPROX(t3.matrix(), t4.matrix());
|
|||||||||||||
t4.rotate(AngleAxisx(-a3,v3));
|
|||||||||||||
VERIFY_IS_APPROX(t4. |