matrix_ops2_unittest.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
// Note: Matrices unit tests have been split in different files since
// building them with eigen3 eats a lot of RAM and may be a problem while
// compiling in small systems.
 
#include <mrpt/math/CMatrixFixedNumeric.h>
#include <mrpt/math/CArrayNumeric.h>
#include <mrpt/math/ops_matrices.h>
//#include <mrpt/math/utils.h>
#include <mrpt/math/geometry.h>
#include <mrpt/random.h>
#include <gtest/gtest.h>
 
using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace std;
 
#define CHECK_AND_RET_ERROR(_COND_, _MSG_) EXPECT_FALSE(_COND_) << _MSG_;
 
TEST(Matrices, inv_4x4_fix)
{
        const double dat_A[] = {
                -0.710681653571291, 0.734469323344333,  -0.656414638791893,
                0.818771495864303,  1.044946492154568,  1.163592359608108,
                -1.069421407670914, 0.307916381104872,  0.185595851677470,
                0.116899590868673,  0.507691343481809,  -3.217842384231890,
                -0.214383515646621, -0.161495561253269, 1.303923696836841,
                0.261535721431038};
        CMatrixDouble44 A(dat_A);
        CMatrixDouble44 C = A.inv();
        const double dat_AInv[] = {
                -0.741952742824035, 0.493481687552705, -0.134764164880760,
                0.083693424291000,  0.638324207063440, 0.519344439204238,
                0.264483337145361,  0.644307267615193, -0.037800456163779,
                0.131794126194075,  0.070338431705792, 0.828591793299072,
                -0.025568212209135, 0.068123300450057, -0.297834184749986,
                0.158964059763645};
        CMatrixDouble44 AInv(dat_AInv);
        CHECK_AND_RET_ERROR(
                (AInv - C).array().abs().sum() > 1e-4, "Error in inv, 4x4 fix")
}
 
TEST(Matrices, inv_6x6_fix)
{
        const double dat_A[] = {
                363.769989013671875, 0.000000000000000,   316.429992675781250,
                0.000000000000000,   87.266998291015625,  0.000000000000000,
                101.540000915527344, 0.000000000000000,   478.709991455078125,
                0.000000000000000,   504.540008544921875, 0.000000000000000,
                1.000000000000000,   0.000000000000000,   1.000000000000000,
                0.000000000000000,   1.000000000000000,   0.000000000000000,
                0.000000000000000,   363.769989013671875, 0.000000000000000,
                316.429992675781250, 0.000000000000000,   87.266998291015625,
                0.000000000000000,   101.540000915527344, 0.000000000000000,
                478.709991455078125, 0.000000000000000,   504.540008544921875,
                0.000000000000000,   1.000000000000000,   0.000000000000000,
                1.000000000000000,   0.000000000000000,   1.000000000000000};
        CMatrixDouble66 A(dat_A);
        CMatrixDouble66 C;
        A.inv(C);
        const double dat_AInv[] = {
                -0.000303131460181, -0.002689371550382, 1.383348917627708,
                0.000000000000000,  0.000000000000000,  0.000000000000000,
                0.000000000000000,  0.000000000000000,  0.000000000000000,
                -0.000303131460181, -0.002689371550382, 1.383348917627708,
                0.004729457992255,  0.003244936115630,  -2.049925698035195,
                0.000000000000000,  0.000000000000000,  0.000000000000000,
                0.000000000000000,  0.000000000000000,  0.000000000000000,
                0.004729457992255,  0.003244936115630,  -2.049925698035195,
                -0.004426326532074, -0.000555564565248, 1.666576780407488,
                0.000000000000000,  0.000000000000000,  0.000000000000000,
                0.000000000000000,  0.000000000000000,  0.000000000000000,
                -0.004426326532074, -0.000555564565248, 1.666576780407488};
        CMatrixDouble66 AInv(dat_AInv);
        CHECK_AND_RET_ERROR(
                std::isnan(C(0, 0)) || !std::isfinite(C(0, 0)) ||
                        (AInv - C).array().abs().sum() > 1e-4,
                "Error in inv, 6x6 fix")
}
 
