demos/solver/demo_analytic_autodiff_comparison.cpp

+// WOLF - Copyright (C) 2020-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 General Public License version 3
+// as published by the Free Software Foundation.
+//
+// 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 General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+// Testing creating wolf tree from imported .graph file
+
+// C includes for sleep, time and main args
+#include "unistd.h"
+
+// std includes
+#include <iostream>
+#include <memory>
+#include <random>
+#include <cmath>
+#include <queue>
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/feature/feature_odom_2d.h"
+#include "core/factor/factor_relative_pose_2d.h"
+#include "../test/dummy/factor_relative_pose_2d_autodiff.h"
+#include "core/problem/problem.h"
+#include "load_graph.h"
+
+std::string wolf_dir = _WOLF_CODE_DIR;
+
+int main(int argc, char** argv)
+{
+    using namespace wolf;
+
+    // Welcome message
+    std::cout << std::endl
+              << " ========= WOLF ANALITIC vs. AUTODIFF FACTORS TEST with loaded graph ===========" << std::endl
+              << std::endl;
+
+    if (argc != 3 || atoi(argv[2]) < 0)
+    {
+        std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl;
+        std::cout << "    FILE_PATH is the .g2o or .toro file path" << std::endl;
+        std::cout << "    MAX_VERTICES max edges to be loaded (0: ALL)" << std::endl;
+        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
+        return -1;
+    }
+
+    // load graph arguments
+    std::string  file_path  = argv[1];
+    unsigned int max_vertex = atoi(argv[2]);
+
+    // Create problem 2D
+    ProblemPtr problem_analitic = Problem::create(2);
+    ProblemPtr problem_autodiff = Problem::create(2);
+
+    // Ceres solver
+    SolverManagerPtr solver_analitic = FactorySolverFile::create(
+        "SolverCeres", problem_analitic, wolf_dir + "/demos/solver/yaml/solver_ceres.yaml", {wolf_dir});
+    SolverManagerPtr solver_autodiff = FactorySolverFile::create(
+        "SolverCeres", problem_autodiff, wolf_dir + "/demos/solver/yaml/solver_ceres.yaml", {wolf_dir});
+
+    // load graph from .txt
+    loadGraph<FactorRelativePose2d>(problem_analitic, file_path, max_vertex);
+    loadGraph<FactorRelativePose2dAutodiff>(problem_autodiff, file_path, max_vertex);
+
+    // UPDATE
+    WOLF_INFO("updating solver_analitic...");
+    auto t1 = clock();
+    solver_analitic->update();
+    double t_update_analitic = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO("updated in ", t_update_analitic, " s")
+    WOLF_INFO("updating solver_autodiff...");
+    t1 = clock();
+    solver_autodiff->update();
+    double t_update_autodiff = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO("updated in ", t_update_autodiff, " s")
+
+    // SOLVE
+    WOLF_INFO("solving analitic...");
+    t1                           = clock();
+    std::string report_analitic  = solver_analitic->solve(SolverManager::ReportVerbosity::FULL);
+    double      t_solve_analitic = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO(report_analitic);
+    WOLF_INFO("solved in ", t_solve_analitic, " s")
+    WOLF_INFO("solving autodiff...");
+    t1                           = clock();
+    std::string report_autodiff  = solver_autodiff->solve(SolverManager::ReportVerbosity::FULL);
+    double      t_solve_autodiff = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO(report_autodiff);
+    WOLF_INFO("solved in ", t_solve_autodiff, " s")
+
+    // COMPUTE COVARIANCES
+    WOLF_INFO("computing marginal covariances analitic...");
+    t1 = clock();
+    solver_analitic->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    double t_cov_analitic = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO("computed in ", t_cov_analitic, " s")
+    WOLF_INFO("computing marginal covariances autodiff...");
+    t1 = clock();
+    solver_autodiff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    double t_cov_autodiff = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO("computed in ", t_cov_autodiff, " s")
+
+    // End message
+    std::cout << " =========================== END ===============================" << std::endl << std::endl;
+
+    // exit
+    return 0;
+}

demos/solver/demo_wolf_imported_graph.cpp

+// WOLF - Copyright (C) 2020-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 General Public License version 3
+// as published by the Free Software Foundation.
+//
+// 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 General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+// Testing creating wolf tree from imported .graph file
+
+// C includes for sleep, time and main args
+#include "unistd.h"
+
+// std includes
+#include <iostream>
+#include <memory>
+#include <random>
+#include <cmath>
+#include <queue>
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/feature/feature_odom_2d.h"
+#include "core/factor/factor_relative_pose_2d.h"
+#include "core/problem/problem.h"
+#include "load_graph.h"
+
+std::string wolf_dir = _WOLF_CODE_DIR;
+
+int main(int argc, char** argv)
+{
+    using namespace wolf;
+
+    // Welcome message
+    std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl;
+
+    if (argc != 3 || atoi(argv[2]) < 0)
+    {
+        std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl;
+        std::cout << "    FILE_PATH is the .g2o or .toro file path" << std::endl;
+        std::cout << "    MAX_VERTICES max edges to be loaded (0: ALL)" << std::endl;
+        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
+        return -1;
+    }
+
+    // load graph arguments
+    std::string  file_path  = argv[1];
+    unsigned int max_vertex = atoi(argv[2]);
+
+    // Create problem 2D
+    ProblemPtr problem = Problem::create(2);
+
+    // Ceres solver
+    SolverManagerPtr solver = FactorySolverFile::create(
+        "SolverCeres", problem, wolf_dir + "/demos/solver/yaml/solver_ceres.yaml", {wolf_dir});
+
+    // load graph from .txt
+    loadGraph<FactorRelativePose2d>(problem, file_path, max_vertex);
+
+    // UPDATE
+    WOLF_INFO("updating solver...");
+    auto t1 = clock();
+    solver->update();
+    double t_update = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO("updated in ", t_update, " s")
+
+    // SOLVE
+    WOLF_INFO("solving...");
+    t1                  = clock();
+    std::string report  = solver->solve(SolverManager::ReportVerbosity::FULL);
+    double      t_solve = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO(report);
+    WOLF_INFO("solved in ", t_solve, " s")
+
+    // COMPUTE COVARIANCES
+    WOLF_INFO("computing marginal covariances...");
+    t1 = clock();
+    solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    double t_cov = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO("computed in ", t_cov, " s")
+
+    // End message
+    std::cout << " =========================== END ===============================" << std::endl << std::endl;
+
+    // exit
+    return 0;
+}

demos/solver/load_graph.h

