Skip to content
Snippets Groups Projects
Commit c4f45dac authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

removed legacy files

parent 38165440
No related branches found
No related tags found
1 merge request!448Draft: Resolve "Implementation of new nodes creation"
Hide whitespace changes
Inline Side-by-side
test/gtest_matrix_composite_OLD.cpp deleted 100644 → 0
+ 0
− 574
View file @ 38165440
// WOLF - Copyright (C) 2020,2021,2022,2023,2024,2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/composite/matrix_composite.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/utils/utils_gtest.h"
using namespace wolf;
using namespace Eigen;
using namespace std;
TEST(MatrixComposite, Constructor_empty)
{
MatrixComposite M;
ASSERT_EQ(M.size(), 0);
ASSERT_TRUE(M.check());
}
TEST(MatrixComposite, Constructor_structure)
{
MatrixComposite M("PO", "POV");
ASSERT_EQ(M.size(), 2);
ASSERT_EQ(M.at('P').size(), 3);
ASSERT_TRUE(M.check());
}
TEST(MatrixComposite, Constructor_structure_sizes)
{
MatrixComposite M("PO", {3, 4}, "POV", {3, 4, 3});
ASSERT_EQ(M.size(), 2);
ASSERT_EQ(M.at('P').size(), 3);
ASSERT_EQ(M.at('P', 'O').rows(), 3);
ASSERT_EQ(M.at('P', 'O').cols(), 4);
ASSERT_EQ(M.matrix("PO", "POV").rows(), 7);
ASSERT_EQ(M.matrix("PO", "POV").cols(), 10);
ASSERT_TRUE(M.check());
}
TEST(MatrixComposite, Constructor_eigenMatrix_structure_sizes)
{
MatrixXd m(MatrixXd::Random(7, 10));
MatrixComposite M(m, "PO", {3, 4}, "POV", {3, 4, 3});
ASSERT_EQ(M.size(), 2);
ASSERT_EQ(M.at('P').size(), 3);
ASSERT_EQ(M.at('P', 'O').rows(), 3);
ASSERT_EQ(M.at('P', 'O').cols(), 4);
ASSERT_EQ(M.matrix("PO", "POV").rows(), 7);
ASSERT_EQ(M.matrix("PO", "POV").cols(), 10);
ASSERT_MATRIX_APPROX(M.at('P', 'O'), m.block(0, 3, 3, 4), 1e-20);
ASSERT_MATRIX_APPROX(M.at('O', 'V'), m.block(3, 7, 4, 3), 1e-20);
ASSERT_TRUE(M.check());
}
TEST(MatrixComposite, Constructor_eigenMatrix_structure_sizes_mismatches)
{
MatrixXd m;
// // input m has too few rows
// m.setRandom(6,10);
// MatrixComposite M1(m, "PO", {3,4}, "POV", {3,4,3});
// // input m has too many rows
// m.setRandom(8,10);
// MatrixComposite M2(m, "PO", {3,4}, "POV", {3,4,3});
// // input m has too few cols
// m.setRandom(7,9) ;
// MatrixComposite M3(m, "PO", {3,4}, "POV", {3,4,3});
// // input m has too many cols
// m.setRandom(7,11) ;
// MatrixComposite M4(m, "PO", {3,4}, "POV", {3,4,3});
}
TEST(MatrixComposite, Zero)
{
MatrixComposite M = MatrixComposite::zero("PO", {3, 4}, "POV", {3, 4, 3});
ASSERT_MATRIX_APPROX(M.matrix("PO", "POV"), MatrixXd::Zero(7, 10), 1e-20);
ASSERT_TRUE(M.check());
}
TEST(MatrixComposite, Identity)
{
MatrixComposite M = MatrixComposite::identity("POV", {3, 4, 3});
ASSERT_MATRIX_APPROX(M.matrix("POV", "POV"), MatrixXd::Identity(10, 10), 1e-20);
ASSERT_TRUE(M.check());
}
TEST(MatrixComposite, emplace_operatorStream)
{
MatrixComposite M;
unsigned int psize, osize;
psize = 2;
osize = 3;
MatrixXd Mpp(psize, psize), Mpo(psize, osize), Mop(osize, psize), Moo(osize, osize);
Mpp.setOnes();
Mpo.setOnes();
Mpo *= 2;
Mop.setOnes();
Mop *= 3;
Moo.setOnes();
Moo *= 4;
ASSERT_TRUE(M.emplace('P', 'P', Mpp));
ASSERT_TRUE(M.emplace('P', 'O', Mpo));
ASSERT_TRUE(M.emplace('O', 'P', Mop));
ASSERT_TRUE(M.emplace('O', 'O', Moo));
cout << "M = " << M << endl;
}
// TEST(MatrixComposite, operatorBrackets)
//{
// MatrixComposite M;
//
// unsigned int psize, osize;
// psize = 2;
// osize = 3;
//
// MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
//
// Mpp.setOnes();
// Mpo.setOnes(); Mpo *= 2;
// Mop.setOnes(); Mop *= 3;
// Moo.setOnes(); Moo *= 4;
//
// M.emplace('P', 'P', Mpp);
// ASSERT_MATRIX_APPROX( M['P']['P'], Mpp, 1e-20);
//
// M.emplace('P', 'O', Mpo);
// ASSERT_MATRIX_APPROX( M['P']['O'], Mpo, 1e-20);
//
// // return default empty matrix if block not present
// MatrixXd N = M['O']['O'];
// ASSERT_EQ(N.rows(), 0);
// ASSERT_EQ(N.cols(), 0);
// }
// TEST(MatrixComposite, operatorParenthesis)
//{
// MatrixComposite M;
//
// unsigned int psize, osize;
// psize = 2;
// osize = 3;
//
// MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
//
// Mpp.setOnes();
// Mpo.setOnes(); Mpo *= 2;
// Mop.setOnes(); Mop *= 3;
// Moo.setOnes(); Moo *= 4;
//
// M.emplace('P', 'P', Mpp);
// ASSERT_MATRIX_APPROX( M('P', 'P'), Mpp, 1e-20);
//
// M.emplace('P', 'O', Mpo);
// ASSERT_MATRIX_APPROX( M('P', 'O'), Mpo, 1e-20);
//
// // return default empty matrix if block not present
// MatrixXd N = M('O', 'O');
// ASSERT_EQ(N.rows(), 0);
// ASSERT_EQ(N.cols(), 0);
// }
TEST(MatrixComposite, operatorAt)
{
MatrixComposite M;
unsigned int psize, osize;
psize = 2;
osize = 3;
MatrixXd Mpp(psize, psize), Mpo(psize, osize), Mop(osize, psize), Moo(osize, osize);
Mpp.setOnes();
Mpo.setOnes();
Mpo *= 2;
Mop.setOnes();
Mop *= 3;
Moo.setOnes();
Moo *= 4;
M.emplace('P', 'P', Mpp);
ASSERT_MATRIX_APPROX(M.at('P', 'P'), Mpp, 1e-20);
M.emplace('P', 'O', Mpo);
ASSERT_MATRIX_APPROX(M.at('P', 'O'), Mpo, 1e-20);
// error if block not present
ASSERT_DEATH(MatrixXd N = M.at('O', 'O'), "");
}
TEST(MatrixComposite, productScalar)
{
unsigned int psize, osize;
psize = 2;
osize = 3;
MatrixComposite M;
MatrixXd Mpp(psize, psize), Mpo(psize, osize), Mop(osize, psize), Moo(osize, osize);
Mpp.setOnes();
Mpo.setOnes();
Mpo *= 2;
Mop.setOnes();
Mop *= 3;
Moo.setOnes();
Moo *= 4;
M.emplace('P', 'P', Mpp);
M.emplace('P', 'O', Mpo);
M.emplace('O', 'P', Mop);
M.emplace('O', 'O', Moo);
WOLF_DEBUG("M = ", M);
// right-multiply by scalar
MatrixComposite R = M * 1.2;
ASSERT_MATRIX_APPROX(R.matrix("PO", "PO"), 1.2 * M.matrix("PO", "PO"), 1e-20);
// left-multiply by scalar
MatrixComposite L = 1.2 * M;
ASSERT_MATRIX_APPROX(L.matrix("PO", "PO"), 1.2 * M.matrix("PO", "PO"), 1e-20);
}
TEST(MatrixComposite, productVector)
{
unsigned int psize, osize;
psize = 2;
osize = 3;
VectorComposite x;
x.emplace('P', Vector2d(1, 1));
x.emplace('O', Vector3d(2, 2, 2));
cout << "x= " << x << endl;
// WOLF_DEBUG("x = " , x);
MatrixComposite M;
MatrixXd Mpp(psize, psize), Mpo(psize, osize), Mop(osize, psize), Moo(osize, osize);
Mpp.setOnes();
Mpo.setOnes();
Mpo *= 2;
Mop.setOnes();
Mop *= 3;
Moo.setOnes();
Moo *= 4;
M.emplace('P', 'P', Mpp);
M.emplace('P', 'O', Mpo);
M.emplace('O', 'P', Mop);
M.emplace('O', 'O', Moo);
WOLF_DEBUG("M = ", M);
VectorComposite y;
y = M * x;
// WOLF_DEBUG("y = M * x = " , y);
/* M * x = y
* p o
* p [1 1 2 2 2] p [1] p [14]
* [1 1 2 2 2] [1] [14]
* [3 3 4 4 4] * [2] = [30]
* o [3 3 4 4 4] o [2] o [30]
* [3 3 4 4 4] [2] [30]
*/
Vector2d yp(14, 14);
Vector3d yo(30, 30, 30);
ASSERT_MATRIX_APPROX(y.at('P'), yp, 1e-20);
ASSERT_MATRIX_APPROX(y.at('O'), yo, 1e-20);
// throw if x has extra blocks
// x.emplace('V', Vector2d(3,3));
// ASSERT_DEATH(y = M * x , ""); // M * x --> does not die if x has extra blocks wrt M
// throw if x has missing blocks
// x.erase('O');
// cout << "x = " << x << endl;
// ASSERT_DEATH(y = M * x , ""); // M * x --> exception if x has missing blocks wrt M, not caught by ASSERT_DEATH
}
TEST(MatrixComposite, product)
{
unsigned int psize, osize, vsize;
psize = 2;
osize = 1;
vsize = 2;
MatrixComposite M, N;
MatrixXd Mpp(psize, psize), Mpo(psize, osize), Mop(osize, psize), Moo(osize, osize);
Mpp.setOnes();
Mpo.setOnes();
Mpo *= 2;
Mop.setOnes();
Mop *= 3;
Moo.setOnes();
Moo *= 4;
M.emplace('P', 'P', Mpp);
M.emplace('P', 'O', Mpo);
M.emplace('O', 'P', Mop);
M.emplace('O', 'O', Moo);
WOLF_DEBUG("M = ", M);
MatrixXd Noo(osize, osize), Nov(osize, vsize), Npo(psize, osize), Npv(psize, vsize);
Noo.setOnes();
Nov.setOnes();
Nov *= 2;
Npo.setOnes();
Npo *= 3;
Npv.setOnes();
Npv *= 4;
N.emplace('O', 'O', Noo);
N.emplace('O', 'V', Nov);
N.emplace('P', 'O', Npo);
N.emplace('P', 'V', Npv);
WOLF_DEBUG("N = ", N);
MatrixComposite MN;
MN = M * N;
WOLF_DEBUG("MN = M * N = ", MN);
/* M * N = MN
* p o o v o v
* p [1 1 2] p [3 4 4] p [ 8 12 12]
* [1 1 2] * [3 4 4] = [ 8 12 12]
* o [3 3 4] o [1 2 2] o [22 32 32]
*/
MatrixXd MNpo(MatrixXd::Ones(psize, osize) * 8);
MatrixXd MNpv(MatrixXd::Ones(psize, vsize) * 12);
MatrixXd MNoo(MatrixXd::Ones(osize, osize) * 22);
MatrixXd MNov(MatrixXd::Ones(osize, vsize) * 32);
ASSERT_MATRIX_APPROX(MN.at('P', 'O'), MNpo, 1e-20);
ASSERT_MATRIX_APPROX(MN.at('P', 'V'), MNpv, 1e-20);
ASSERT_MATRIX_APPROX(MN.at('O', 'O'), MNoo, 1e-20);
ASSERT_MATRIX_APPROX(MN.at('O', 'V'), MNov, 1e-20);
ASSERT_TRUE(MN.check());
}
TEST(MatrixComposite, propagate)
{
unsigned int psize, osize, vsize, wsize;
psize = 2;
osize = 3;
vsize = 4;
wsize = 1;
MatrixComposite Q, J;
MatrixXd Qpp(psize, psize), Qpo(psize, osize), Qop(osize, psize), Qoo(osize, osize);
Qpp.setOnes();
Qpo.setOnes();
Qpo *= 2;
Qop.setOnes();
Qop *= 2;
Qoo.setOnes();
Qoo *= 3;
Q.emplace('P', 'P', Qpp);
Q.emplace('P', 'O', Qpo);
Q.emplace('O', 'P', Qop);
Q.emplace('O', 'O', Qoo);
WOLF_DEBUG("Q = ", Q);
MatrixXd Jvp(vsize, psize), Jvo(vsize, osize), Jwp(wsize, psize), Jwo(wsize, osize);
Jvp.setOnes();
Jvo.setOnes();
Jvo *= 2;
Jwp.setOnes();
Jwp *= 3;
Jwo.setOnes();
Jwo *= 4;
J.emplace('V', 'P', Jvp);
J.emplace('V', 'O', Jvo);
J.emplace('W', 'P', Jwp);
J.emplace('W', 'O', Jwo);
WOLF_DEBUG("J = ", J);
MatrixComposite JQJt;
JQJt = J.propagate(Q);
WOLF_DEBUG("JQJt = J * Q * J.tr = ", JQJt);
WOLF_DEBUG("JQJt = J * Q * J.tr = \n", JQJt.matrix("VW", "VW"));
ASSERT_MATRIX_APPROX(
JQJt.matrix("VW", "VW"), J.matrix("VW", "PO") * Q.matrix("PO", "PO") * J.matrix("VW", "PO").transpose(), 1e-8);
ASSERT_TRUE(JQJt.check());
}
TEST(MatrixComposite, sum)
{
unsigned int psize, osize;
psize = 2;
osize = 1;
MatrixComposite M, N;
MatrixXd Mpp(psize, psize), Mpo(psize, osize), Mop(osize, psize), Moo(osize, osize);
Mpp.setOnes();
Mpo.setOnes();
Mpo *= 2;
Mop.setOnes();
Mop *= 3;
Moo.setOnes();
Moo *= 4;
M.emplace('P', 'P', Mpp);
M.emplace('P', 'O', Mpo);
M.emplace('O', 'P', Mop);
M.emplace('O', 'O', Moo);
WOLF_DEBUG("M = ", M);
N.emplace('P', 'P', Mpp);
N.emplace('P', 'O', Mpo);
N.emplace('O', 'P', Mop);
N.emplace('O', 'O', Moo);
WOLF_DEBUG("N = ", N);
MatrixComposite MpN;
MpN = M + N;
WOLF_DEBUG("MpN = M + N = ", MpN);
ASSERT_MATRIX_APPROX(MpN.at('P', 'P'), 2 * Mpp, 1e-10);
ASSERT_MATRIX_APPROX(MpN.at('P', 'O'), 2 * Mpo, 1e-10);
ASSERT_MATRIX_APPROX(MpN.at('O', 'P'), 2 * Mop, 1e-10);
ASSERT_MATRIX_APPROX(MpN.at('O', 'O'), 2 * Moo, 1e-10);
}
TEST(MatrixComposite, difference)
{
unsigned int psize, osize;
psize = 2;
osize = 1;
MatrixComposite M, N;
MatrixXd Mpp(psize, psize), Mpo(psize, osize), Mop(osize, psize), Moo(osize, osize);
Mpp.setOnes();
Mpo.setOnes();
Mpo *= 2;
Mop.setOnes();
Mop *= 3;
Moo.setOnes();
Moo *= 4;
M.emplace('P', 'P', Mpp);
M.emplace('P', 'O', Mpo);
M.emplace('O', 'P', Mop);
M.emplace('O', 'O', Moo);
WOLF_DEBUG("M = ", M);
N.emplace('P', 'P', Mpp);
N.emplace('P', 'O', Mpo);
N.emplace('O', 'P', Mop);
N.emplace('O', 'O', Moo);
WOLF_DEBUG("N = ", N);
MatrixComposite MmN;
MmN = M - N;
WOLF_DEBUG("MmN = M - N = ", MmN);
ASSERT_MATRIX_APPROX(MmN.at('P', 'P'), Mpp * 0, 1e-10);
ASSERT_MATRIX_APPROX(MmN.at('P', 'O'), Mpo * 0, 1e-10);
ASSERT_MATRIX_APPROX(MmN.at('O', 'P'), Mop * 0, 1e-10);
ASSERT_MATRIX_APPROX(MmN.at('O', 'O'), Moo * 0, 1e-10);
}
TEST(MatrixComposite, unary_minus)
{
unsigned int psize, osize;
psize = 2;
osize = 1;
MatrixComposite M, N;
MatrixXd Mpp(psize, psize), Mpo(psize, osize), Mop(osize, psize), Moo(osize, osize);
Mpp.setOnes();
Mpo.setOnes();
Mpo *= 2;
Mop.setOnes();
Mop *= 3;
Moo.setOnes();
Moo *= 4;
M.emplace('P', 'P', Mpp);
M.emplace('P', 'O', Mpo);
M.emplace('O', 'P', Mop);
M.emplace('O', 'O', Moo);
WOLF_DEBUG("M = ", M);
MatrixComposite m;
m = -M;
WOLF_DEBUG("m = - M = ", m);
ASSERT_MATRIX_APPROX(m.at('P', 'P'), -M.at('P', 'P'), 1e-10);
ASSERT_MATRIX_APPROX(m.at('P', 'O'), -M.at('P', 'O'), 1e-10);
ASSERT_MATRIX_APPROX(m.at('O', 'P'), -M.at('O', 'P'), 1e-10);
ASSERT_MATRIX_APPROX(m.at('O', 'O'), -M.at('O', 'O'), 1e-10);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
// restrict to a group of tests only
// ::testing::GTEST_FLAG(filter) = "VectorComposite.*";
// restrict to this test only
// ::testing::GTEST_FLAG(filter) = "MatrixComposite.propagate";
// ::testing::GTEST_FLAG(filter) = "MatrixComposite.*";
return RUN_ALL_TESTS();
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment