Use new Vector4 class

This commit is contained in:
Matthias Grob
2023-03-21 15:01:26 +01:00
parent 08d6465ce5
commit b0189d95af
12 changed files with 32 additions and 56 deletions
+3 -3
View File
@@ -50,7 +50,7 @@ class AxisAngle;
* described by this class.
*/
template<typename Type>
class Quaternion : public Vector<Type, 4>
class Quaternion : public Vector4<Type>
{
public:
using Matrix41 = Matrix<Type, 4, 1>;
@@ -62,7 +62,7 @@ public:
* @param data_ array
*/
explicit Quaternion(const Type data_[4]) :
Vector<Type, 4>(data_)
Vector4<Type>(data_)
{
}
@@ -84,7 +84,7 @@ public:
* @param other Matrix41 to copy
*/
Quaternion(const Matrix41 &other) :
Vector<Type, 4>(other)
Vector4<Type>(other)
{
}
+5 -7
View File
@@ -260,7 +260,7 @@ TEST(MatrixAttitudeTest, Attitude)
// quaterion copy ctors
float data_v4[] = {1, 2, 3, 4};
Vector<float, 4> v4(data_v4);
Vector4f v4(data_v4);
Quatf q_from_v(v4);
EXPECT_EQ(q_from_v, v4);
@@ -270,15 +270,13 @@ TEST(MatrixAttitudeTest, Attitude)
// quaternion derivative in frame 1
Quatf q1(0, 1, 0, 0);
Vector<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
Vector<float, 4> q1_dot1_check(data_q_dot1_check);
Vector4f q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
Vector4f q1_dot1_check(-0.5f, 0.0f, -1.5f, 1.0f);
EXPECT_EQ(q1_dot1, q1_dot1_check);
// quaternion derivative in frame 2
Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
Vector<float, 4> q1_dot2_check(data_q_dot2_check);
Vector4f q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
Vector4f q1_dot2_check(-0.5f, 0.0f, 1.5f, -1.0f);
EXPECT_EQ(q1_dot2, q1_dot2_check);
// quaternion product
+6 -21
View File
@@ -50,19 +50,12 @@ TEST(MatrixLeastSquaresTest, 4x3)
-1.f, -1.1f, -1.2f
};
Matrix<float, 4, 3> A(data);
float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
Vector<float, 4> b(b_data);
float x_check_data[3] = {-0.69168233f,
-0.26227593f,
-1.03767522f
};
Vector<float, 3> x_check(x_check_data);
Vector4f b(2.0f, 3.0f, 4.0f, 5.0f);
Vector3f x_check(-0.69168233f, -0.26227593f, -1.03767522f);
LeastSquaresSolver<float, 4, 3> qrd = LeastSquaresSolver<float, 4, 3>(A);
Vector<float, 3> x = qrd.solve(b);
Vector3f x = qrd.solve(b);
EXPECT_EQ(x, x_check);
}
@@ -75,20 +68,12 @@ TEST(MatrixLeastSquaresTest, 4x4)
-1.f, -1.1f, -1.2f, -1.3f
};
Matrix<float, 4, 4> A(data);
float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
Vector<float, 4> b(b_data);
float x_check_data[4] = { 0.97893433f,
-2.80798701f,
-0.03175765f,
-2.19387649f
};
Vector<float, 4> x_check(x_check_data);
Vector4f b(2.0f, 3.0f, 4.0f, 5.0f);
Vector4f x_check(0.97893433f, -2.80798701f, -0.03175765f, -2.19387649f);
LeastSquaresSolver<float, 4, 4> qrd = LeastSquaresSolver<float, 4, 4>(A);
Vector<float, 4> x = qrd.solve(b);
Vector4f x = qrd.solve(b);
EXPECT_EQ(x, x_check);
}
@@ -92,11 +92,11 @@ TEST(MatrixSparseVectorTest, setZero)
TEST(MatrixSparseVectorTest, additionWithDenseVector)
{
Vector<float, 4> dense_vec;
Vector4f dense_vec;
dense_vec.setAll(1.f);
const float data[3] = {1.f, 2.f, 3.f};
const SparseVectorf<4, 1, 2, 3> sparse_vec(data);
const Vector<float, 4> res = sparse_vec + dense_vec;
const Vector4f res = sparse_vec + dense_vec;
EXPECT_FLOAT_EQ(res(0), 1.f);
EXPECT_FLOAT_EQ(res(1), 2.f);
EXPECT_FLOAT_EQ(res(2), 3.f);
@@ -115,7 +115,7 @@ TEST(MatrixSparseVectorTest, addScalar)
TEST(MatrixSparseVectorTest, dotProductWithDenseVector)
{
Vector<float, 4> dense_vec;
Vector4f dense_vec;
dense_vec.setAll(3.f);
const float data[3] = {1.f, 2.f, 3.f};
const SparseVectorf<4, 1, 2, 3> sparse_vec(data);
+1 -7
View File
@@ -71,13 +71,7 @@ TEST(MatrixVector3Test, Vector3)
Vector3f h;
EXPECT_EQ(h, Vector3f(0, 0, 0));
Vector<float, 4> j;
j(0) = 1;
j(1) = 2;
j(2) = 3;
j(3) = 4;
Vector4f j(1.f, 2.f, 3.f, 4.f);
Vector3f k = j.slice<3, 1>(0, 0);
Vector3f k_test(1, 2, 3);
EXPECT_EQ(k, k_test);
@@ -61,9 +61,8 @@ TEST_F(ArxRlsTest, test211)
_rls.update(1, 2);
_rls.update(3, 4);
_rls.update(5, 6);
const Vector<float, 4> coefficients = _rls.getCoefficients();
float data_check[] = {-1.79f, 0.97f, 0.42f, -0.48f}; // generated from Python script
const Vector<float, 4> coefficients_check(data_check);
const Vector4f coefficients = _rls.getCoefficients();
const Vector4f coefficients_check(-1.79f, 0.97f, 0.42f, -0.48f); // generated from Python script
float eps = 1e-2;
EXPECT_TRUE((coefficients - coefficients_check).abs().max() < eps);
}