+// WOLF - Copyright (C) 2020-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 General Public License version 3
+// as published by the Free Software Foundation.
+//
+// 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 General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+// Testing creating wolf tree from imported .graph file
+
+// C includes for sleep, time and main args
+#include "unistd.h"
+
+// std includes
+#include <iostream>
+#include <memory>
+#include <random>
+#include <cmath>
+#include <queue>
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/feature/feature_odom_2d.h"
+#include "core/problem/problem.h"
+
+namespace wolf
+{
+template <class FactorType>
+void loadGraph(ProblemPtr problem, std::string file_path, const int& max_vertexs)
+{
+    std::map<unsigned int, FrameBasePtr> index_2_frame_ptr;
+
+    // toro or g2o file
+    WOLF_DEBUG("file_path: ", file_path);
+    bool g2o = false;
+    if (file_path.substr(file_path.size() - 4, 4) == ".g2o")
+        g2o = true;
+    else if (file_path.substr(file_path.size() - 6, 6) == ".graph")
+        g2o = false;
+    else
+        throw std::runtime_error("loadGraph: extension not supported, should be .g2o or .toro. " + file_path);
+
+    WOLF_DEBUG_COND(g2o, "Decoding a g2o file!");
+    WOLF_DEBUG_COND(not g2o, "Decoding a TORO file!");
+
+    // load graph from .txt
+    std::ifstream offline_file;
+    bool          is_2d;
+    offline_file.open(file_path.c_str(), std::ifstream::in);
+    if (offline_file.is_open())
+    {
+        std::string buffer;
+        // Line by line
+        while (std::getline(offline_file, buffer))
+        {
+            // std::cout << "new line:" << buffer << std::endl;
+            std::string  bNum;
+            unsigned int i = 0;
+
+            // VERTEX
+            if (buffer.at(0) == 'V')
+            {
+                // skip rest of VERTEX word
+                while (buffer.at(i) != ' ') i++;
+                // 2D?
+                is_2d = (buffer.at(i - 1) == '2');
+                assert(is_2d && "loadGraph decding 3D datasets not implemented yet.");
+
+                // skip white spaces
+                while (buffer.at(i) == ' ') i++;
+
+                // vertex index
+                while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                unsigned int vertex_index = atoi(bNum.c_str());
+                bNum.clear();
+
+                if (max_vertexs == 0 or vertex_index <= max_vertexs + 1)
+                {
+                    // skip white spaces
+                    while (buffer.at(i) == ' ') i++;
+
+                    // vertex pose
+                    Eigen::Vector3d vertex_pose;
+                    // x
+                    while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                    vertex_pose(0) = atof(bNum.c_str());
+                    bNum.clear();
+                    // skip white spaces
+                    while (buffer.at(i) == ' ') i++;
+                    // y
+                    while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                    vertex_pose(1) = atof(bNum.c_str());
+                    bNum.clear();
+                    // skip white spaces
+                    while (buffer.at(i) == ' ') i++;
+                    // theta
+                    while (i < buffer.size() && buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                    vertex_pose(2) = atof(bNum.c_str());
+                    bNum.clear();
+
+                    // add frame to problem
+                    FrameBasePtr vertex_frame =
+                        problem->emplaceFrame(TimeStamp(double(vertex_index)),
+                                              VectorComposite{{'P', vertex_pose.head(2)}, {'O', vertex_pose.tail(1)}});
+
+                    // Fix first frame
+                    if (vertex_index == 1)  // FIXME: 1 or 0?
+                        vertex_frame->fix();
+
+                    // store
+                    index_2_frame_ptr[vertex_index] = vertex_frame;
+
+                    // std::cout << "Added vertex! index: " << vertex_index << " id: " << vertex_frame_ptr->id()
+                    // << std::endl << "pose: " << vertex_pose.transpose() << std::endl;
+                }
+            }
+            // EDGE
+            else if (buffer.at(0) == 'E')
+            {
+                // skip rest of EDGE word
+                while (buffer.at(i) != ' ') i++;
+                // 2D?
+                is_2d = (buffer.at(i - 1) == '2');
+                assert(is_2d && "loadGraph decding 3D datasets not implemented yet.");
+
+                // skip white spaces
+                while (buffer.at(i) == ' ') i++;
+
+                // from
+                while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                unsigned int edge_from = atoi(bNum.c_str());
+                bNum.clear();
+                // skip white spaces
+                while (buffer.at(i) == ' ') i++;
+
+                // to index
+                while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                unsigned int edge_to = atoi(bNum.c_str());
+                bNum.clear();
+
+                if (max_vertexs == 0 or (edge_to <= max_vertexs + 1 && edge_from <= max_vertexs + 1))
+                {
+                    assert(index_2_frame_ptr.find(edge_from) != index_2_frame_ptr.end() &&
+                           "edge_from vertex not added!");
+                    assert(index_2_frame_ptr.find(edge_to) != index_2_frame_ptr.end() && "edge_to vertex not added!");
+
+                    // skip white spaces
+                    while (buffer.at(i) == ' ') i++;
+
+                    // edge vector
+                    Eigen::Vector3d edge_vector;
+                    // x
+                    while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                    edge_vector(0) = atof(bNum.c_str());
+                    bNum.clear();
+                    // skip white spaces
+                    while (buffer.at(i) == ' ') i++;
+                    // y
+                    while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                    edge_vector(1) = atof(bNum.c_str());
+                    bNum.clear();
+                    // skip white spaces
+                    while (buffer.at(i) == ' ') i++;
+                    // theta
+                    while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                    edge_vector(2) = atof(bNum.c_str());
+                    bNum.clear();
+                    // skip white spaces
+                    while (buffer.at(i) == ' ') i++;
+
+                    // edge covariance
+                    Eigen::Matrix3d edge_information;
+                    // g2o:  I11 I12 I13 I22 I23 I33”
+                    if (g2o)
+                    {
+                        // xx
+                        while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                        edge_information(0, 0) = atof(bNum.c_str());
+                        bNum.clear();
+                        // skip white spaces
+                        while (buffer.at(i) == ' ') i++;
+                        // xy
+                        while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                        edge_information(0, 1) = atof(bNum.c_str());
+                        edge_information(1, 0) = atof(bNum.c_str());
+                        bNum.clear();
+                        // skip white spaces
+                        while (buffer.at(i) == ' ') i++;
+                        // xtheta
+                        while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                        edge_information(0, 2) = atof(bNum.c_str());
+                        edge_information(2, 0) = atof(bNum.c_str());
+                        bNum.clear();
+                        // skip white spaces
+                        while (buffer.at(i) == ' ') i++;
+                        // yy
+                        while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                        edge_information(1, 1) = atof(bNum.c_str());
+                        bNum.clear();
+                        // skip white spaces
+                        while (buffer.at(i) == ' ') i++;
+                        // ytheta
+                        while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                        edge_information(1, 2) = atof(bNum.c_str());
+                        edge_information(2, 1) = atof(bNum.c_str());
+                        bNum.clear();
+                        // skip white spaces
+                        while (buffer.at(i) == ' ') i++;
+                        // thetatheta
+                        while (i < buffer.size() && buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                        edge_information(2, 2) = atof(bNum.c_str());
+                        bNum.clear();
+                    }
+                    // TORO: I11 I12 I22 I33 I13 I23”
+                    else
+                    {
+                        // xx
+                        while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                        edge_information(0, 0) = atof(bNum.c_str());
+                        bNum.clear();
+                        // skip white spaces
+                        while (buffer.at(i) == ' ') i++;
+                        // xy
+                        while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                        edge_information(0, 1) = atof(bNum.c_str());
+                        edge_information(1, 0) = atof(bNum.c_str());
+                        bNum.clear();
+                        // skip white spaces
+                        while (buffer.at(i) == ' ') i++;
+                        // yy
+                        while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                        edge_information(1, 1) = atof(bNum.c_str());
+                        bNum.clear();
+                        // skip white spaces
+                        while (buffer.at(i) == ' ') i++;
+                        // thetatheta
+                        while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                        edge_information(2, 2) = atof(bNum.c_str());
+                        bNum.clear();
+                        // skip white spaces
+                        while (buffer.at(i) == ' ') i++;
+                        // xtheta
+                        while (buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                        edge_information(0, 2) = atof(bNum.c_str());
+                        edge_information(2, 0) = atof(bNum.c_str());
+                        bNum.clear();
+                        // skip white spaces
+                        while (buffer.at(i) == ' ') i++;
+                        // ytheta
+                        while (i < buffer.size() && buffer.at(i) != ' ') bNum.push_back(buffer.at(i++));
+                        edge_information(1, 2) = atof(bNum.c_str());
+                        edge_information(2, 1) = atof(bNum.c_str());
+                        bNum.clear();
+                    }
+
+                    // add capture, feature and factor to problem
+                    FrameBasePtr frame_from = index_2_frame_ptr[edge_from];
+                    FrameBasePtr frame_to   = index_2_frame_ptr[edge_to];
+                    auto         cap        = CaptureBase::emplace<CaptureOdom2d>(frame_to,  //
+                                                                   TimeStamp(edge_to),
+                                                                   nullptr,
+                                                                   edge_vector,
+                                                                   edge_information.inverse());
+                    auto feat = FeatureBase::emplace<FeatureOdom2d>(cap, edge_vector, edge_information.inverse());
+                    FactorBase::emplace<FactorType>(feat,
+                                                    feat->getMeasurement(),
+                                                    feat->getMeasurementSquareRootInformationUpper(),
+                                                    frame_from,
+                                                    frame_to,
+                                                    nullptr,
+                                                    false,
+                                                    TOP_GEOM);
+
+                    // std::cout << "Added edge! " << factor_ptr->id() << " from vertex " <<
+                    // factor_ptr->getCapture()->getFrame()->id() << " to " << factor_ptr->getFrameTo()->id()
+                    // << std::endl; std::cout << "vector " << factor_ptr->getMeasurement().transpose() <<
+                    // std::endl; std::cout << "information " << std::endl << edge_information << std::endl; std::cout
+                    // << "covariance " << std::endl << factor_ptr->getMeasurementCovariance() << std::endl;
+                }
+            }
+            else
+                assert("unknown line");
+        }
+        printf("\nGraph loaded!\n");
+    }
+    else
+        printf("\nError opening file\n");
+}
+
+}  // namespace wolf

demos/solver/test_SPQR.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of 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/>.
-//
-//--------LICENSE_END--------
-/*
- * test_SPQR.cpp
- *
- *  Created on: Jun 18, 2015
- *      Author: jvallve
- */
-
-#include <iostream>
-#include <Eigen/SPQRSupport>
-#include <Eigen/CholmodSupport>
-#include "SuiteSparseQR.hpp"
-
-using namespace Eigen;
-
-int main (int argc, char **argv)
-{
-    ///////////////////////////////////////////////////////////////////////
-    // Eigen Support SPQR
-    SPQR < SparseMatrix<double> > solver;
-    //solver.setSPQROrdering(0); // no ordering -> segmentation fault
-
-    SparseMatrix<double> matA(4,3);
-    matA.coeffRef(0,0) = 0.1;
-    matA.coeffRef(1,0) = 0.4;
-    matA.coeffRef(1,1) = 0.2;
-    matA.coeffRef(2,1) = 0.4;
-    matA.coeffRef(2,2) = 0.2;
-    matA.coeffRef(3,2) = 0.1;
-
-    std::cout << "matA: " << std::endl << matA << std::endl;
-
-    VectorXd b_ = VectorXd::Ones(4);
-    VectorXd x_(3);
-
-    std::cout << "b_: " << std::endl << b_ << std::endl;
-
-    solver.compute(matA);
-    if (solver.info() != Success)
-    {
-        std::cout << "decomposition failed" << std::endl;
-        return 0;
-    }
-    std::cout << "R: " << std::endl << solver.matrixR() << std::endl;
-    x_ = solver.solve(b_);
-    std::cout << "solved x_" << std::endl << x_ << std::endl;
-    std::cout << "ordering: " << solver.colsPermutation().indices().transpose() << std::endl;
-
-    ///////////////////////////////////////////////////////////////////////
-    // Directly in suitesparse
-    cholmod_common Common, *cc ;
-    cholmod_sparse A ;
-    cholmod_dense *X, *B, *Residual ;
-    double rnorm, one [2] = {1,0}, minusone [2] = {-1,0} ;
-    int mtype ;
-
-    // start CHOLMOD
-    cc = &Common ;
-    cholmod_l_start (cc) ;
-
-    // load A
-    A = viewAsCholmod(matA);
-    //A = (cholmod_sparse *) cholmod_l_read_matrix (stdin, 1, &mtype, cc) ;
-    std::cout << "A.xtype " << A.xtype << std::endl;
-    std::cout << "A.nrow " << A.nrow << std::endl;
-    std::cout << "A.ncol " << A.ncol << std::endl;
-
-    // B = ones (size (A,1),1)
-    B = cholmod_l_ones (A.nrow, 1, A.xtype, cc) ;
-
-    std::cout << "2" << std::endl;
-    // X = A\B
-    //X = SuiteSparseQR <double> (0, SPQR_DEFAULT_TOL, &A, B, cc) ;
-    X = SuiteSparseQR <double> (&A, B, cc);
-
-    std::cout << "3" << std::endl;
-    // rnorm = norm (B-A*X)
-    Residual = cholmod_l_copy_dense (B, cc) ;
-    std::cout << "4" << std::endl;
-    cholmod_l_sdmult (&A, 0, minusone, one, X, Residual, cc) ;
-    std::cout << "5" << std::endl;
-    rnorm = cholmod_l_norm_dense (Residual, 2, cc) ;
-    printf ("2-norm of residual: %8.1e\n", rnorm) ;
-    printf ("rank %ld\n", cc->SPQR_istat [4]) ;
-
-    // free everything and finish CHOLMOD
-    cholmod_l_free_dense (&Residual, cc) ;
-    //cholmod_l_free_sparse (A, cc) ;
-    cholmod_l_free_dense (&X, cc) ;
-    cholmod_l_free_dense (&B, cc) ;
-    cholmod_l_finish (cc) ;
-    return (0) ;
-}

demos/solver/test_ccolamd.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of 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/>.
-//
-//--------LICENSE_END--------
-/*
- * test_ccolamd.cpp
- *
- *  Created on: Jun 11, 2015
- *      Author: jvallve
- */
-
-// Wolf includes
-#include "core/common/wolf.h"
-
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// ccolamd
-#include "solver/ccolamd_ordering.h"
-
-// eigen includes
-#include <Eigen/OrderingMethods>
-#include <Eigen/CholmodSupport>
-#include <Eigen/SparseLU>
-
-using namespace Eigen;
-using namespace wolf;
-
-//main
-int main(int argc, char *argv[])
-{
-    if (argc != 2 || atoi(argv[1]) < 1)
-    {
-        std::cout << "Please call me with: [./test_ccolamd SIZE], where:" << std::endl;
-        std::cout << "     - SIZE: integer size of the problem" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-    SizeEigen size = atoi(argv[1]);
-
-    SparseMatrix<double, ColMajor, SizeEigen> A(size, size), Aordered(size, size);
-    CholmodSupernodalLLT < SparseMatrix<double, ColMajor, SizeEigen> > solver;
-    PermutationMatrix<Dynamic, Dynamic, SizeEigen> perm(size);
-    CCOLAMDOrdering<SizeEigen> ordering;
-    Matrix<SizeEigen, Dynamic, 1> ordering_factors = Matrix<SizeEigen, Dynamic, 1>::Ones(size);
-    VectorXd b(size), bordered(size), xordered(size), x(size);
-    clock_t t1, t2, t3;
-    double time1, time2, time3;
-
-    // BUILD THE PROBLEM ----------------------------
-    //Fill A & b
-    A.insert(0, 0) = 5;
-    b(0) = 1;
-    for (int i = 1; i < size; i++)
-    {
-        A.insert(i, i) = 5;
-        A.insert(i, i - 1) = 1;
-        A.insert(i - 1, i) = 1;
-        b(i) = i + 1;
-    }
-    A.insert(size - 1, 0) = 2;
-    A.insert(0, size - 1) = 2;
-
-    std::cout << "Solving Ax = b:" << std::endl << "A = " << std::endl << A << std::endl << std::endl;
-    std::cout << "b = " << std::endl << b.transpose() << std::endl << std::endl;
-
-    // SOLVING WITHOUT REORDERING ------------------------------------
-    // solve Ax = b
-    t1 = clock();
-    solver.compute(A);
-    if (solver.info() != Success)
-    {
-        std::cout << "decomposition failed" << std::endl;
-        return 0;
-    }
-    x = solver.solve(b);
-    time1 = ((double) clock() - t1) / CLOCKS_PER_SEC;
-    std::cout << "solved in " << time1 << "seconds" << std::endl;
-    std::cout << "x = " << x.transpose() << std::endl;
-
-    // SOLVING AFTER REORDERING ------------------------------------
-    // ordering factors
-    ordering_factors(size-1) = 2;
-    ordering_factors(0) = 2;
-
-    // ordering
-    t2 = clock();
-    A.makeCompressed();
-
-    std::cout << "Reordering using CCOLAMD:" << std::endl;
-    std::cout << "ordering_factors = " << std::endl << ordering_factors.transpose() << std::endl << std::endl;
-    ordering(A, perm, ordering_factors.data());
-    std::cout << "perm = " << std::endl << perm.indices().transpose() << std::endl << std::endl;
-
-    bordered = perm * b;
-    Aordered = A.twistedBy(perm);
-    std::cout << "reordered A = " << std::endl << Aordered * MatrixXd::Identity(size, size) << std::endl << std::endl;
-    std::cout << "reordered b = " << std::endl << bordered.transpose() << std::endl << std::endl;
-
-    // solve Ax = b
-    solver.compute(Aordered);
-    if (solver.info() != Success)
-    {
-        std::cout << "decomposition failed" << std::endl;
-        return 0;
-    }
-    xordered = solver.solve(bordered);
-    time2 = ((double) clock() - t2) / CLOCKS_PER_SEC;
-    std::cout << "solved in " << time2 << "seconds" << std::endl;
-    std::cout << "x = " << (perm.inverse() * xordered).transpose() << std::endl;
-    std::cout << "x = " << x.transpose() << " (solution without reordering)" << std::endl;
-
-    // SOLVING AND REORDERING ------------------------------------
-    t3 = clock();
-    SparseLU<SparseMatrix<double, ColMajor, SizeEigen>, CCOLAMDOrdering<SizeEigen> > solver2;
-    solver2.compute(A);
-    if (solver2.info() != Success)
-    {
-        std::cout << "decomposition failed" << std::endl;
-        return 0;
-    }
-    x = solver2.solve(b);
-    time3 = ((double) clock() - t3) / CLOCKS_PER_SEC;
-    std::cout << "solved in " << time3 << "seconds" << std::endl;
-    std::cout << "x = " << x.transpose() << std::endl;
-}
-

demos/solver/test_ccolamd_blocks.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of 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/>.
-//
-//--------LICENSE_END--------
-/*
- * test_ccolamd_blocks.cpp
- *
- *  Created on: Jun 12, 2015
- *      Author: jvallve
- */
-
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// eigen includes
-#include <Eigen/OrderingMethods>
-#include <Eigen/CholmodSupport>
-#include <Eigen/SparseLU>
-
-// ccolamd
-#include "solver/ccolamd_ordering.h"
-
-using namespace Eigen;
-
-void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols)
-{
-  for (unsigned int i = row; i < row + Nrows; i++)
-    for (unsigned int j = col; j < row + Ncols; j++)
-      original.coeffRef(i,j) = 0.0;
-
-  original.makeCompressed();
-}
-
-void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
-{
-  for (unsigned int r=0; r<ins.rows(); ++r)
-    for (unsigned int c = 0; c < ins.cols(); c++)
-      original.coeffRef(r + row, c + col) += ins(r,c);
-}
-
-void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm, PermutationMatrix<Dynamic, Dynamic, int> &perm_blocks, const int dim, const int size)
-{
-    ArrayXXi idx(dim, size);
-    idx.row(0) = dim * perm.indices().transpose();
-    for (int i = 1; i<dim; i++)
-        idx.row(i) = idx.row(i-1) + 1;
-    Map<ArrayXi> idx_blocks(idx.data(), dim*size, 1);
-    perm_blocks.indices() = idx_blocks;
-}
-
-//main
-int main(int argc, char *argv[])
-{
-    if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
-    {
-        std::cout << "Please call me with: [./test_ccolamd SIZE DIM], where:" << std::endl;
-        std::cout << "     - SIZE: integer size of the problem" << std::endl;
-        std::cout << "     - DIM: integer dimension of the nodes" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-    int size = atoi(argv[1]);
-    int dim = atoi(argv[2]);
-
-    SparseMatrix<double> H(size * dim, size * dim), H_ordered(size * dim, size * dim), H_block_ordered(size * dim, size * dim);
-    SparseMatrix<int> FactorMatrix(size,size);
-    CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3;
-    PermutationMatrix<Dynamic, Dynamic, int> perm(size), perm_blocks(size * dim);
-    CCOLAMDOrdering<int> ordering;
-    VectorXi block_ordering_factors = VectorXi::Ones(size);
-    VectorXi ordering_factors = VectorXi::Ones(size*dim);
-    VectorXd b(size * dim), b_ordered(size * dim), b_block_ordered(size * dim), x_block_ordered(size * dim), x_ordered(size * dim), x(size * dim);
-    clock_t t1, t2, t3;
-    double time1, time2, time3;
-
-    MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
-
-    // BUILD THE PROBLEM ----------------------------
-    //Fill H & b
-    for (int i = 0; i < size; i++)
-    {
-        addSparseBlock(5*omega, H, i*dim, i*dim);
-        FactorMatrix.insert(i,i) = 1;
-        if (i > 0)
-        {
-            addSparseBlock(omega, H, i*dim, (i-1)*dim);
-            addSparseBlock(omega, H, (i-1)*dim, i*dim);
-            FactorMatrix.insert(i,i-1) = 1;
-            FactorMatrix.insert(i-1,i) = 1;
-        }
-        b.segment(i*dim, dim) = VectorXd::Constant(dim, i+1);
-    }
-    addSparseBlock(2*omega, H, 0, (size - 1)*dim);
-    addSparseBlock(2*omega, H, (size-1)*dim, 0);
-    FactorMatrix.insert(0,size-1) = 1;
-    FactorMatrix.insert(size-1,0) = 1;
-
-    std::cout << "Solving factor graph:" << std::endl;
-    std::cout << "Factors: " << std::endl << FactorMatrix * MatrixXi::Identity(size,size) << std::endl << std::endl;
-
-    // SOLVING WITHOUT REORDERING ------------------------------------
-    // solve Hx = b
-    t1 = clock();
-    solver.compute(H);
-    if (solver.info() != Success)
-    {
-        std::cout << "decomposition failed" << std::endl;
-        return 0;
-    }
-    x = solver.solve(b);
-    time1 = ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-    // SOLVING AFTER REORDERING ------------------------------------
-    // ordering factors
-    ordering_factors.segment(dim * (size-1), dim) = VectorXi::Constant(dim,2);
-    ordering_factors.segment(0, dim) = VectorXi::Constant(dim,2);
-
-    // variable ordering
-    t2 = clock();
-    H.makeCompressed();
-
-    std::cout << "Reordering using CCOLAMD:" << std::endl;
-    std::cout << "ordering_factors = " << std::endl << ordering_factors.transpose() << std::endl << std::endl;
-    ordering(H, perm, ordering_factors.data());
-
-    b_ordered = perm * b;
-    H_ordered = H.twistedBy(perm);
-
-    // solve Hx = b
-    solver2.compute(H_ordered);
-    if (solver2.info() != Success)
-    {
-        std::cout << "decomposition failed" << std::endl;
-        return 0;
-    }
-    x_ordered = solver2.solve(b_ordered);
-    x_ordered = perm.inverse() * x_ordered;
-    time2 = ((double) clock() - t2) / CLOCKS_PER_SEC;
-
-    // SOLVING AFTER BLOCK REORDERING ------------------------------------
-    // ordering factors
-    block_ordering_factors(size-1) = 2;
-    block_ordering_factors(0) = 2;
-
-    // block ordering
-    t3 = clock();
-    FactorMatrix.makeCompressed();
-
-    std::cout << "Reordering using Block CCOLAMD:" << std::endl;
-    std::cout << "block_ordering_factors = " << std::endl << block_ordering_factors.transpose() << std::endl << std::endl;
-    ordering(FactorMatrix, perm_blocks, block_ordering_factors.data());
-
-    // variable ordering
-    permutation_2_block_permutation(perm_blocks, perm , dim, size);
-    b_block_ordered = perm * b;
-    H_block_ordered = H.twistedBy(perm);
-
-    // solve Hx = b
-    solver3.compute(H_block_ordered);
-    if (solver3.info() != Success)
-    {
-        std::cout << "decomposition failed" << std::endl;
-        return 0;
-    }
-    x_block_ordered = solver3.solve(b_block_ordered);
-    x_block_ordered = perm.inverse() * x_block_ordered;
-    time3 = ((double) clock() - t3) / CLOCKS_PER_SEC;
-
-    // RESULTS ------------------------------------
-    std::cout << "NO REORDERING:    solved in " << time1*1e3 << " ms" << std::endl;
-    std::cout << "REORDERING:       solved in " << time2*1e3 << " ms" << std::endl;
-    std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl;
-    //std::cout << "x = " << x.transpose() << std::endl;
-    //std::cout << "x = " << x_ordered.transpose() << std::endl;
-    //std::cout << "x = " << x_block_ordered.transpose() << std::endl;
-}
-

demos/solver/test_iQR.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of 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/>.
-//
-//--------LICENSE_END--------
-/*
- * test_iQR.cpp
- *
- *  Created on: Jun 17, 2015
- *      Author: jvallve
- */
-
-/*
- * test_ccolamd_blocks.cpp
- *
- *  Created on: Jun 12, 2015
- *      Author: jvallve
- */
-
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// eigen includes
-#include <Eigen/OrderingMethods>
-#include <Eigen/SparseQR>
-#include <Eigen/SPQRSupport>
-
-// ccolamd
-#include "solver/ccolamd_ordering.h"
-
-using namespace Eigen;
-
-class block_pruning
-{
-    public:
-        int col, row, Nrows, Ncols;
-        block_pruning(int _col, int _row, int _Nrows, int _Ncols) :
-                col(_col),
-                row(_row),
-                Nrows(_Nrows),
-                Ncols(_Ncols)
-        {
-            //
-        }
-        bool operator()(int i, int j, double) const
-        {
-            return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1);
-        }
-};
-
-void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
-{
-    // prune all non-zero elements that not satisfy the 'keep' operand
-    // elements that are not in the block rows or are not in the block columns should be kept
-    //original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1); });
-
-    block_pruning bp(row, col, Nrows, Ncols);
-    original.prune(bp);
-
-//  for (unsigned int i = row; i < row + Nrows; i++)
-//    for (unsigned int j = col; j < row + Ncols; j++)
-//      original.coeffRef(i,j) = 0.0;
-//
-//  original.prune(0);
-}
-
-void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
-{
-  for (unsigned int r=0; r<ins.rows(); ++r)
-      for (unsigned int c = 0; c < ins.cols(); c++)
-          if (ins(r,c) != 0)
-              original.coeffRef(r + row, c + col) += ins(r,c);
-}
-
-void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables, const int dim)
-{
-    ArrayXXi idx(dim, perm_nodes.indices().rows());
-    idx.row(0) = dim * perm_nodes.indices().transpose();
-
-    for (int i = 1; i<dim; i++)
-        idx.row(i) = idx.row(i-1) + 1;
-    Map<ArrayXi> idx_blocks(idx.data(), dim*perm_nodes.indices().rows(), 1);
-    perm_variables.indices() = idx_blocks;
-}
-
-void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const int new_size)
-{
-    int old_size = perm.indices().size();
-    int dim = new_size - old_size;
-    VectorXi new_indices(new_size);
-    new_indices.head(old_size)= perm.indices();
-    new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size-1);
-    perm.resize(new_size);
-    perm.indices() = new_indices;
-}
-
-//main
-int main(int argc, char *argv[])
-{
-    if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
-    {
-        std::cout << "Please call me with: [./test_iQR SIZE DIM], where:" << std::endl;
-        std::cout << "     - SIZE: integer size of the problem" << std::endl;
-        std::cout << "     - DIM: integer dimension of the nodes" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-    int size = atoi(argv[1]);
-    int dim = atoi(argv[2]);
-
-    // Problem variables
-    SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver_ordered, solver_unordered, solver_ordered_partial;
-
-    MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
-    SparseMatrix<double> A(0,0),
-                         A_ordered(0,0),
-                         R(0,0);
-    VectorXd b,
-             x,
-             x_ordered,
-             x_ordered_partial;
-    int n_measurements = 0;
-    int n_nodes = 0;
-
-    // ordering variables
-    SparseMatrix<int> A_nodes_ordered(0,0);
-    PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix(0);
-
-    CCOLAMDOrdering<int> ordering, partial_ordering;
-    VectorXi nodes_ordering_factors;
-
-    // results variables
-    clock_t t_ordering, t_solving_ordered_full, t_solving_unordered, t_solving_ordered_partial, t4;
-    double time_ordering=0, time_solving_unordered=0, time_solving_ordered=0, time_solving_ordered_partial=0;
-
-    std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
-
-    // GENERATING MEASUREMENTS
-    std::vector<std::vector<int>> measurements;
-    for (int i = 0; i < size; i++)
-    {
-        std::vector<int> meas(0);
-        if (i == 0) //prior
-        {
-            meas.push_back(0);
-            measurements.push_back(meas);
-            meas.clear();
-        }
-        else //odometry
-        {
-            meas.push_back(i-1);
-            meas.push_back(i);
-            measurements.push_back(meas);
-            meas.clear();
-        }
-        if (i > size / 2) // loop closures
-        {
-            meas.push_back(0);
-            meas.push_back(i);
-            measurements.push_back(meas);
-            meas.clear();
-        }
-    }
-
-    // INCREMENTAL LOOP
-    for (unsigned int i = 0; i < measurements.size(); i++)
-    {
-        std::cout << "========================= MEASUREMENT " << i << ":" << std::endl;
-        std::vector<int> measurement = measurements.at(i);
-
-        // AUGMENT THE PROBLEM ----------------------------
-        n_measurements++;
-        while (n_nodes < measurement.back()+1)
-        {
-            n_nodes++;
-            // Resize accumulated permutations
-            augment_permutation(acc_permutation_nodes_matrix, n_nodes);
-
-            // Resize state
-            x.conservativeResize(n_nodes*dim);
-            x_ordered.conservativeResize(n_nodes*dim);
-            x_ordered_partial.conservativeResize(n_nodes*dim);
-        }
-        A.conservativeResize(n_measurements*dim,n_nodes*dim);
-        A_ordered.conservativeResize(n_measurements*dim,n_nodes*dim);
-        R.conservativeResize(n_nodes*dim,n_nodes*dim);
-        b.conservativeResize(n_measurements*dim);
-        A_nodes_ordered.conservativeResize(n_measurements,n_nodes);
-
-        // ADD MEASUREMENTS
-        int min_ordered_node = n_nodes;
-        for (unsigned int j = 0; j < measurement.size(); j++)
-        {
-            int ordered_node = acc_permutation_nodes_matrix.indices()(measurement.at(j));
-
-            addSparseBlock(2*omega, A, A.rows()-dim, measurement.at(j) * dim);
-            addSparseBlock(2*omega, A_ordered, A_ordered.rows()-dim, ordered_node * dim);
-
-            A_nodes_ordered.coeffRef(A_nodes_ordered.rows()-1, ordered_node) = 1;
-
-            b.segment(b.size() - dim, dim) = VectorXd::LinSpaced(dim, b.size()-dim, b.size()-1);
-            // store minimum ordered node
-            if (min_ordered_node > ordered_node)
-                min_ordered_node = ordered_node;
-        }
-
-//        std::cout << "Solving Ax = b" << std::endl;
-//        std::cout << "A_nodes_ordered: " << std::endl << MatrixXi::Identity(A_nodes_ordered.rows(), A_nodes_ordered.rows()) * A_nodes_ordered  << std::endl << std::endl;
-//        std::cout << "A: " << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A  << std::endl << std::endl;
-//        std::cout << "A_ordered: " << std::endl << MatrixXd::Identity(A.rows(), A.rows()) * A_ordered  << std::endl << std::endl;
-//        std::cout << "b: " << std::endl << b.transpose() << std::endl << std::endl;
-
-        // BLOCK REORDERING ------------------------------------
-        t_ordering = clock();
-        int ordered_nodes = n_nodes - min_ordered_node;
-        int unordered_nodes = n_nodes - ordered_nodes;
-        if (n_nodes > 1 && ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
-        {
-            // SUBPROBLEM ORDERING (from first node variable to last one)
-            std::cout << "ordering partial problem: " << min_ordered_node << " to "<< n_nodes - 1 << std::endl;
-            SparseMatrix<int> sub_A_nodes_ordered = A_nodes_ordered.rightCols(ordered_nodes);
-
-            // partial ordering factors
-            VectorXi nodes_partial_ordering_factors = sub_A_nodes_ordered.bottomRows(1).transpose();
-
-            // computing nodes partial ordering
-            A_nodes_ordered.makeCompressed();
-            PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes_matrix(ordered_nodes);
-            partial_ordering(sub_A_nodes_ordered, partial_permutation_nodes_matrix, nodes_partial_ordering_factors.data());
-
-            // node ordering to variable ordering
-            PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_matrix(A_ordered.cols());
-            permutation_2_block_permutation(partial_permutation_nodes_matrix, partial_permutation_matrix , dim);
-
-            // apply partial orderings
-            A_nodes_ordered.rightCols(ordered_nodes) = (A_nodes_ordered.rightCols(ordered_nodes) * partial_permutation_nodes_matrix.transpose()).sparseView();
-            A_ordered.rightCols(ordered_nodes * dim) = (A_ordered.rightCols(ordered_nodes * dim) * partial_permutation_matrix.transpose()).sparseView();
-            R.rightCols(ordered_nodes * dim) = (R.rightCols(ordered_nodes * dim) * partial_permutation_matrix.transpose()).sparseView();
-
-            // ACCUMULATING PERMUTATIONS
-            PermutationMatrix<Dynamic, Dynamic, int> permutation_nodes_matrix(VectorXi::LinSpaced(n_nodes, 0, n_nodes - 1)); // identity permutation
-            permutation_nodes_matrix.indices().tail(ordered_nodes) = partial_permutation_nodes_matrix.indices() + VectorXi::Constant(ordered_nodes, n_nodes - ordered_nodes);
-            acc_permutation_nodes_matrix = permutation_nodes_matrix * acc_permutation_nodes_matrix;
-        }
-        time_ordering += ((double) clock() - t_ordering) / CLOCKS_PER_SEC;
-        // std::cout << "incrementally ordered A Block structure: " << std::endl << MatrixXi::Identity(A_nodes_ordered.rows(), A_nodes_ordered.rows()) * A_nodes_ordered  << std::endl << std::endl;
-        //std::cout << "ordered A: " << std::endl << MatrixXd::Identity(A_ordered.rows(), A_ordered.rows()) * A_ordered << std::endl << std::endl;
-        //std::cout << "b: " << std::endl << b.transpose() << std::endl << std::endl;
-
-        // SOLVING
-        // solving ordered subproblem
-        t_solving_ordered_partial = clock();
-        A_nodes_ordered.makeCompressed();
-        A_ordered.makeCompressed();
-
-        // finding measurements block
-        SparseMatrix<int> measurements_to_initial = A_nodes_ordered.col(min_ordered_node);
-//        std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
-//        std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
-        int initial_measurement = measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
-
-        SparseMatrix<double> A_ordered_partial = A_ordered.bottomRightCorner((n_nodes - initial_measurement) * dim, ordered_nodes * dim);
-        solver_ordered_partial.compute(A_ordered_partial);
-        if (solver_ordered_partial.info() != Success)
-        {
-            std::cout << "decomposition failed" << std::endl;
-            return 0;
-        }
-        std::cout << "R new" << std::endl << MatrixXd::Identity(ordered_nodes * dim, ordered_nodes * dim) * solver_ordered_partial.matrixR() << std::endl;
-        x_ordered_partial.tail(ordered_nodes * dim) = solver_ordered_partial.solve(b.tail(ordered_nodes * dim));
-        std::cout << "x_ordered_partial.tail(ordered_nodes * dim)" << std::endl << x_ordered_partial.tail(ordered_nodes * dim).transpose() << std::endl;
-        // store new part of R (equivalent to R.bottomRightCorner(ordered_nodes * dim, ordered_nodes * dim) = solver3.matrixR();)
-        eraseSparseBlock(R, unordered_nodes * dim, unordered_nodes * dim, ordered_nodes * dim, ordered_nodes * dim);
-        addSparseBlock(solver_ordered_partial.matrixR(), R, unordered_nodes * dim, unordered_nodes * dim);
-        std::cout << "R" << std::endl << MatrixXd::Identity(R.rows(), R.rows()) * R << std::endl;
-        R.makeCompressed();
-
-        // solving not ordered subproblem
-        if (unordered_nodes > 0)
-        {
-            std::cout << "--------------------- solving unordered part" << std::endl;
-            SparseMatrix<double> R1 = R.topLeftCorner(unordered_nodes * dim, unordered_nodes * dim);
-            std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
-            SparseMatrix<double> R2 = R.topRightCorner(unordered_nodes * dim, ordered_nodes * dim);
-            std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
-            solver_ordered_partial.compute(R1);
-            if (solver_ordered_partial.info() != Success)
-            {
-                std::cout << "decomposition failed" << std::endl;
-                return 0;
-            }
-            x_ordered_partial.head(unordered_nodes * dim) = solver_ordered_partial.solve(b.head(unordered_nodes * dim) - R2 * x_ordered_partial.tail(ordered_nodes * dim));
-        }
-        // undo ordering
-        PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix(A_ordered.cols());
-        permutation_2_block_permutation(acc_permutation_nodes_matrix, acc_permutation_matrix , dim);
-        x_ordered_partial = acc_permutation_matrix.inverse() * x_ordered_partial;
-        time_solving_ordered_partial += ((double) clock() - t_solving_ordered_partial) / CLOCKS_PER_SEC;
-
-        // SOLVING
-        // full ordered problem
-        t_solving_ordered_full = clock();
-        A_nodes_ordered.makeCompressed();
-        A_ordered.makeCompressed();
-        solver_ordered.compute(A_ordered);
-        if (solver_ordered.info() != Success)
-        {
-            std::cout << "decomposition failed" << std::endl;
-            return 0;
-        }
-        x_ordered = solver_ordered.solve(b);
-        std::cout << "solver_ordered.matrixR()" << std::endl << MatrixXd::Identity(A_ordered.cols(), A_ordered.cols()) * solver_ordered.matrixR() << std::endl;
-        // undo ordering
-        PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix2(A_ordered.cols());
-        permutation_2_block_permutation(acc_permutation_nodes_matrix, acc_permutation_matrix2 , dim);
-        x_ordered = acc_permutation_matrix.inverse() * x_ordered;
-        time_solving_ordered += ((double) clock() - t_solving_ordered_full) / CLOCKS_PER_SEC;
-
-        // WITHOUT ORDERING
-        t_solving_unordered = clock();
-        A.makeCompressed();
-        solver_unordered.compute(A);
-        if (solver_unordered.info() != Success)
-        {
-            std::cout << "decomposition failed" << std::endl;
-            return 0;
-        }
-        //std::cout << "no ordering? " << solver_unordered.colsPermutation().indices().transpose() << std::endl;
-        x = solver_unordered.solve(b);
-        std::cout << "solver_unordered.matrixR()" << std::endl << MatrixXd::Identity(A.cols(), A.cols()) * solver_unordered.matrixR() << std::endl;
-        time_solving_unordered += ((double) clock() - t_solving_unordered) / CLOCKS_PER_SEC;
-
-        // RESULTS ------------------------------------
-        std::cout << "========================= RESULTS " << i << ":" << std::endl;
-        std::cout << "NO REORDERING:      solved in " << time_solving_unordered*1e3 << " ms | " << solver_unordered.matrixR().nonZeros() << " nonzeros in R"<< std::endl;
-        std::cout << "BLOCK REORDERING:   solved in " << time_solving_ordered*1e3 << " ms | " << solver_ordered.matrixR().nonZeros() << " nonzeros in R"<< std::endl;
-        std::cout << "BLOCK REORDERING 2: solved in " << time_solving_ordered_partial*1e3 << " ms | " << R.nonZeros() << " nonzeros in R"<< std::endl;
-
-        std::cout << "x =                 " << x.transpose() << std::endl;
-        std::cout << "x_ordered =         " << x_ordered.transpose() << std::endl;
-        std::cout << "x_ordered_partial = " << x_ordered_partial.transpose() << std::endl;
-        if ((x_ordered_partial-x_ordered).maxCoeff() < 1e-10)
-            std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial-x_ordered).maxCoeff() << ")" << std::endl;
-        else
-            std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " << (x_ordered_partial-x_ordered).maxCoeff() << std::endl;
-    }
-}
-

demos/solver/test_iQR_wolf.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of 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/>.
-//
-//--------LICENSE_END--------
-/*
- * test_iQR_wolf.cpp
- *
- *  Created on: Jun 17, 2015
- *      Author: jvallve
- */
-
-//std includes
-#include <cstdlib>
-#include <string>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// eigen includes
-#include <Eigen/OrderingMethods>
-#include <Eigen/SparseQR>
-#include <Eigen/SPQRSupport>
-
-// ccolamd
-#include "solver/ccolamd_ordering.h"
-
-using namespace Eigen;
-
-class block_pruning
-{
-    public:
-        int col, row, Nrows, Ncols;
-        block_pruning(int _col, int _row, int _Nrows, int _Ncols) :
-                col(_col),
-                row(_row),
-                Nrows(_Nrows),
-                Ncols(_Ncols)
-        {
-            //
-        }
-        bool operator()(int i, int j, double) const
-        {
-            return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1);
-        }
-};
-
-void eraseSparseBlock(SparseMatrix<double, ColMajor>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
-{
-    // prune all non-zero elements that not satisfy the 'keep' operand
-    // elements that are not in the block rows or are not in the block columns should be kept
-    //original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1); });
-
-    block_pruning bp(row, col, Nrows, Ncols);
-    original.prune(bp);
-
-//  for (unsigned int i = row; i < row + Nrows; i++)
-//    for (unsigned int j = col; j < row + Ncols; j++)
-//      original.coeffRef(i,j) = 0.0;
-//
-//  original.prune(0);
-}
-
-void addSparseBlock(const MatrixXd& ins, SparseMatrix<double, ColMajor>& original, const unsigned int& row, const unsigned int& col)
-{
-  for (unsigned int r=0; r<ins.rows(); ++r)
-      for (unsigned int c = 0; c < ins.cols(); c++)
-          if (ins(r,c) != 0)
-              original.coeffRef(r + row, c + col) += ins(r,c);
-}
-
-struct node
-{
-    public:
-        int id;
-        int dim;
-        int location;
-        int order;
-
-        node(const int _id, const int _dim, const int _location, const int _order) :
-            id(_id),
-            dim(_dim),
-            location(_location),
-            order(_order)
-        {
-
-        }
-};
-
-struct measurement
-{
-    public:
-        std::vector<MatrixXd> jacobians;
-        std::vector<int> nodes_idx;
-        VectorXd error;
-        int dim;
-        bool odometry_type;
-        int location;
-
-        measurement(const MatrixXd & _jacobian1, const int _idx1, const VectorXd &_error, const int _meas_dim, bool _odometry_type=false) :
-            jacobians({_jacobian1}),
-            nodes_idx({_idx1}),
-            error(_error),
-            dim(_meas_dim),
-            odometry_type(_odometry_type),
-            location(0)
-        {
-            //jacobians.push_back(_jacobian1);
-        }
-
-        measurement(const MatrixXd & _jacobian1, const int _idx1, const MatrixXd & _jacobian2, const int _idx2, const VectorXd &_error, const int _meas_dim, bool _odometry_type=false) :
-            jacobians({_jacobian1, _jacobian2}),
-            nodes_idx({_idx1, _idx2}),
-            error(_error),
-            dim(_meas_dim),
-            odometry_type(_odometry_type),
-            location(0)
-        {
-
-        }
-};
-
-class SolverQR
-{
-    protected:
-        std::string name_;
-        SparseQR < SparseMatrix<double, ColMajor>, NaturalOrdering<int>> solver_;
-        SparseMatrix<double, ColMajor> A_, R_;
-        VectorXd b_, x_incr_;
-        int n_measurements;
-        int n_nodes_;
-        std::vector<node> nodes_;
-        std::vector<measurement> measurements_;
-
-        // ordering
-        SparseMatrix<int, ColMajor> A_nodes_;
-        PermutationMatrix<Dynamic, Dynamic, int> acc_node_permutation_;
-
-        CCOLAMDOrdering<int> orderer_;
-        VectorXi node_ordering_restrictions_;
-        int first_ordered_node_;
-
-        // time
-        clock_t t_ordering_, t_solving_, t_managing_;
-        double time_ordering_, time_solving_, time_managing_;
-
-    public:
-        SolverQR(const std::string &_name) :
-            name_(_name),
-            A_(0,0),
-            R_(0,0), 
-//            b_(0),
-//            x_(0),
-            n_measurements(0),
-            n_nodes_(0),
-            A_nodes_(0,0),
-            acc_node_permutation_(0),
-//            nodes_(0),
-//            measurements_(0),
-            first_ordered_node_(0),
-            t_ordering_(0),
-            t_solving_(0),
-            t_managing_(0),
-            time_ordering_(0),
-            time_solving_(0),
-            time_managing_(0)
-        {
-            //
-        }
-
-        virtual ~SolverQR()
-        {
-            
-        }
-
-        void add_state_unit(const int node_dim, const int node_idx)
-        {
-            t_managing_ = clock();
-
-            n_nodes_++;
-            nodes_.push_back(node(node_idx, node_dim, x_incr_.size(), n_nodes_-1));
-
-            // Resize accumulated permutations
-            augment_permutation(acc_node_permutation_, n_nodes_);
-
-            // Resize state
-            x_incr_.conservativeResize(x_incr_.size() + node_dim);
-
-            // Resize problem
-            A_.conservativeResize(A_.rows(), A_.cols() + node_dim);
-            R_.conservativeResize(R_.cols() + node_dim, R_.cols() + node_dim);
-            //A_nodes_.conservativeResize(n_measurements, n_nodes); // not necessary
-
-            time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
-        }
-
-        void addFactor(const measurement& _meas)
-        {
-            t_managing_ = clock();
-
-            assert(_meas.jacobians.size() == _meas.nodes_idx.size());
-            assert(_meas.error.size() == _meas.dim);
-
-            n_measurements++;
-            measurements_.push_back(_meas);
-            measurements_.back().location = A_.rows();
-
-            // Resize problem
-            A_.conservativeResize(A_.rows() + _meas.dim, A_.cols());
-            b_.conservativeResize(b_.size() + _meas.dim);
-            A_nodes_.conservativeResize(n_measurements,n_nodes_);
-
-            // ADD MEASUREMENTS
-            first_ordered_node_ = n_nodes_;
-            for (unsigned int j = 0; j < _meas.nodes_idx.size(); j++)
-            {
-                assert(acc_node_permutation_.indices()(_meas.nodes_idx.at(j)) == nodes_.at(_meas.nodes_idx.at(j)).order);
-
-                int ordered_node = nodes_.at(_meas.nodes_idx.at(j)).order;//acc_permutation_nodes_.indices()(_nodes_idx.at(j));
-
-                addSparseBlock(_meas.jacobians.at(j), A_, A_.rows()-_meas.dim, nodes_.at(_meas.nodes_idx.at(j)).location);
-
-                A_nodes_.coeffRef(A_nodes_.rows()-1, ordered_node) = 1;
-
-                assert(_meas.jacobians.at(j).cols() == nodes_.at(_meas.nodes_idx.at(j)).dim);
-                assert(_meas.jacobians.at(j).rows() == _meas.dim);
-
-                // store minimum ordered node
-                if (first_ordered_node_ > ordered_node)
-                    first_ordered_node_ = ordered_node;
-            }
-
-            // error
-            b_.tail(_meas.dim) = _meas.error;
-
-            time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
-        }
-
-        void ordering(const int & _first_ordered_node)
-        {
-            t_ordering_ = clock();
-
-            // full problem ordering
-            if (_first_ordered_node == 0)
-            {
-                // ordering ordering factors
-                node_ordering_restrictions_.resize(n_nodes_);
-                node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose();
-
-                // computing nodes partial ordering_
-                A_nodes_.makeCompressed();
-                PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(n_nodes_);
-                orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data());
-
-                // node ordering to variable ordering
-                PermutationMatrix<Dynamic, Dynamic, int> incr_permutation(A_.cols());
-                nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation);
-
-                // apply partial_ordering orderings
-                A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView();
-                A_ = (A_ * incr_permutation.transpose()).sparseView();
-
-                // ACCUMULATING PERMUTATIONS
-                accumulatePermutation(incr_permutation_nodes);
-            }
-
-            // partial ordering
-            else
-            {
-                int ordered_nodes = n_nodes_ - _first_ordered_node;
-                int unordered_nodes = n_nodes_ - ordered_nodes;
-                if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
-                {
-                    // SUBPROBLEM ORDERING (from first node variable to last one)
-                    //std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl;
-                    SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
-
-                    // _partial_ordering ordering_ factors
-                    node_ordering_restrictions_.resize(ordered_nodes);
-                    node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose();
-
-                    // computing nodes partial ordering_
-                    sub_A_nodes_.makeCompressed();
-                    PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes(ordered_nodes);
-                    orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data());
-
-                    // node ordering to variable ordering
-                    PermutationMatrix<Dynamic, Dynamic, int> partial_permutation(A_.cols());
-                    nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation);
-
-                    // apply partial_ordering orderings
-                    int ordered_variables = A_.cols() - nodes_.at(_first_ordered_node).location;
-                    A_nodes_.rightCols(ordered_nodes) = (A_nodes_.rightCols(ordered_nodes) * partial_permutation_nodes.transpose()).sparseView();
-                    A_.rightCols(ordered_variables) = (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
-                    R_.rightCols(ordered_variables) = (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
-
-                    // ACCUMULATING PERMUTATIONS
-                    accumulatePermutation(partial_permutation_nodes);
-                }
-            }
-            time_ordering_ += ((double) clock() - t_ordering_) / CLOCKS_PER_SEC;
-        }
-
-        bool solve(const int mode)
-        {
-            bool batch = (mode !=2 || first_ordered_node_ == 0);
-            bool order = (mode !=0 && n_nodes_ > 1);
-
-            // BATCH
-            if (batch)
-            {
-                // REORDER
-                if (order)
-                    ordering(0);
-
-                //print_problem();
-
-                // SOLVE
-                t_solving_ = clock();
-                A_.makeCompressed();
-                solver_.compute(A_);
-                if (solver_.info() != Success)
-                {
-                    std::cout << "decomposition failed" << std::endl;
-                    return 0;
-                }
-                x_incr_ = solver_.solve(b_);
-                R_ = solver_.matrixR();
-                //std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
-                time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
-            }
-            // INCREMENTAL
-            else
-            {
-                // REORDER SUBPROBLEM
-                ordering(first_ordered_node_);
-                //print_problem();
-
-                // SOLVE ORDERED SUBPROBLEM
-                t_solving_= clock();
-                A_nodes_.makeCompressed();
-                A_.makeCompressed();
-
-                // finding measurements block
-                SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_);
-        //        std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
-        //        std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
-                int first_ordered_measurement = measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
-                int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
-                int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location;
-                int unordered_variables = nodes_.at(first_ordered_node_).location;
-
-                SparseMatrix<double, ColMajor> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
-                solver_.compute(A_partial);
-                if (solver_.info() != Success)
-                {
-                    std::cout << "decomposition failed" << std::endl;
-                    return 0;
-                }
-                //std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl;
-                x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements));
-
-                // store new part of R
-                eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
-                //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
-                addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
-                //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
-                R_.makeCompressed();
-
-                // solving not ordered subproblem
-                if (unordered_variables > 0)
-                {
-                    //std::cout << "--------------------- solving unordered part" << std::endl;
-                    SparseMatrix<double, ColMajor> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
-                    //std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
-                    SparseMatrix<double, ColMajor> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
-                    //std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
-                    solver_.compute(R1);
-                    if (solver_.info() != Success)
-                    {
-                        std::cout << "decomposition failed" << std::endl;
-                        return 0;
-                    }
-                    x_incr_.head(unordered_variables) = solver_.solve(b_.head(unordered_variables) - R2 * x_incr_.tail(ordered_variables));
-                }
-
-            }
-            // UNDO ORDERING FOR RESULT
-            PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
-            nodePermutation2VariablesPermutation(acc_node_permutation_, acc_permutation); // TODO via pointers
-            x_incr_ = acc_permutation.inverse() * x_incr_;
-
-            time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
-
-            return 1;
-        }
-
-        void nodePermutation2VariablesPermutation(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables)
-        {
-            ArrayXi locations = perm_nodes_2_locations(_perm_nodes);
-
-            int last_idx = 0;
-            for (unsigned int i = 0; i<locations.size(); i++)
-            {
-                perm_variables.indices().segment(last_idx, nodes_.at(i).dim) = VectorXi::LinSpaced(nodes_.at(i).dim, locations(i), locations(i)+nodes_.at(i).dim-1);
-                last_idx += nodes_.at(i).dim;
-            }
-        }
-
-        ArrayXi perm_nodes_2_locations(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes)
-        {
-            ArrayXi indices = _perm_nodes.indices().array();
-
-            for (unsigned int i = 0; i<indices.size(); i++)
-                indices = (indices > indices(i)).select(indices + nodes_.at(i).dim-1, indices);
-
-            return indices;
-        }
-
-        void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const int new_size)
-        {
-            int old_size = perm.indices().size();
-            int dim = new_size - old_size;
-            VectorXi new_indices(new_size);
-            new_indices.head(old_size)= perm.indices();
-            new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size-1);
-            perm.resize(new_size);
-            perm.indices() = new_indices;
-        }
-
-        void accumulatePermutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm)
-        {
-            printName();
-            //std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
-            //std::cout << "incr perm " << perm.indices().transpose() << std::endl;
-
-            // acumulate permutation
-            if (perm.size() == acc_node_permutation_.size()) //full permutation
-                acc_node_permutation_ = perm * acc_node_permutation_;
-            else //partial permutation
-            {
-                PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(VectorXi::LinSpaced(n_nodes_, 0, n_nodes_ - 1)); // identity permutation
-                incr_permutation_nodes.indices().tail(perm.size()) = perm.indices() + VectorXi::Constant(perm.size(), n_nodes_ - perm.size());
-                //std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl;
-                acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_;
-            }
-            //std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
-
-            // update nodes orders and locations
-            ArrayXi locations = perm_nodes_2_locations(acc_node_permutation_);
-            for (unsigned int i = 0; i < nodes_.size(); i++)
-            {
-                nodes_.at(i).order = acc_node_permutation_.indices()(i);
-                nodes_.at(i).location = locations(i);
-            }
-        }
-
-        void printName()
-        {
-            std::cout << name_;
-        }
-
-        void printResults()
-        {
-            printName();
-            std::cout << " solved in " << time_solving_*1e3 << " ms | " << R_.nonZeros() << " nonzeros in R"<< std::endl;
-            std::cout << "x = " << x_incr_.transpose() << std::endl;
-        }
-
-        void printProblem()
-        {
-            printName();
-            std::cout << std::endl << "A_nodes_: " << std::endl << MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_  << std::endl << std::endl;
-            std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_  << std::endl << std::endl;
-            std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl;
-        }
-};
-
-//main
-int main(int argc, char *argv[])
-{
-    if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
-    {
-        std::cout << "Please call me with: [./test_iQR SIZE DIM], where:" << std::endl;
-        std::cout << "     - SIZE: integer size of the problem" << std::endl;
-        std::cout << "     - DIM: integer dimension of the nodes" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-    int size = atoi(argv[1]);
-    int dim = atoi(argv[2]);
-
-    // Problems
-    SolverQR solver_ordered("FULL ORDERED");
-    SolverQR solver_unordered("UNORDERED");
-    SolverQR solver_ordered_partial("PARTIALLY ORDERED");
-
-    MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
-
-    // results variables
-    clock_t t_ordering, t_solving_ordered_full, t_solving_unordered, t_solving_ordered_partial, t4;
-    double time_ordering=0, time_solving_unordered=0, time_solving_ordered=0, time_solving_ordered_partial=0;
-
-    std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
-
-    // GENERATING MEASUREMENTS
-    std::vector<measurement> measurements;
-    for (int i = 0; i < size; i++)
-    {
-        std::vector<int> meas(0);
-        if (i == 0) //prior
-            measurements.push_back(measurement(omega, 0, VectorXd::LinSpaced(dim, 0, dim-1), dim));
-
-        else //odometry
-            measurements.push_back(measurement(2*omega, i-1, 2*omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i+1)-1), dim, true));
-
-        if (i > size / 2) //loop closures
-            measurements.push_back(measurement(4*omega, 0, 4*omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i+1)-1), dim));
-    }
-
-    // INCREMENTAL LOOP
-    for (unsigned int i = 0; i < measurements.size(); i++)
-    {
-        std::cout << "========================= MEASUREMENT " << i << ":" << std::endl;
-
-        // AUGMENT THE PROBLEM ----------------------------
-        if (measurements.at(i).odometry_type || i == 0) // if odometry, augment the problem
-        {
-            solver_unordered.add_state_unit(dim, i);
-            solver_ordered.add_state_unit(dim, i);
-            solver_ordered_partial.add_state_unit(dim,i);
-        }
-
-        // ADD MEASUREMENTS
-        solver_unordered.addFactor(measurements.at(i));
-        solver_ordered.addFactor(measurements.at(i));
-        solver_ordered_partial.addFactor(measurements.at(i));
-
-        // PRINT PROBLEM
-        solver_unordered.printProblem();
-        solver_ordered.printProblem();
-        solver_ordered_partial.printProblem();
-
-        // SOLVING
-        solver_unordered.solve(0);
-        solver_ordered.solve(1);
-        solver_ordered_partial.solve(2);
-
-        // RESULTS ------------------------------------
-        std::cout << "========================= RESULTS " << i << ":" << std::endl;
-        solver_unordered.printResults();
-        solver_ordered.printResults();
-        solver_ordered_partial.printResults();
-
-//        if ((x_ordered_partial-x_ordered).maxCoeff() < 1e-10)
-//            std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial-x_ordered).maxCoeff() << ")" << std::endl;
-//        else
-//            std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " << (x_ordered_partial-x_ordered).maxCoeff() << std::endl;
-    }
-}
-

demos/solver/test_iQR_wolf2.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of 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/>.
-//
-//--------LICENSE_END--------
-/*
- * test_iQR_wolf.cpp
- *
- *  Created on: Jun 17, 2015
- *      Author: jvallve
- */
-
-//std includes
-#include <cstdlib>
-#include <string>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-//Wolf includes
-#include "core/state_block/state_block.h"
-#include "core/factor/factor_base.h"
-#include "core/sensor/sensor_laser_2D.h"
-#include "wolf_manager.h"
-
-// wolf solver
-#include "solver/qr_solver.h"
-
-//C includes for sleep, time and main args
-#include "unistd.h"
-
-//faramotics includes
-#include "faramotics/dynamicSceneRender.h"
-#include "faramotics/rangeScan2D.h"
-#include "btr-headers/pose3d.h"
-
-//Ceres includes
-#include "glog/logging.h"
-
-#include "../../include/core/ceres_wrapper/solver_ceres.h"
-#include "iri-algorithms/laser_scan_utils/corner_detector.h"
-#include "iri-algorithms/laser_scan_utils/entities.h"
-
-//function travel around
-void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_)
-{
-    if (ii <= 120)
-    {
-        displacement_ = 0.1;
-        rotation_ = 0;
-    }
-    else if ((ii > 120) && (ii <= 170))
-    {
-        displacement_ = 0.2;
-        rotation_ = 1.8 * M_PI / 180;
-    }
-    else if ((ii > 170) && (ii <= 220))
-    {
-        displacement_ = 0;
-        rotation_ = -1.8 * M_PI / 180;
-    }
-    else if ((ii > 220) && (ii <= 310))
-    {
-        displacement_ = 0.1;
-        rotation_ = 0;
-    }
-    else if ((ii > 310) && (ii <= 487))
-    {
-        displacement_ = 0.1;
-        rotation_ = -1. * M_PI / 180;
-    }
-    else if ((ii > 487) && (ii <= 600))
-    {
-        displacement_ = 0.2;
-        rotation_ = 0;
-    }
-    else if ((ii > 600) && (ii <= 700))
-    {
-        displacement_ = 0.1;
-        rotation_ = -1. * M_PI / 180;
-    }
-    else if ((ii > 700) && (ii <= 780))
-    {
-        displacement_ = 0;
-        rotation_ = -1. * M_PI / 180;
-    }
-    else
-    {
-        displacement_ = 0.3;
-        rotation_ = 0.0 * M_PI / 180;
-    }
-
-    pose.moveForward(displacement_);
-    pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll());
-}
-
-//main
-int main(int argc, char *argv[])
-{
-    using namespace Eigen;
-    using namespace wolf;
-
-    // USER INPUT ============================================================================================
-    if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[1]) > 1100 || atoi(argv[2]) < 0 || atoi(argv[2]) > 2)
-    {
-        std::cout << "Please call me with: [./test_ceres_manager NI MODE], where:" << std::endl;
-        std::cout << "     - NI is the number of iterations (0 < NI < 1100)" << std::endl;
-        std::cout << "     - MODE is the solver mode (0 batch (no ordering), 1 batch (ordering), 2 incremental" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-    bool complex_angle = false;
-    unsigned int solving_mode = (unsigned int) atoi(argv[2]); //solving mode
-    unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
-
-    // INITIALIZATION ============================================================================================
-
-    //init random generators
-    double odom_std_factor = 0.1;
-    double gps_std = 10;
-    std::default_random_engine generator(1);
-    std::normal_distribution<double> gaussian_distribution(0.0, 1);
-
-    // Faramotics stuff
-    Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
-    vector < Cpose3d > devicePoses;
-    vector<float> scan1, scan2;
-    string modelFileName;
-
-    //model and initial view point
-    modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
-    //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
-    //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
-    devicePose.setPose(2, 8, 0.2, 0, 0, 0);
-    viewPoint.setPose(devicePose);
-    viewPoint.moveForward(10);
-    viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
-    viewPoint.moveForward(-15);
-    //glut initialization
-    faramotics::initGLUT(argc, argv);
-
-    //create a viewer for the 3D model and scan points
-    CdynamicSceneRender* myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
-    myRender->loadAssimpModel(modelFileName, true); //with wireframe
-    //create scanner and load 3D model
-    CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG);  //HOKUYO_UTM30LX_180DEG or LEUZE_RS4
-    myScanner->loadAssimpModel(modelFileName);
-
-    //variables
-    Eigen::Vector3d odom_reading;
-    Eigen::Vector2d gps_fix_reading;
-    Eigen::VectorXd pose_odom(3); //current odometry integred pose
-    Eigen::VectorXd ground_truth(n_execution * 3); //all true poses
-    Eigen::VectorXd odom_trajectory(n_execution * 3); //open loop trajectory
-    Eigen::VectorXd mean_times = Eigen::VectorXd::Zero(7);
-    clock_t t1, t2;
-
-    // Wolf manager initialization
-    Eigen::Vector3d odom_pose = Eigen::Vector3d::Zero();
-    Eigen::Vector3d gps_pose = Eigen::Vector3d::Zero();
-    Eigen::Vector4d laser_1_pose, laser_2_pose; //xyz + theta
-    laser_1_pose << 1.2, 0, 0, 0; //laser 1
-    laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
-    SensorOdom2D odom_sensor(std::make_shared<StatePoint2d>(odom_pose.head(2)), std::make_shared<StateAngle>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
-    SensorGPSFix gps_sensor(std::make_shared<StatePoint2d>(gps_pose.head(2)), std::make_shared<StateAngle>(gps_pose.tail(1)), gps_std);
-    SensorLaser2D laser_1_sensor(std::make_shared<StatePoint2d>(laser_1_pose.head(2)), std::make_shared<StateAngle>(laser_1_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
-    SensorLaser2D laser_2_sensor(std::make_shared<StatePoint2d>(laser_2_pose.head(2)), std::make_shared<StateAngle>(laser_2_pose.tail(1)), laserscanutils::LaserScanParams({M_PI/2,-M_PI/2, -M_PI/720,0.01,0.2,100,0.01,0.01}));
-
-    // Initial pose
-    pose_odom << 2, 8, 0;
-    ground_truth.head(3) = pose_odom;
-    odom_trajectory.head(3) = pose_odom;
-
-    WolfManager* wolf_manager_QR = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution*10, 0.01);
-    WolfManager* wolf_manager_ceres = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution*10, 0.01);
-
-    // Ceres initialization
-    ceres::Solver::Options ceres_options;
-    ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
-    ceres_options.max_line_search_step_contraction = 1e-3;
-    //    ceres_options.minimizer_progress_to_stdout = false;
-    //    ceres_options.line_search_direction_type = ceres::LBFGS;
-    //    ceres_options.max_num_iterations = 100;
-    SolverCeres* ceres_manager = new SolverCeres(wolf_manager_ceres->getProblem(), ceres_options);
-    std::ofstream log_file, landmark_file;  //output file
-
-    // Own solver
-    SolverQR solver_(wolf_manager_QR->getProblem());
-
-    std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
-    std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
-    // START TRAJECTORY ============================================================================================
-    for (unsigned int step = 1; step < n_execution; step++)
-    {
-        //get init time
-        t2 = clock();
-
-        // ROBOT MOVEMENT ---------------------------
-        //std::cout << "ROBOT MOVEMENT..." << std::endl;
-        // moves the device position
-        t1 = clock();
-        motionCampus(step, devicePose, odom_reading(0), odom_reading(2));
-        odom_reading(1) = 0;
-        devicePoses.push_back(devicePose);
-
-        // SENSOR DATA ---------------------------
-        //std::cout << "SENSOR DATA..." << std::endl;
-        // store groundtruth
-        ground_truth.segment(step * 3, 3) << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
-
-        // compute odometry
-        odom_reading(0) += gaussian_distribution(generator) * odom_std_factor * (odom_reading(0) == 0 ? 1e-6 : odom_reading(0));
-        odom_reading(1) += gaussian_distribution(generator) * odom_std_factor * 1e-6;
-        odom_reading(2) += gaussian_distribution(generator) * odom_std_factor * (odom_reading(2) == 0 ? 1e-6 : odom_reading(2));
-
-        // odometry integration
-        pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2)) - odom_reading(1) * sin(pose_odom(2));
-        pose_odom(1) = pose_odom(1) + odom_reading(0) * sin(pose_odom(2)) + odom_reading(1) * cos(pose_odom(2));
-        pose_odom(2) = pose_odom(2) + odom_reading(1);
-        odom_trajectory.segment(step * 3, 3) = pose_odom;
-
-        // compute GPS
-        gps_fix_reading << devicePose.pt(0), devicePose.pt(1);
-        gps_fix_reading(0) += gaussian_distribution(generator) * gps_std;
-        gps_fix_reading(1) += gaussian_distribution(generator) * gps_std;
-
-        //compute scans
-        scan1.clear();
-        scan2.clear();
-        // scan 1
-        laser1Pose.setPose(devicePose);
-        laser1Pose.moveForward(laser_1_pose(0));
-        myScanner->computeScan(laser1Pose, scan1);
-        // scan 2
-        laser2Pose.setPose(devicePose);
-        laser2Pose.moveForward(laser_2_pose(0));
-        laser2Pose.rt.setEuler(laser2Pose.rt.head() + M_PI, laser2Pose.rt.pitch(), laser2Pose.rt.roll());
-        myScanner->computeScan(laser2Pose, scan2);
-
-        mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // ADD CAPTURES ---------------------------
-        //std::cout << "ADD CAPTURES..." << std::endl;
-        // adding new sensor captures
-        wolf_manager_QR->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading));       //, odom_std_factor * Eigen::MatrixXd::Identity(2,2)));
-        wolf_manager_QR->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3,3)));
-        wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading));       //, odom_std_factor * Eigen::MatrixXd::Identity(2,2)));
-        wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3,3)));
-        //wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
-        //wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
-        // updating problem
-        wolf_manager_QR->update();
-        wolf_manager_ceres->update();
-
-        // UPDATING SOLVER ---------------------------
-        //std::cout << "UPDATING..." << std::endl;
-        // update state units and factors in ceres
-        solver_.update();
-
-        // PRINT PROBLEM
-        //solver_.printProblem();
-
-        // SOLVE OPTIMIZATION ---------------------------
-        //std::cout << "SOLVING..." << std::endl;
-        ceres::Solver::Summary summary = ceres_manager->solve();
-        solver_.solve(solving_mode);
-
-        std::cout << "========================= RESULTS " << step << ":" << std::endl;
-        //solver_.printResults();
-        std::cout << "QR vehicle pose    " << wolf_manager_QR->getVehiclePose().transpose() << std::endl;
-        std::cout << "ceres vehicle pose " << wolf_manager_ceres->getVehiclePose().transpose() << std::endl;
-
-        // COMPUTE COVARIANCES ---------------------------
-        //std::cout << "COMPUTING COVARIANCES..." << std::endl;
-        // TODO
-
-        // DRAWING STUFF ---------------------------
-        // draw detected corners
-//        std::list < laserscanutils::Corner > corner_list;
-//        std::vector<double> corner_vector;
-//        CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
-//        last_scan.extractCorners(corner_list);
-//        for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
-//        {
-//            corner_vector.push_back(corner_it->pt_(0));
-//            corner_vector.push_back(corner_it->pt_(1));
-//        }
-//        myRender->drawCorners(laser1Pose, corner_vector);
-
-        // draw landmarks
-        std::vector<double> landmark_vector;
-        for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList())
-        {
-            double* position_ptr = landmark->getP()->get();
-            landmark_vector.push_back(*position_ptr); //x
-            landmark_vector.push_back(*(position_ptr + 1)); //y
-            landmark_vector.push_back(0.2); //z
-        }
-        myRender->drawLandmarks(landmark_vector);
-
-        // draw localization and sensors
-        estimated_vehicle_pose.setPose(wolf_manager_QR->getVehiclePose()(0), wolf_manager_QR->getVehiclePose()(1), 0.2, wolf_manager_QR->getVehiclePose()(2), 0, 0);
-        estimated_laser_1_pose.setPose(estimated_vehicle_pose);
-        estimated_laser_1_pose.moveForward(laser_1_pose(0));
-        estimated_laser_2_pose.setPose(estimated_vehicle_pose);
-        // instead of laser 2 we draw ceres solution
-        //estimated_laser_2_pose.moveForward(laser_2_pose(0));
-        //estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + M_PI, estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
-        estimated_laser_2_pose.setPose(wolf_manager_ceres->getVehiclePose()(0), wolf_manager_ceres->getVehiclePose()(1), 0.2, wolf_manager_ceres->getVehiclePose()(2), 0, 0);
-
-        myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
-
-        //Set view point and render the scene
-        //locate visualization view point, somewhere behind the device
-        myRender->setViewPoint(viewPoint);
-        myRender->drawPoseAxis(devicePose);
-        myRender->drawScan(laser1Pose, scan1, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
-        myRender->render();
-
-        // TIME MANAGEMENT ---------------------------
-        double dt = ((double) clock() - t2) / CLOCKS_PER_SEC;
-        mean_times(6) += dt;
-        if (dt < 0.1)
-            usleep(100000 - 1e6 * dt);
-
-//      std::cout << "\nTree after step..." << std::endl;
-//      wolf_manager->getProblem()->print();
-    }
-
-    // DISPLAY RESULTS ============================================================================================
-    mean_times /= n_execution;
-    std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
-    std::cout << "  data generation:    " << mean_times(0) << std::endl;
-    std::cout << "  wolf managing:      " << mean_times(1) << std::endl;
-    std::cout << "  ceres managing:     " << mean_times(2) << std::endl;
-    std::cout << "  ceres optimization: " << mean_times(3) << std::endl;
-    std::cout << "  ceres covariance:   " << mean_times(4) << std::endl;
-    std::cout << "  results drawing:    " << mean_times(5) << std::endl;
-    std::cout << "  loop time:          " << mean_times(6) << std::endl;
-
-//  std::cout << "\nTree before deleting..." << std::endl;
-//  wolf_manager->getProblem()->print();
-
-    // Draw Final result -------------------------
-    std::cout << "Drawing final results..." << std::endl;
-    std::vector<double> landmark_vector;
-    for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList())
-    {
-        double* position_ptr = landmark->getP()->get();
-        landmark_vector.push_back(*position_ptr); //x
-        landmark_vector.push_back(*(position_ptr + 1)); //y
-        landmark_vector.push_back(0.2); //z
-    }
-    myRender->drawLandmarks(landmark_vector);
-//  viewPoint.setPose(devicePoses.front());
-//  viewPoint.moveForward(10);
-//  viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/4, viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
-//  viewPoint.moveForward(-10);
-    myRender->setViewPoint(viewPoint);
-    myRender->render();
-
-    // Print Final result in a file -------------------------
-    std::cout << "Printing results in a file..." << std::endl;
-    // Vehicle poses
-    std::cout << "Vehicle poses..." << std::endl;
-    int i = 0;
-    Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->size() * 3);
-    for (auto frame : wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap())
-    {
-        if (complex_angle)
-            state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), atan2(*frame->getO()->get(), *(frame->getO()->get() + 1));
-        else
-            state_poses.segment(i, 3) << *frame->getP()->get(), *(frame->getP()->get() + 1), *frame->getO()->get();
-        i += 3;
-    }
-
-    // Landmarks
-    std::cout << "Landmarks..." << std::endl;
-    i = 0;
-    Eigen::VectorXd landmarks(wolf_manager_QR->getProblem()->getMap()->getLandmarkList().size() * 2);
-    for (auto landmark : wolf_manager_QR->getProblem()->getMap()->getLandmarkList())
-    {
-        Eigen::Map<Eigen::Vector2d> landmark_p(landmark->getP()->get());
-        landmarks.segment(i, 2) = landmark_p;
-        i += 2;
-    }
-
-    // Print log files
-    std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt"));
-    log_file.open(filepath, std::ofstream::out); //open log file
-
-    if (log_file.is_open())
-    {
-        log_file << 0 << std::endl;
-        for (unsigned int ii = 0; ii < n_execution; ii++)
-            log_file << state_poses.segment(ii * 3, 3).transpose() << "\t" << ground_truth.segment(ii * 3, 3).transpose() << "\t" << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t" << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl;
-        log_file.close(); //close log file
-        std::cout << std::endl << "Result file " << filepath << std::endl;
-    }
-    else
-        std::cout << std::endl << "Failed to write the log file " << filepath << std::endl;
-
-    std::string filepath2 = getenv("HOME") + (complex_angle ? std::string("/Desktop/landmarks_file_3.txt") : std::string("/Desktop/landmarks_file_2.txt"));
-    landmark_file.open(filepath2, std::ofstream::out); //open log file
-
-    if (landmark_file.is_open())
-    {
-        for (unsigned int ii = 0; ii < landmarks.size(); ii += 2)
-            landmark_file << landmarks.segment(ii, 2).transpose() << std::endl;
-        landmark_file.close(); //close log file
-        std::cout << std::endl << "Landmark file " << filepath << std::endl;
-    }
-    else
-        std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl;
-
-    std::cout << "Press any key for ending... " << std::endl << std::endl;
-    std::getchar();
-
-    delete myRender;
-    delete myScanner;
-    delete wolf_manager_QR;
-    delete wolf_manager_ceres;
-    std::cout << "wolf deleted" << std::endl;
-
-    std::cout << " ========= END ===========" << std::endl << std::endl;
-
-    //exit
-    return 0;
-}
-

demos/solver/test_incremental_ccolamd_blocks.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of 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/>.
-//
-//--------LICENSE_END--------
-/*
- * test_ccolamd_blocks.cpp
- *
- *  Created on: Jun 12, 2015
- *      Author: jvallve
- */
-
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// eigen includes
-#include <Eigen/OrderingMethods>
-#include <Eigen/CholmodSupport>
-#include <Eigen/SparseLU>
-
-// ccolamd
-#include "solver/ccolamd_ordering.h"
-
-using namespace Eigen;
-
-void eraseSparseBlock(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& Nrows, const unsigned int& col, const unsigned int& Ncols)
-{
-  for (unsigned int i = row; i < row + Nrows; i++)
-    for (unsigned int j = col; j < row + Ncols; j++)
-      original.coeffRef(i,j) = 0.0;
-
-  original.makeCompressed();
-}
-
-void addSparseBlock(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
-{
-  for (unsigned int r=0; r<ins.rows(); ++r)
-      for (unsigned int c = 0; c < ins.cols(); c++)
-          if (ins(r,c) != 0)
-              original.coeffRef(r + row, c + col) += ins(r,c);
-}
-
-void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm, PermutationMatrix<Dynamic, Dynamic, int> &perm_blocks, const int dim, const int size)
-{
-    ArrayXXi idx(dim, size);
-    idx.row(0) = dim * perm.indices().transpose();
-
-    for (int i = 1; i<dim; i++)
-        idx.row(i) = idx.row(i-1) + 1;
-    Map<ArrayXi> idx_blocks(idx.data(), dim*size, 1);
-    perm_blocks.indices() = idx_blocks;
-}
-
-//main
-int main(int argc, char *argv[])
-{
-    if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
-    {
-        std::cout << "Please call me with: [./test_ccolamd SIZE DIM], where:" << std::endl;
-        std::cout << "     - SIZE: integer size of the problem" << std::endl;
-        std::cout << "     - DIM: integer dimension of the nodes" << std::endl;
-        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
-        return -1;
-    }
-    int size = atoi(argv[1]);
-    int dim = atoi(argv[2]);
-
-    // Problem variables
-    //CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3;
-    SparseLU < SparseMatrix<double>, NaturalOrdering<int> > solver, solver2, solver3;
-    MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
-    SparseMatrix<double> H(dim,dim),
-                         H_ordered(dim,dim),
-                         H_b_ordered(dim,dim);
-    VectorXd b(dim),
-             b_ordered(dim),
-             b_b_ordered(dim),
-             x_b_ordered(dim),
-             x_ordered(dim),
-             x(dim);
-
-    // ordering variables
-    SparseMatrix<int> factors(1,1), factors_ordered(1,1);
-    ArrayXi acc_permutation(dim),
-            acc_permutation_b(dim),
-            acc_permutation_factors(1);
-    acc_permutation = ArrayXi::LinSpaced(dim,0,dim-1);
-    acc_permutation_b = acc_permutation;
-    acc_permutation_factors(0) = 0;
-
-    CCOLAMDOrdering<int> ordering;
-    VectorXi factor_ordering_factors(1);
-    VectorXi ordering_factors(1);
-
-    // results variables
-    clock_t t1, t2, t3;
-    double time1=0, time2=0, time3=0;
-
-    // INITIAL STATE
-    addSparseBlock(5*omega, H, 0, 0);
-    factors.insert(0,0) = 1;
-    b.head(dim) = VectorXd::LinSpaced(Sequential, dim, 0, dim-1);
-
-    std::cout << "STARTING INCREMENTAL TEST" << std::endl << std::endl;
-
-    // INCREMENTAL LOOP
-    for (int i = 1; i < size; i++)
-    {
-        std::cout << "========================= STEP " << i << ":" << std::endl;
-        // AUGMENT THE PROBLEM ----------------------------
-        H.conservativeResize((i+1)*dim,(i+1)*dim);
-        H_ordered.conservativeResize((i+1)*dim,(i+1)*dim);
-        H_b_ordered.conservativeResize((i+1)*dim,(i+1)*dim);
-        b.conservativeResize((i+1)*dim);
-        b_ordered.conservativeResize((i+1)*dim);
-        b_b_ordered.conservativeResize((i+1)*dim);
-        x.conservativeResize((i+1)*dim);
-        x_ordered.conservativeResize((i+1)*dim);
-        x_b_ordered.conservativeResize((i+1)*dim);
-        factors.conservativeResize(i+1, i+1);
-
-        // Odometry
-        addSparseBlock(5*omega, H, i*dim, i*dim);
-        addSparseBlock(omega, H, i*dim, (i-1)*dim);
-        addSparseBlock(omega, H, (i-1)*dim, i*dim);
-        factors.insert(i,i) = 1;
-        factors.insert(i,i-1) = 1;
-        factors.insert(i-1,i) = 1;
-
-        // Loop Closure
-        if (i == size-1)
-        {
-            addSparseBlock(2*omega, H, 0, i*dim);
-            addSparseBlock(2*omega, H, i*dim, 0);
-            factors.insert(0,i) = 1;
-            factors.insert(i,0) = 1;
-        }
-
-        // r.h.v
-        b.segment(i*dim, dim) = VectorXd::LinSpaced(Sequential, dim, dim*i, dim *(i+1)-1);
-
-        std::cout << "Solving factor graph:" << std::endl;
-        std::cout << "Factors: " << std::endl << factors * MatrixXi::Identity((i+1), (i+1)) << std::endl << std::endl;
-//        std::cout << "H: " << std::endl << H * MatrixXd::Identity(dim*(i+1), dim*(i+1)) << std::endl << std::endl;
-
-        // SOLVING WITHOUT REORDERING ------------------------------------
-        // solve Hx = b
-        t1 = clock();
-        solver.compute(H);
-        if (solver.info() != Success)
-        {
-            std::cout << "decomposition failed" << std::endl;
-            return 0;
-        }
-        x = solver.solve(b);
-        time1 += ((double) clock() - t1) / CLOCKS_PER_SEC;
-
-        // SOLVING WITH REORDERING ------------------------------------
-        // Order with previous orderings
-        acc_permutation.conservativeResize(dim*(i+1));
-        acc_permutation.tail(dim) = ArrayXi::LinSpaced(dim,dim*i,dim*(i+1)-1);
-        PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_matrix(dim*(i+1));
-        acc_permutation_matrix.indices() = acc_permutation;
-        b_ordered = acc_permutation_matrix * b;
-        H_ordered = H.twistedBy(acc_permutation_matrix);
-
-        // ordering factors
-        ordering_factors.resize(dim*(i+1));
-        ordering_factors = ((H_ordered.rightCols(3) * MatrixXd::Ones(3,1)).array() == 0).select(VectorXi::Zero(dim*(i+1)),VectorXi::Ones(dim*(i+1)));
-
-        // variable ordering
-        t2 = clock();
-        H_ordered.makeCompressed();
-
-        PermutationMatrix<Dynamic, Dynamic, int> permutation_matrix(dim*(i+1));
-        ordering(H_ordered, permutation_matrix, ordering_factors.data());
-
-        // applying ordering
-        acc_permutation_matrix = permutation_matrix * acc_permutation_matrix;
-        acc_permutation = acc_permutation_matrix.indices();
-        b_ordered = permutation_matrix * b_ordered;
-        H_ordered = H_ordered.twistedBy(permutation_matrix);
-
-        // solve Hx = b
-        solver2.compute(H_ordered);
-        if (solver2.info() != Success)
-        {
-            std::cout << "decomposition failed" << std::endl;
-            return 0;
-        }
-        x_ordered = solver2.solve(b_ordered);
-        x_ordered = acc_permutation_matrix.inverse() * x_ordered;
-        time2 += ((double) clock() - t2) / CLOCKS_PER_SEC;
-
-        // SOLVING WITH BLOCK REORDERING ------------------------------------
-        // Order with previous orderings
-        acc_permutation_b.conservativeResize(dim*(i+1));
-        acc_permutation_b.tail(dim) = ArrayXi::LinSpaced(dim,dim*i,dim*(i+1)-1);
-        PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_b_matrix(dim*(i+1));
-        acc_permutation_b_matrix.indices() = acc_permutation_b;
-        b_b_ordered = acc_permutation_b_matrix * b;
-        H_b_ordered = H.twistedBy(acc_permutation_b_matrix);
-
-        acc_permutation_factors.conservativeResize(i+1);
-        acc_permutation_factors(i) = i;
-        PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_factors_matrix(dim*(i+1));
-        acc_permutation_factors_matrix.indices() = acc_permutation_factors;
-        factors_ordered = factors.twistedBy(acc_permutation_factors_matrix);
-
-        // ordering factors
-        factor_ordering_factors.resize(i);
-        factor_ordering_factors = factors_ordered.rightCols(1);
-
-        // block ordering
-        t3 = clock();
-        factors_ordered.makeCompressed();
-
-        PermutationMatrix<Dynamic, Dynamic, int> permutation_factors_matrix(i+1);
-        ordering(factors_ordered, permutation_factors_matrix, factor_ordering_factors.data());
-
-        // applying ordering
-        permutation_2_block_permutation(permutation_factors_matrix, permutation_matrix , dim, i+1);
-        acc_permutation_factors_matrix = permutation_factors_matrix * acc_permutation_factors_matrix;
-        acc_permutation_factors = acc_permutation_factors_matrix.indices();
-        acc_permutation_b_matrix = permutation_matrix * acc_permutation_b_matrix;
-        acc_permutation_b = acc_permutation_b_matrix.indices();
-        b_b_ordered = permutation_matrix * b_b_ordered;
-        H_b_ordered = H_b_ordered.twistedBy(permutation_matrix);
-
-        // solve Hx = b
-        solver3.compute(H_b_ordered);
-        if (solver3.info() != Success)
-        {
-            std::cout << "decomposition failed" << std::endl;
-            return 0;
-        }
-        x_b_ordered = solver3.solve(b_b_ordered);
-        x_b_ordered = acc_permutation_b_matrix.inverse() * x_b_ordered;
-        time3 += ((double) clock() - t3) / CLOCKS_PER_SEC;
-
-        // RESULTS ------------------------------------
-        std::cout << "========================= RESULTS " << i << ":" << std::endl;
-        std::cout << "NO REORDERING:    solved in " << time1*1e3 << " ms" << std::endl;
-        std::cout << "REORDERING:       solved in " << time2*1e3 << " ms" << std::endl;
-        std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl;
-        std::cout << "x1 = " << x.transpose() << std::endl;
-        std::cout << "x2 = " << x_ordered.transpose() << std::endl;
-        std::cout << "x3 = " << x_b_ordered.transpose() << std::endl;
-    }
-
-    // RESULTS ------------------------------------
-    std::cout << "NO REORDERING:    solved in " << time1*1e3 << " ms" << std::endl;
-    std::cout << "REORDERING:       solved in " << time2*1e3 << " ms" << std::endl;
-    std::cout << "BLOCK REORDERING: solved in " << time3*1e3 << " ms" << std::endl;
-
-        //std::cout << "x = " << x.transpose() << std::endl;
-        //std::cout << "x = " << x_ordered.transpose() << std::endl;
-        //std::cout << "x = " << x_b_ordered.transpose() << std::endl;
-}
-