TEST(Matrices, inv_6x6_dyn)
{
        const double dat_A[] = {
                363.769989013671875, 0.000000000000000,   316.429992675781250,
                0.000000000000000,   87.266998291015625,  0.000000000000000,
                101.540000915527344, 0.000000000000000,   478.709991455078125,
                0.000000000000000,   504.540008544921875, 0.000000000000000,
                1.000000000000000,   0.000000000000000,   1.000000000000000,
                0.000000000000000,   1.000000000000000,   0.000000000000000,
                0.000000000000000,   363.769989013671875, 0.000000000000000,
                316.429992675781250, 0.000000000000000,   87.266998291015625,
                0.000000000000000,   101.540000915527344, 0.000000000000000,
                478.709991455078125, 0.000000000000000,   504.540008544921875,
                0.000000000000000,   1.000000000000000,   0.000000000000000,
                1.000000000000000,   0.000000000000000,   1.000000000000000};
        CMatrixDouble A(6, 6, dat_A);
        CMatrixDouble C = A.inv();
        const double dat_AInv[] = {
                -0.000303131460181, -0.002689371550382, 1.383348917627708,
                0.000000000000000,  0.000000000000000,  0.000000000000000,
                0.000000000000000,  0.000000000000000,  0.000000000000000,
                -0.000303131460181, -0.002689371550382, 1.383348917627708,
                0.004729457992255,  0.003244936115630,  -2.049925698035195,
                0.000000000000000,  0.000000000000000,  0.000000000000000,
                0.000000000000000,  0.000000000000000,  0.000000000000000,
                0.004729457992255,  0.003244936115630,  -2.049925698035195,
                -0.004426326532074, -0.000555564565248, 1.666576780407488,
                0.000000000000000,  0.000000000000000,  0.000000000000000,
                0.000000000000000,  0.000000000000000,  0.000000000000000,
                -0.004426326532074, -0.000555564565248, 1.666576780407488};
        CMatrixDouble AInv(6, 6, dat_AInv);
        CHECK_AND_RET_ERROR(
                std::isnan(C(0, 0)) || !std::isfinite(C(0, 0)) ||
                        (AInv - C).array().abs().sum() > 1e-4,
                "Error in inv, 6x6 dyn")
}
 
TEST(Matrices, transpose)
{
        const double dat_A[] = {1, 2, 3, 4, 5, 6};
        const double dat_At[] = {1, 4, 2, 5, 3, 6};
        const CMatrixDouble A(2, 3, dat_A);
        const CMatrixDouble At(3, 2, dat_At);
 
        EXPECT_EQ(A.t(), At);
        EXPECT_EQ(~A, At);
        EXPECT_EQ(A.t().t(), A);
}
 
TEST(Matrices, multiply_A_skew3)
{
        {
                const double dat_A[] = {1, 2, 3, 4, 5, 6};
                const CMatrixDouble A(2, 3, dat_A);
                const std::vector<double> v{1.0, 2.0, 3.0};
                const CMatrixDouble S = CMatrixDouble(mrpt::math::skew_symmetric3(v));
 
                CMatrixDouble R;
                R.multiply_A_skew3(A, v);
                EXPECT_EQ(R, (A * S).eval());
        }
        {
                const double dat_A[] = {1, 2, 3, 4, 5, 6};
                const double dat_v[] = {1, 2, 3};
                const CMatrixFixedNumeric<double, 2, 3> A(dat_A);
                const CArrayDouble<3> v(dat_v);
                const CMatrixFixedNumeric<double, 3, 3> S =
                        mrpt::math::skew_symmetric3(v);
 
                CMatrixFixedNumeric<double, 2, 3> R;
                R.multiply_A_skew3(A, v);
                EXPECT_TRUE(R == A * S);
        }
}
 
TEST(Matrices, multiply_skew3_A)
{
        {
                const double dat_A[] = {1, 2, 3, 4, 5, 6};
                const CMatrixDouble A(3, 2, dat_A);
                const std::vector<double> v{1.0, 2.0, 3.0};
                const CMatrixDouble S = CMatrixDouble(mrpt::math::skew_symmetric3(v));
 
                CMatrixDouble R;
                R.multiply_skew3_A(v, A);
                EXPECT_TRUE(R == S * A);
        }
        {
                const double dat_A[] = {1, 2, 3, 4, 5, 6};
                const double dat_v[] = {1, 2, 3};
                const CMatrixFixedNumeric<double, 3, 2> A(dat_A);
                const CArrayDouble<3> v(dat_v);
                const CMatrixFixedNumeric<double, 3, 3> S =
                        mrpt::math::skew_symmetric3(v);
 
                CMatrixFixedNumeric<double, 3, 2> R;
                R.multiply_skew3_A(v, A);
                EXPECT_TRUE(R == S * A);
        }
}
 
