Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/wolf
1 result
Show changes
Showing
with 925 additions and 2801 deletions
// 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;
}
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
...@@ -5,8 +5,8 @@ max_num_iterations: 1000 ...@@ -5,8 +5,8 @@ max_num_iterations: 1000
n_threads: 2 n_threads: 2
function_tolerance: 0.000001 function_tolerance: 0.000001
gradient_tolerance: 0.0000000001 gradient_tolerance: 0.0000000001
minimizer: "LEVENBERG_MARQUARDT" minimizer: "levenberg_marquardt"
use_nonmonotonic_steps: false # only for LEVENBERG_MARQUARDT and DOGLEG use_nonmonotonic_steps: false # only for levenberg_marquardt and DOGLEG
max_consecutive_nonmonotonic_steps: 5 # only if use_nonmonotonic_steps = true max_consecutive_nonmonotonic_steps: 5 # only if use_nonmonotonic_steps = true
compute_cov: true compute_cov: true
cov_period: 1 #only if compute_cov cov_period: 1 #only if compute_cov
......
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // 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 // 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 // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU 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-------- // You should have received a copy of the GNU General Public License
/** // along with this program. If not, see <http://www.gnu.org/licenses/>.
* \file diff_drive_tools.h #pragma once
*
* Created on: Oct 20, 2016
* \author: Jeremie Deray
*/
#ifndef CAPTURE_DIFF_DRIVE_H_
#define CAPTURE_DIFF_DRIVE_H_
//wolf includes // wolf includes
#include "core/capture/capture_motion.h" #include "core/capture/capture_motion.h"
namespace wolf { namespace wolf
{
WOLF_PTR_TYPEDEFS(CaptureDiffDrive) WOLF_PTR_TYPEDEFS(CaptureDiffDrive)
/**
* @brief The CaptureWheelJointPosition class
*
* Represents a list of wheel positions in radian.
*/
class CaptureDiffDrive : public CaptureMotion class CaptureDiffDrive : public CaptureMotion
{ {
public:
public: /**
* \brief Constructor
/** **/
* \brief Constructor CaptureDiffDrive(const TimeStamp& _ts,
**/ const SensorBasePtr& _sensor_ptr,
CaptureDiffDrive(const TimeStamp& _ts, const Eigen::VectorXd& _data,
const SensorBasePtr& _sensor_ptr, const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _data, CaptureBasePtr _capture_origin_ptr,
const Eigen::MatrixXd& _data_cov, const VectorComposite& _state_vectors = {},
CaptureBasePtr _capture_origin_ptr, const PriorComposite& _state_priors = {});
StateBlockPtr _p_ptr = nullptr,
StateBlockPtr _o_ptr = nullptr, ~CaptureDiffDrive() override = default;
StateBlockPtr _intr_ptr = nullptr);
~CaptureDiffDrive() override = default;
}; };
} // namespace wolf } // namespace wolf
#endif /* CAPTURE_DIFF_DRIVE_H_ */