demos/solver/test_permutations.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of 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/>.
-//
-//--------LICENSE_END--------
-/*
- * test_permutations.cpp
- *
- *  Created on: Jun 15, 2015
- *      Author: jvallve
- */
-
-//std includes
-#include <cstdlib>
-#include <iostream>
-#include <fstream>
-#include <memory>
-#include <random>
-#include <typeinfo>
-#include <ctime>
-#include <queue>
-
-// eigen includes
-#include <Eigen/OrderingMethods>
-
-using namespace Eigen;
-
-//main
-int main(int argc, char *argv[])
-{
-    PermutationMatrix<Dynamic, Dynamic, int> P1(5), P2(5), P3(5), P4(5);
-    P1.setIdentity();
-    P2.setIdentity();
-    P3.setIdentity();
-
-    VectorXd a = VectorXd::LinSpaced(5,1,5);
-    MatrixXd A= a.asDiagonal();
-    SparseMatrix<double> B = A.sparseView();
-    B.makeCompressed();
-
-    std::cout << "A (dense)" << std::endl << A << std::endl << std::endl;
-    std::cout << "B (sparse)" << std::endl << B << std::endl << std::endl;
-
-    P1.indices()(3) = 4;
-    P1.indices()(4) = 3;
-
-    std::cout << "Permutation 1" << std::endl << P1.indices().transpose() << std::endl << std::endl;
-
-    P2.indices()(0) = 4;
-    P2.indices()(4) = 0;
-
-    std::cout << "Permutation 2" << std::endl << P2.indices().transpose() << std::endl << std::endl;
-
-    std::cout << "Pre-multiplying: Permutating rows" << std::endl;
-    std::cout << "P1 * A" << std::endl << P1 * A << std::endl << std::endl;
-    std::cout << "P1 * B" << std::endl << P1 * B << std::endl << std::endl;
-    SparseMatrix<double> C = (P1 * B).sparseView();
-    std::cout << "(P1 * B).bottomRows(1)" << std::endl << C.bottomRows(1) << std::endl << std::endl;
-
-    std::cout << "Post-multiplying: Permutating cols" << std::endl;
-    std::cout << "A * P1.transpose()" << std::endl << A  * P1.transpose()<< std::endl << std::endl;
-    std::cout << "B * P1.transpose()" << std::endl << B  * P1.transpose()<< std::endl << std::endl;
-
-    std::cout << "Pre&post-multiplying:" << std::endl;
-    std::cout << "P1 * A * P1.transpose()" << std::endl << P1 * A * P1.transpose() << std::endl << std::endl;
-    std::cout << "P2 * P1 * A * P1.transpose() * P2.transpose()" << std::endl << P2 * P1 * A * P1.transpose() * P2.transpose() << std::endl << std::endl;
-    std::cout << "P1 * P2 * A * P2.transpose() * P1.transpose()" << std::endl << P1 * P2 * A * P2.transpose() * P1.transpose() << std::endl << std::endl;
-
-    P3 = P1 * P2;
-
-    std::cout << "Permutation P3 = P1 * P2" << std::endl << P3.indices().transpose() << std::endl << std::endl;
-    std::cout << "P3 * A * P3.transpose()" << std::endl << P3 * A  * P3.transpose() << std::endl << std::endl;
-
-    std::cout << "PERMUTATING INDICES" << std::endl;
-    ArrayXi acc_permutations(5);
-    acc_permutations << 0,1,2,3,4;
-
-    std::cout << "acc_permutations: " << acc_permutations.transpose() << std::endl;
-
-    std::cout << "P1: " << P1.indices().transpose() << std::endl;
-    std::cout << "P1 * acc_permutations: " << (P1 * acc_permutations.matrix()).transpose() << std::endl;
-    std::cout << "P1.inverse() * acc_permutations: " << (P1.inverse() * acc_permutations.matrix()).transpose() << std::endl;
-
-    std::cout << "P2: " << P2.indices().transpose() << std::endl;
-    std::cout << "P2 * (P1 * acc_permutations): " << (P2 * (P1 * acc_permutations.matrix())).transpose() << std::endl;
-    std::cout << "(P2 * P1).inverse() * acc_permutations): " << ((P2 * P1).inverse() * acc_permutations.matrix()).transpose() << std::endl;
-    P4 = P1 * P2 * P3;
-    std::cout << "Permutation P4 = P1 * P2 * P3:   " << P4.indices().transpose() << std::endl;
-    std::cout << "P3 * (P2 * (P1 * acc_permutations)): " << (P3 * (P2 * (P1 * acc_permutations.matrix()))).transpose() << std::endl;
-    std::cout << "accumulated permutations can not be stored in vectors..." << std::endl;
-
-    std::cout << std::endl << "PARTIALL PERMUTATIONS" << std::endl;
-    PermutationMatrix<Dynamic, Dynamic, int> P5(2);
-    P5.indices()(0) = 1;
-    P5.indices()(1) = 0;
-    std::cout << "P5 (equivalent to P1 but partiall): " << std::endl << P5.indices().transpose() << std::endl;
-    std::cout << "P2: " << P2.indices().transpose() << std::endl << std::endl;
-    std::cout << "A * P2.transpose(): " << std::endl << A * P2.transpose() << std::endl << std::endl;
-    std::cout << "(A * P2.transpose()).rightCols(2) * P5.transpose(): " << std::endl << (A * P2.transpose()).rightCols(2) * P5.transpose() << std::endl << std::endl;
-    PermutationMatrix<Dynamic, Dynamic, int> P6 = P2;
-    P6.indices().tail(2) = P5 * P6.indices().tail(2);
-    std::cout << "P6 = P2, P6.indices().tail(2) = P5 * P6.indices().tail(2): " << P6.indices().transpose() << std::endl << std::endl;
-    std::cout << "A * P6.transpose(): " << std::endl << A * P6.transpose() << std::endl << std::endl;
-    std::cout << "(P1 * P2): " << std::endl << (P1 * P2).indices().transpose() << std::endl << std::endl;
-    std::cout << "A * (P1 * P2).transpose(): " << std::endl << A * (P1 * P2).transpose() << std::endl << std::endl;
-    std::cout << "Partiall permutations can not be accumulated, for doing that they should be full size" << std::endl;
-
-    std::cout << std::endl << "PERMUTATION OF MAPPED VECTORS" << std::endl;
-    std::cout << "a = " << a.transpose() << std::endl << std::endl;
-    Map<VectorXd> mapped_a(a.data(), 1);
-    std::cout << "mapped_a1 = " << mapped_a << std::endl << std::endl;
-    a = P2 * a;
-    std::cout << "a = P2 * a: " << std::endl << a.transpose() << std::endl << std::endl;
-    std::cout << "mapped_a = " << mapped_a.transpose() << std::endl << std::endl;
-    std::cout << "maps are affected of the reorderings in mapped vectors" << std::endl;
-
-//    Map<>
-}