TEST(Matrices, fromMatlabStringFormat)
{
        const char* mat1 = "[1 2 3;-3 -6 -5]";
        const double vals1[] = {1, 2, 3, -3, -6, -5};
 
        const char* mat2 =
                " [       -8.2   9.232 ; -2e+2          +6 ; 1.000  7 ] ";  // With tabs and
        // spaces...
        const double vals2[] = {-8.2, 9.232, -2e+2, +6, 1.000, 7};
 
        const char* mat3 = "[9]";
        const char* mat4 =
                "[1 2 3 4 5 6 7 9 10  ; 1 2 3 4 5 6 7 8 9 10 11]";  // An invalid matrix
        const char* mat5 = "[  ]";  // Empty
        const char* mat6 = "[ -405.200 42.232 ; 1219.600    -98.696 ]";  // M1 * M2
 
        const char* mat13 = "[9 8 7]";
        const char* mat31 = "[9; 8; 7]";
 
        CMatrixDouble M1, M2, M3, M4, M5, M6;
 
        if (!M1.fromMatlabStringFormat(mat1) ||
                (CMatrixFixedNumeric<double, 2, 3>(vals1) - M1).array().abs().sum() >
                        1e-4)
                GTEST_FAIL() << mat1;
 
        {
                CMatrixFixedNumeric<double, 2, 3> M1b;
                if (!M1b.fromMatlabStringFormat(mat1) ||
                        (CMatrixFixedNumeric<double, 2, 3>(vals1) - M1b)
                                        .array()
                                        .abs()
                                        .sum() > 1e-4)
                        GTEST_FAIL() << mat1;
        }
 
        if (!M2.fromMatlabStringFormat(mat2) || M2.cols() != 2 || M2.rows() != 3 ||
                (CMatrixFixedNumeric<double, 3, 2>(vals2) - M2).array().abs().sum() >
                        1e-4)
                GTEST_FAIL() << mat2;
 
        {
                CMatrixFixedNumeric<double, 3, 2> M2b;
                if (!M2b.fromMatlabStringFormat(mat2) ||
                        (CMatrixFixedNumeric<double, 3, 2>(vals2) - M2b)
                                        .array()
                                        .abs()
                                        .sum() > 1e-4)
                        GTEST_FAIL() << mat2;
        }
 
        if (!M3.fromMatlabStringFormat(mat3)) GTEST_FAIL() << mat3;
 
        {
                CVectorDouble m;
                if (!m.fromMatlabStringFormat(mat3) || m.size() != 1)
                        GTEST_FAIL() << "CVectorDouble:" << mat3;
        }
        {
                CArrayDouble<1> m;
                if (!m.fromMatlabStringFormat(mat3))
                        GTEST_FAIL() << "CArrayDouble<1>:" << mat3;
        }
 
        {
                CVectorDouble m;
                if (!m.fromMatlabStringFormat(mat31) || m.size() != 3)
                        GTEST_FAIL() << "CVectorDouble:" << mat31;
        }
        {
                CArrayDouble<3> m;
                if (!m.fromMatlabStringFormat(mat31))
                        GTEST_FAIL() << "CArrayDouble<3>:" << mat31;
        }
 
        {
                Eigen::Matrix<double, 1, 3> m;
                if (!m.fromMatlabStringFormat(mat13))
                        GTEST_FAIL() << "Matrix<double,1,3>:" << mat13;
        }
        {
                Eigen::Matrix<double, 1, Eigen::Dynamic> m;
                if (!m.fromMatlabStringFormat(mat13) || m.size() != 3)
                        GTEST_FAIL() << "Matrix<double,1,Dynamic>:" << mat13;
        }
 
        // This one MUST BE detected as WRONG:
        if (M4.fromMatlabStringFormat(mat4, nullptr /*dont dump errors to cerr*/))
                GTEST_FAIL() << mat4;
 
        if (!M5.fromMatlabStringFormat(mat5) || M5.rows() != 0 || M5.cols() != 0)
                GTEST_FAIL() << mat5;
 
        if (!M6.fromMatlabStringFormat(mat6)) GTEST_FAIL() << mat6;
 
        // Check correct values loaded:
        CMatrixDouble RES = M1 * M2;
 
        EXPECT_NEAR(0, (M6 - M1 * M2).array().square().sum(), 1e-3);
}