Skip to content
Snippets Groups Projects

Draft: Resolve "Implementation of new nodes creation"

Open Draft: Resolve "Implementation of new nodes creation"
454-implementation-of-new-nodes-creation into devel
Open Joan Vallvé Navarro requested to merge 454-implementation-of-new-nodes-creation into devel
Viewing commit c4f45dac
Prev Next
Show latest version
1 file
+ 0
− 574
Compare changes
  • Side-by-side
  • Inline
test/gtest_matrix_composite_OLD.cpp deleted 100644 → 0
+ 0
− 574
// 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();
}