test/yaml/solver.yaml → demos/solver/yaml/solver_ceres.yaml

@@ -5,8 +5,8 @@ max_num_iterations: 1000
 n_threads: 2
 function_tolerance: 0.000001
 gradient_tolerance: 0.0000000001
-minimizer: "LEVENBERG_MARQUARDT"
-use_nonmonotonic_steps: false         # only for LEVENBERG_MARQUARDT and DOGLEG
+minimizer: "levenberg_marquardt"
+use_nonmonotonic_steps: false         # only for levenberg_marquardt and DOGLEG
 max_consecutive_nonmonotonic_steps: 5 # only if use_nonmonotonic_steps = true
 compute_cov: true
 cov_period: 1                         #only if compute_cov

include/core/capture/capture_base.h

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-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
+// 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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-#ifndef CAPTURE_BASE_H_
-#define CAPTURE_BASE_H_
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+#pragma once
 
 // Forward declarations for node templates
-namespace wolf{
+namespace wolf
+{
 class FrameBase;
 class FeatureBase;
-}
+}  // namespace wolf
 
-//Wolf includes
+// Wolf includes
 #include "core/common/wolf.h"
-#include "core/common/node_base.h"
+#include "core/common/node_state_blocks.h"
 #include "core/common/time_stamp.h"
