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;
+}

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_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