diff --git a/test/gtest_matrix_composite_OLD.cpp b/test/gtest_matrix_composite_OLD.cpp
deleted file mode 100644
index ef0fcfce1f333c7e6effe713cc35b17ecd2c08a7..0000000000000000000000000000000000000000
--- a/test/gtest_matrix_composite_OLD.cpp
+++ /dev/null
@@ -1,574 +0,0 @@
-// 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();
-}