-#include "core/state_block/has_state_blocks.h"
 
-//std includes
+// std includes
 
-namespace wolf{
-
-//class CaptureBase
-class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<CaptureBase>
+namespace wolf
+{
+// class CaptureBase
+class CaptureBase : public NodeStateBlocks
 {
-    friend FeatureBase;
-    friend FactorBase;
-    friend FrameBase;
-
-    private:
-        FrameBaseWPtr   frame_ptr_;
-        FeatureBasePtrList feature_list_;
-        FactorBasePtrList constrained_by_list_;
-        SensorBaseWPtr  sensor_ptr_; ///< Pointer to sensor
-
-        static unsigned int capture_id_count_;
-
-    protected:
-        unsigned int capture_id_;
-        TimeStamp time_stamp_;      ///< Time stamp
-        void setProblem(ProblemPtr _problem) override final;
-
-    public:
-
-        CaptureBase(const std::string& _type,
-                    const TimeStamp& _ts,
-                    SensorBasePtr _sensor_ptr   = nullptr,
-                    StateBlockPtr _p_ptr        = nullptr,
-                    StateBlockPtr _o_ptr        = nullptr,
-                    StateBlockPtr _intr_ptr     = nullptr);
-
-        ~CaptureBase() override;
-        virtual void remove(bool viral_remove_empty_parent=false);
-
-        // Type
-        virtual bool isMotion() const { return false; }
-
-        bool process();
-
-        unsigned int id() const;
-        TimeStamp getTimeStamp() const;
-        void setTimeStamp(const TimeStamp& _ts);
-        void setTimeStampToNow();
-
-        FrameBaseConstPtr getFrame() const;
-        FrameBasePtr getFrame();
-    private:
-        void setFrame(const FrameBasePtr _frm_ptr);
-
-    public:
-        FeatureBaseConstPtrList getFeatureList() const;
-        FeatureBasePtrList getFeatureList();
-        bool hasFeature(const FeatureBaseConstPtr &_feature) const;
-        
-        FactorBaseConstPtrList getFactorList() const;
-        FactorBasePtrList getFactorList();
-        void getFactorList(FactorBaseConstPtrList& _fac_list) const;
-        void getFactorList(FactorBasePtrList& _fac_list);
-
-        SensorBaseConstPtr getSensor() const;
-        SensorBasePtr getSensor();
-        virtual void setSensor(const SensorBasePtr sensor_ptr);
-
-        // constrained by
-    private:
-        virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
-        virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
-
-    public:
-        unsigned int getHits() const;
-        FactorBaseConstPtrList getConstrainedByList() const;
-        FactorBasePtrList getConstrainedByList();
-        bool isConstrainedBy(const FactorBaseConstPtr &_factor) const;
-
-        // State blocks
-        StateBlockConstPtr  getStateBlock(const char& _key) const;
-        StateBlockPtr       getStateBlock(const char& _key);
-        StateBlockConstPtr  getSensorP() const;
-        StateBlockPtr       getSensorP();
-        StateBlockConstPtr  getSensorO() const;
-        StateBlockPtr       getSensorO();
-        StateBlockConstPtr  getSensorIntrinsic() const;
-        StateBlockPtr       getSensorIntrinsic();
-
-        void fix() override;
-        void unfix() override;
-
-        void move(FrameBasePtr);
-        void link(FrameBasePtr);
-        void unlink();
-
-        template<typename classType, typename... T>
-        static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
-
-        virtual void printHeader(int depth, //
-                                 bool constr_by, //
-                                 bool metric, //
-                                 bool state_blocks,
-                                 std::ostream& stream ,
-                                 std::string _tabs = "") const;
-
-        void print(int depth, //
-                   bool constr_by, //
-                   bool metric, //
-                   bool state_blocks,
-                   std::ostream& stream = std::cout,
-                   std::string _tabs = "") const;
-
-        virtual CheckLog localCheck(bool _verbose, CaptureBaseConstPtr _cap_ptr, std::ostream& _stream, std::string _tabs = "") const;
-        bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
-
-    private:
-        FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
-        void removeFeature(FeatureBasePtr _ft_ptr);
+    friend class FeatureBase;
+    friend class FactorBase;
+    friend class FrameBase;
+
+  private:
+    FrameBaseWPtr      frame_ptr_;
+    FeatureBasePtrList feature_list_;
+    SensorBaseWPtr     sensor_ptr_;  ///< Pointer to sensor
+
+    static unsigned int capture_id_count_;
+
+  protected:
+    unsigned int capture_id_;
+    TimeStamp    time_stamp_;  ///< Time stamp
+    void         setProblem(ProblemPtr _problem) override final;
+
+  public:
+    /** \brief Constructor
+     *
+     * \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: derived type name
+     * \param _ts TimeStamp of the capture.
+     * \param _sensor_ptr Pointer to the sensor that produced the capture. If unknown: nullptr.
+     * \param _state_types Composite of types of the states that the capture can have. Probably to be hardcoded in the
+     * derived class constructor.
+     * \param _state_vectors Composite of vectors of the states of the capture. A 'StateBlock' for each key in
+     * '_state_vectors' will be emplaced (of the type specified in '_state_types'). Not all keys of '_state_types' are
+     * required to be specified, but all keys of '_state_vectors' should be in '_state_types'.
+     * \param _state_priors Composite of priors (initial_guess, factor or fixed) of the states of the capture. Not all
+     * keys of '_state_vectors' are required to be specified, but all keys of '_state_priors' should be in
+     * '_state_vectors'.
+     *
+     **/
+    CaptureBase(const std::string&     _type,
+                const TimeStamp&       _ts,
+                SensorBasePtr          _sensor_ptr    = nullptr,
+                const TypeComposite&   _state_types   = {},
+                const VectorComposite& _state_vectors = {},
+                const PriorComposite&  _state_priors  = {});
+
+    ~CaptureBase() override = default;
+
+    /** \brief Remove the capture
+     *
+     * \param viral_remove_parent_without_children If true, removes the parent if it has no other children.
+     */
+    void remove(bool viral_remove_parent_without_children = false) override;
+
+    /** \brief Check if the capture has any children
+     *
+     * \return true if the capture has children, false otherwise
+     */
+    bool hasChildren() const override;
+
+    /** \brief Emplace a FactorBlockDifference with zero difference between the state _key of this capture and
+     * _capture_origin
+     *
+     * \param _capture_origin The origin capture to add the factor drift.
+     * \param _key The key of the state to be factored.
+     * \param _apply_loss_function Whether to apply a loss function to the factor.
+     */
+    virtual FactorBasePtr emplaceDriftFactor(CaptureBasePtr _capture_origin, char _key, bool _apply_loss_function);
+
+    /**
+     * \brief Is a motion if there is motion detected.
+     *
+     * \return true if the capture is of type motion, false otherwise.
+     */
+    virtual bool isMotion() const;
+
+    /** \brief Process the capture
+     *
+     * \return true if the process is successful, false otherwise
+     */
+    bool process();
+
+    unsigned int id() const override;
+    TimeStamp    getTimeStamp() const;
+    void         setTimeStamp(const TimeStamp& _ts);
+    void         setTimeStampToNow();
+
+    FrameBaseConstPtr getFrame() const;
+    FrameBasePtr      getFrame();
+
+  private:
+    void setFrame(const FrameBasePtr _frm_ptr);
+
+  public:
+    FeatureBaseConstPtrList getFeatureList() const;
+    FeatureBasePtrList      getFeatureList();
+    bool                    hasFeature(const FeatureBaseConstPtr& _feature) const;
+
+    FactorBaseConstPtrList getFactorList() const;
+    FactorBasePtrList      getFactorList();
+    void                   getFactorList(FactorBaseConstPtrList& _fac_list) const;
+    void                   getFactorList(FactorBasePtrList& _fac_list);
+
+    SensorBaseConstPtr getSensor() const;
+    SensorBasePtr      getSensor();
+    virtual void       setSensor(const SensorBasePtr sensor_ptr);
+
+  public:
+    // State blocks
+    StateBlockConstPtr getStateBlock(const char& _key) const override;
+    StateBlockPtr      getStateBlock(const char& _key) override;
+    StateBlockConstPtr getSensorP() const;
+    StateBlockPtr      getSensorP();
+    StateBlockConstPtr getSensorO() const;
+    StateBlockPtr      getSensorO();
+    StateBlockConstPtr getSensorIntrinsic() const;
+    StateBlockPtr      getSensorIntrinsic();
+
+    void fix() override;
+    void unfix() override;
+
+    /** \brief Move the capture to a new frame
+     *
+     * \param _frm_ptr Pointer to the new frame.
+     */
+    void move(FrameBasePtr _frm_ptr);
+
+    /** \brief Link the capture to a frame
+     *
+     * \param _frm_ptr Pointer to the frame.
+     */
+    void link(FrameBasePtr _frm_ptr);
+
+    /** \brief Unlink the capture from its current frame
+     */
+    void unlink();
+
+    /** \brief Emplace a new capture of type classType
+     *
+     * \param _frm_ptr Pointer to the frame to link the new capture to.
+     * \param all Variadic template arguments to forward to the constructor of classType.
+     * \return A shared pointer to the newly created capture.
+     */
+    template <typename classType, typename... T>
+    static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
+
+    /** \brief Print the header information of the capture
+     *
+     * \param depth The depth of the header.
+     * \param factored_by Whether to include factors in the print.
+     * \param metric Whether to print metric information.
+     * \param state_blocks Whether to print state blocks information.
+     * \param stream The output stream to print to.
+     * \param _tabs The tabs to use for indentation.
+     */
+    virtual void printHeader(int           depth,
+                             bool          factored_by,
+                             bool          metric,
+                             bool          state_blocks,
+                             std::ostream& stream,
+                             std::string   _tabs = "") const;
+
+    /** \brief Print the capture details
+     *
+     * \param depth The depth of the print.
+     * \param factored_by Whether to include factors in the print.
+     * \param metric Whether to include metric information.
+     * \param state_blocks Whether to include state blocks.
+     * \param stream The output stream to print to.
+     * \param _tabs The tabs to use for indentation.
+     */
+    void print(int           depth,
+               bool          factored_by,
+               bool          metric,
+               bool          state_blocks,
+               std::ostream& stream = std::cout,
+               std::string   _tabs  = "") const;
+
+    /** \brief Perform a local check on the capture
+     *
+     * \param _verbose Whether to print verbose output.
+     * \param _stream The output stream to print to.
+     * \param _tabs The tabs to use for indentation.
+     * \return A CheckLog object containing the results of the check.
+     */
+    virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
+    /** \brief Check the capture
+     *
+     * \param _log The log to store check results.
+     * \param _verbose Whether to enable verbose output.
+     * \param _stream The output stream to print to.
+     * \param _tabs The tabs to use for indentation.
+     * \return true if the check is successful, false otherwise
+     */
+    bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
+  private:
+    FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
+    void           removeFeature(FeatureBasePtr _ft_ptr);
+
+  public:
+    CaptureBasePtr      shared_from_this_capture();
+    CaptureBaseConstPtr shared_from_this_capture() const;
 };
 
-}
+}  // namespace wolf
 
 #include "core/sensor/sensor_base.h"
 #include "core/frame/frame_base.h"
 #include "core/feature/feature_base.h"
 #include "core/state_block/state_block.h"
 
-namespace wolf{
+namespace wolf
+{
+inline bool CaptureBase::isMotion() const
+{
+    return false;
+}
 
-template<typename classType, typename... T>
+template <typename classType, typename... T>
 std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... all)
 {
     std::shared_ptr<classType> cpt = std::make_shared<classType>(std::forward<T>(all)...);
+    cpt->emplacePriors();
     cpt->link(_frm_ptr);
     return cpt;
 }
@@ -206,14 +296,18 @@ inline unsigned int CaptureBase::id() const
 
 inline FrameBaseConstPtr CaptureBase::getFrame() const
 {
-    if(frame_ptr_.expired()) return nullptr;
-    else return frame_ptr_.lock();
+    if (frame_ptr_.expired())
+        return nullptr;
+    else
+        return frame_ptr_.lock();
 }
 
 inline FrameBasePtr CaptureBase::getFrame()
 {
-    if(frame_ptr_.expired()) return nullptr;
-    else return frame_ptr_.lock();
+    if (frame_ptr_.expired())
+        return nullptr;
+    else
+        return frame_ptr_.lock();
 }
 
 inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
@@ -224,8 +318,7 @@ inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
 inline FeatureBaseConstPtrList CaptureBase::getFeatureList() const
 {
     FeatureBaseConstPtrList list_const;
-    for (auto&& obj_ptr : feature_list_)
-        list_const.push_back(obj_ptr);
+    for (auto&& obj_ptr : feature_list_) list_const.push_back(obj_ptr);
     return list_const;
 }
 
@@ -234,24 +327,6 @@ inline FeatureBasePtrList CaptureBase::getFeatureList()
     return feature_list_;
 }
 
-inline unsigned int CaptureBase::getHits() const
-{
-    return constrained_by_list_.size();
-}
-
-inline FactorBaseConstPtrList CaptureBase::getConstrainedByList() const
-{
-    FactorBaseConstPtrList list_const;
-    for (auto&& obj_ptr : constrained_by_list_)
-        list_const.push_back(obj_ptr);
-    return list_const;
-}
-
-inline FactorBasePtrList CaptureBase::getConstrainedByList()
-{
-    return constrained_by_list_;
-}
-
 inline TimeStamp CaptureBase::getTimeStamp() const
 {
     return time_stamp_;
@@ -269,7 +344,7 @@ inline SensorBasePtr CaptureBase::getSensor()
 
 inline void CaptureBase::setSensor(const SensorBasePtr sensor_ptr)
 {
-  sensor_ptr_ = sensor_ptr;
+    sensor_ptr_ = sensor_ptr;
 }
 
 inline void CaptureBase::setTimeStamp(const TimeStamp& _ts)
@@ -282,6 +357,14 @@ inline void CaptureBase::setTimeStampToNow()
     time_stamp_.setToNow();
 }
 
-} // namespace wolf
+inline CaptureBasePtr CaptureBase::shared_from_this_capture()
+{
+    return std::static_pointer_cast<CaptureBase>(shared_from_this());
+}
+
+inline CaptureBaseConstPtr CaptureBase::shared_from_this_capture() const
+{
+    return std::static_pointer_cast<const CaptureBase>(shared_from_this());
+}
 
-#endif
+}  // namespace wolf

include/core/capture/capture_diff_drive.h

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-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
+// 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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/**
- * \file diff_drive_tools.h
- *
- *  Created on: Oct 20, 2016
- *  \author: Jeremie Deray
- */
-
-#ifndef CAPTURE_DIFF_DRIVE_H_
-#define CAPTURE_DIFF_DRIVE_H_
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+#pragma once
 
-//wolf includes
+// wolf includes
 #include "core/capture/capture_motion.h"
 
-namespace wolf {
-
+namespace wolf
+{
 WOLF_PTR_TYPEDEFS(CaptureDiffDrive)
 
-/**
- * @brief The CaptureWheelJointPosition class
- *
- * Represents a list of wheel positions in radian.
- */
 class CaptureDiffDrive : public CaptureMotion
 {
-
-public:
-
-        /**
-         * \brief Constructor
-         **/
-        CaptureDiffDrive(const TimeStamp& _ts,
-                         const SensorBasePtr& _sensor_ptr,
-                         const Eigen::VectorXd& _data,
-                         const Eigen::MatrixXd& _data_cov,
-                         CaptureBasePtr _capture_origin_ptr,
-                         StateBlockPtr _p_ptr = nullptr,
-                         StateBlockPtr _o_ptr = nullptr,
-                         StateBlockPtr _intr_ptr = nullptr);
-
-        ~CaptureDiffDrive() override = default;
+  public:
+    /**
+     * \brief Constructor
+     **/
+    CaptureDiffDrive(const TimeStamp&       _ts,
+                     const SensorBasePtr&   _sensor_ptr,
+                     const Eigen::VectorXd& _data,
+                     const Eigen::MatrixXd& _data_cov,
+                     CaptureBasePtr         _capture_origin_ptr,
+                     const VectorComposite& _state_vectors = {},
+                     const PriorComposite&  _state_priors  = {});
+
+    ~CaptureDiffDrive() override = default;
 };
 
-} // namespace wolf
-
-#endif /* CAPTURE_DIFF_DRIVE_H_ */
+}  // namespace wolf

include/core/capture/capture_landmarks_external.h

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-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
+// 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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 #pragma once
 
 // Wolf includes
@@ -26,11 +23,10 @@
 
 namespace wolf
 {
-
 struct LandmarkDetection
 {
-    int id;                      // id of landmark
-    int type;                   // type of landmark
+    int             id;          // id of landmark
+    int             type;        // type of landmark
     Eigen::VectorXd measure;     // either pose or position
     Eigen::MatrixXd covariance;  // covariance of the measure
     double          quality;     // [0, 1] quality of the detection

include/core/capture/capture_motion.h

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-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
+// 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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/**
- * \file capture_motion.h
- *
- *  Created on: Mar 16, 2016
- *      \author: jsola
- */
-
-#ifndef SRC_CAPTURE_MOTION_H_
-#define SRC_CAPTURE_MOTION_H_
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+#pragma once
 
 // Wolf includes
 #include "core/capture/capture_base.h"
@@ -39,10 +28,9 @@
 #include <iterator>
 #include <utility>
 
-namespace wolf {
-    
+namespace wolf
+{
 WOLF_PTR_TYPEDEFS(CaptureMotion);
-    
 
 /** \brief Base class for motion Captures.
  *
@@ -58,79 +46,83 @@ WOLF_PTR_TYPEDEFS(CaptureMotion);
  *  - until the frame of this capture.
  *
  * Once a keyframe is generated, this buffer is frozen and kept in the Capture for eventual later uses.
- * It is then used to compute the factor that links the Frame of this capture to the previous key-frame in the Trajectory.
+ * It is then used to compute the factor that links the Frame of this capture to the previous key-frame in the
+ * Trajectory.
  */
 
 class CaptureMotion : public CaptureBase
 {
-
-        // public interface:
-
-    public:
-        CaptureMotion(const std::string & _type,
-                      const TimeStamp& _ts,
-                      SensorBasePtr _sensor_ptr,
-                      const Eigen::VectorXd& _data,
-                      CaptureBasePtr _capture_origin_ptr);
-
-        CaptureMotion(const std::string & _type,
-                      const TimeStamp& _ts,
-                      SensorBasePtr _sensor_ptr,
-                      const Eigen::VectorXd& _data,
-                      const Eigen::MatrixXd& _data_cov,
-                      CaptureBasePtr _capture_origin_ptr,
-                      StateBlockPtr _p_ptr = nullptr,
-                      StateBlockPtr _o_ptr = nullptr,
-                      StateBlockPtr _intr_ptr = nullptr);
-
-        ~CaptureMotion() override;
-
-        // Type
-        bool isMotion() const override { return true; }
-
-        // Data
-        const Eigen::VectorXd& getData() const;
-        const Eigen::MatrixXd& getDataCovariance() const;
-        void setData(const Eigen::VectorXd& _data);
-        void setDataCovariance(const Eigen::MatrixXd& _data_cov);
-
-        // Buffer
-        MotionBuffer& getBuffer();
-        const MotionBuffer& getBuffer() const;
-
-        // Buffer's initial conditions for pre-integration
-        VectorXd getCalibrationPreint() const;
-        void setCalibrationPreint(const VectorXd& _calib_preint);
-        MatrixXd getJacobianCalib() const;
-        MatrixXd getJacobianCalib(const TimeStamp& _ts) const;
-
-        // Get delta preintegrated, and corrected for changes on calibration params
-        VectorXd getDeltaPreint() const;
-        VectorXd getDeltaPreint(const TimeStamp& _ts) const;
-        MatrixXd getDeltaPreintCov() const;
-        MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const;
-
-        // Origin frame and capture
-        CaptureBasePtr getOriginCapture();
-        CaptureBaseConstPtr getOriginCapture() const;
-        void setOriginCapture(CaptureBasePtr _capture_origin_ptr);
-
-        bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const;
-
-        void printHeader(int depth, //
-                         bool constr_by, //
-                         bool metric, //
-                         bool state_blocks,
-                         std::ostream& stream ,
-                         std::string _tabs = "") const override;
-
-        // member data:
-    private:
-        Eigen::VectorXd data_;              ///< Motion data in form of vector mandatory
-        Eigen::MatrixXd data_cov_;          ///< Motion data covariance
-        Eigen::VectorXd calib_preint_;      ///< Calibration parameters used during pre-integration
-        MotionBuffer buffer_;               ///< Buffer of motions between this Capture and the next one.
-        CaptureBaseWPtr capture_origin_ptr_;    ///< Pointer to the origin capture of the motion
+  public:
+    CaptureMotion(const std::string&     _type,
+                  const TimeStamp&       _ts,
+                  SensorBasePtr          _sensor_ptr,
+                  const Eigen::VectorXd& _data,
+                  CaptureBasePtr         _capture_origin_ptr,
+                  const TypeComposite&   _state_types   = {},
+                  const VectorComposite& _state_vectors = {},
+                  const PriorComposite&  _state_priors  = {});
+
+    CaptureMotion(const std::string&     _type,
+                  const TimeStamp&       _ts,
+                  SensorBasePtr          _sensor_ptr,
+                  const Eigen::VectorXd& _data,
+                  const Eigen::MatrixXd& _data_cov,
+                  CaptureBasePtr         _capture_origin_ptr,
+                  const TypeComposite&   _state_types   = {},
+                  const VectorComposite& _state_vectors = {},
+                  const PriorComposite&  _state_priors  = {});
+
+    ~CaptureMotion() override;
+
+    // Type
+    bool isMotion() const override
+    {
+        return true;
+    }
+
+    // Data
+    const Eigen::VectorXd& getData() const;
+    const Eigen::MatrixXd& getDataCovariance() const;
+    void                   setData(const Eigen::VectorXd& _data);
+    void                   setDataCovariance(const Eigen::MatrixXd& _data_cov);
+
+    // Buffer
+    MotionBuffer&       getBuffer();
+    const MotionBuffer& getBuffer() const;
+
+    // Buffer's initial conditions for pre-integration
+    VectorXd getCalibrationPreint() const;
+    void     setCalibrationPreint(const VectorXd& _calib_preint);
+    MatrixXd getJacobianCalib() const;
+    MatrixXd getJacobianCalib(const TimeStamp& _ts) const;
+
+    // Get delta preintegrated, and corrected for changes on calibration params
+    VectorXd getDeltaPreint() const;
+    VectorXd getDeltaPreint(const TimeStamp& _ts) const;
+    MatrixXd getDeltaPreintCov() const;
+    MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const;
+
+    // Origin frame and capture
+    CaptureBasePtr      getOriginCapture();
+    CaptureBaseConstPtr getOriginCapture() const;
+    void                setOriginCapture(CaptureBasePtr _capture_origin_ptr);
+
+    bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const;
+
+    void printHeader(int           depth,        //
+                     bool          factored_by,  //
+                     bool          metric,       //
+                     bool          state_blocks,
+                     std::ostream& stream,
+                     std::string   _tabs = "") const override;
+
+    // member data:
+  private:
+    Eigen::VectorXd data_;                ///< Motion data in form of vector mandatory
+    Eigen::MatrixXd data_cov_;            ///< Motion data covariance
+    Eigen::VectorXd calib_preint_;        ///< Calibration parameters used during pre-integration
+    MotionBuffer    buffer_;              ///< Buffer of motions between this Capture and the next one.
+    CaptureBaseWPtr capture_origin_ptr_;  ///< Pointer to the origin capture of the motion
 };
 
 inline const Eigen::VectorXd& CaptureMotion::getData() const
@@ -221,6 +213,4 @@ inline MatrixXd CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
     return getBuffer().getMotion(_ts).delta_integr_cov_;
 }
 
-} // namespace wolf
-
-#endif /* SRC_CAPTURE_MOTION_H_ */
+}  // namespace wolf

include/core/capture/capture_odom_2d.h

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-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
+// 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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/*
- * capture_odom_2d.h
- *
- *  Created on: Oct 16, 2017
- *      Author: jsola
- */
-
-#ifndef CAPTURE_ODOM_2d_H_
-#define CAPTURE_ODOM_2d_H_
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+#pragma once
 
 #include "core/capture/capture_motion.h"
 
@@ -35,27 +24,23 @@
 
 namespace wolf
 {
-
 WOLF_PTR_TYPEDEFS(CaptureOdom2d);
 
 class CaptureOdom2d : public CaptureMotion
 {
-    public:
-        CaptureOdom2d(const TimeStamp& _init_ts,
-                      SensorBasePtr _sensor_ptr,
-                      const Eigen::VectorXd& _data,
-                      CaptureBasePtr _capture_origin_ptr = nullptr);
-
-        CaptureOdom2d(const TimeStamp& _init_ts,
-                      SensorBasePtr _sensor_ptr,
-                      const Eigen::VectorXd& _data,
-                      const Eigen::MatrixXd& _data_cov,
-                      CaptureBasePtr _capture_origin_ptr = nullptr);
-
-        ~CaptureOdom2d() override;
-
+  public:
+    CaptureOdom2d(const TimeStamp&       _init_ts,
+                  SensorBasePtr          _sensor_ptr,
+                  const Eigen::VectorXd& _data,
+                  CaptureBasePtr         _capture_origin_ptr = nullptr);
+
+    CaptureOdom2d(const TimeStamp&       _init_ts,
+                  SensorBasePtr          _sensor_ptr,
+                  const Eigen::VectorXd& _data,
+                  const Eigen::MatrixXd& _data_cov,
+                  CaptureBasePtr         _capture_origin_ptr = nullptr);
+
+    ~CaptureOdom2d() override;
 };
 
 } /* namespace wolf */
-
-#endif /* CAPTURE_ODOM_2d_H_ */

include/core/capture/capture_odom_3d.h

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-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
+// 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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/*
- * capture_odom_3d.h
- *
- *  Created on: Oct 16, 2017
- *      Author: jsola
- */
-
-#ifndef CAPTURE_ODOM_3d_H_
-#define CAPTURE_ODOM_3d_H_
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+#pragma once
 
 #include "core/capture/capture_motion.h"
 
@@ -35,27 +24,23 @@
 
 namespace wolf
 {
-
 WOLF_PTR_TYPEDEFS(CaptureOdom3d);
 
 class CaptureOdom3d : public CaptureMotion
 {
-    public:
-        CaptureOdom3d(const TimeStamp& _init_ts,
-                      SensorBasePtr _sensor_ptr,
-                      const Eigen::VectorXd& _data,
-                      CaptureBasePtr _capture_origin_ptr = nullptr);
-
-        CaptureOdom3d(const TimeStamp& _init_ts,
-                      SensorBasePtr _sensor_ptr,
-                      const Eigen::VectorXd& _data,
-                      const Eigen::MatrixXd& _data_cov,
-                      CaptureBasePtr _capture_origin_ptr = nullptr);
-
-        ~CaptureOdom3d() override;
-
+  public:
+    CaptureOdom3d(const TimeStamp&       _init_ts,
+                  SensorBasePtr          _sensor_ptr,
+                  const Eigen::VectorXd& _data,
+                  CaptureBasePtr         _capture_origin_ptr = nullptr);
+
+    CaptureOdom3d(const TimeStamp&       _init_ts,
+                  SensorBasePtr          _sensor_ptr,
+                  const Eigen::VectorXd& _data,
+                  const Eigen::MatrixXd& _data_cov,
+                  CaptureBasePtr         _capture_origin_ptr = nullptr);
+
+    ~CaptureOdom3d() override;
 };
 
 } /* namespace wolf */
-
-#endif /* CAPTURE_ODOM_3d_H_ */

include/core/capture/capture_pose.h

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-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
+// 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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-#ifndef CAPTURE_POSE_H_
-#define CAPTURE_POSE_H_
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+#pragma once
 
-//Wolf includes
+// Wolf includes
 #include "core/capture/capture_base.h"
 #include "core/factor/factor_pose_2d.h"
 #include "core/factor/factor_pose_3d.h"
 #include "core/feature/feature_pose.h"
 
-//std includes
+// std includes
 //
 
-namespace wolf {
-
+namespace wolf
+{
 WOLF_PTR_TYPEDEFS(CapturePose);
 
-//class CapturePose
+// class CapturePose
 class CapturePose : public CaptureBase
 {
-    protected:
-        Eigen::VectorXd data_; ///< Raw data.
-        Eigen::MatrixXd data_covariance_; ///< Noise of the capture.
-
-    public:
-
-        CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_covariance);
-
-        ~CapturePose() override;
-
-        virtual void emplaceFeatureAndFactor();
-        Eigen::VectorXd getData(){    return data_;}
-        Eigen::MatrixXd getDataCovariance(){ return data_covariance_;}
-
+  protected:
+    Eigen::VectorXd data_;             ///< Raw data.
+    Eigen::MatrixXd data_covariance_;  ///< Noise of the capture.
+
+  public:
+    CapturePose(const TimeStamp&       _ts,
+                SensorBasePtr          _sensor_ptr,
+                const Eigen::VectorXd& _data,
+                const Eigen::MatrixXd& _data_covariance);
+
+    ~CapturePose() override;
+
+    virtual void    emplaceFeatureAndFactor(const ProcessorBasePtr& _processor           = nullptr,
+                                            bool                    _apply_loss_function = false);
+    Eigen::VectorXd getData() const
+    {
+        return data_;
+    }
+    Eigen::MatrixXd getDataCovariance() const
+    {
+        return data_covariance_;
+    }
+    void setData(const Eigen::VectorXd& _data);
+    void setDataCovariance(const Eigen::MatrixXd& _data_covariance);
 };
 
-} //namespace wolf
-#endif
+}  // namespace wolf

include/core/capture/capture_void.h

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-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
+// 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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-#ifndef CAPTURE_VOID_H_
-#define CAPTURE_VOID_H_
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+#pragma once
 
-//Wolf includes
+// Wolf includes
 #include "core/capture/capture_base.h"
 
-namespace wolf {
-
+namespace wolf
+{
 WOLF_PTR_TYPEDEFS(CaptureVoid);
-   
-    
-//class CaptureVoid
+
+// class CaptureVoid
 class CaptureVoid : public CaptureBase
 {
-    public:
-        CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr);
-        ~CaptureVoid() override;
-
+  public:
+    CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr);
+    ~CaptureVoid() override;
 };
 
-} // namespace wolf
-
-#endif
+}  // namespace wolf