Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
base_optimizer.hpp
Go to the documentation of this file.
1#pragma once
2
3#include <gtsam/nonlinear/NonlinearFactorGraph.h>
4#include <gtsam/nonlinear/Values.h>
5
7
14protected:
16
17public:
18 explicit BaseOptimizer(const SLAMParameters& params) : _params_(params){};
19 BaseOptimizer(const BaseOptimizer& other) : _params_(other._params_){};
21 if (this == &other) return *this; // Prevent self-assignment
22
23 // Note: _params_ is a reference, so we do not copy it
24 return *this;
25 }
26
34 virtual std::shared_ptr<BaseOptimizer> clone() const = 0;
35
36 virtual ~BaseOptimizer() = default;
37
50 virtual gtsam::Values optimize(gtsam::NonlinearFactorGraph& factor_graph,
51 gtsam::Values& graph_values,
52 [[maybe_unused]] unsigned int pose_num,
53 [[maybe_unused]] unsigned int landmark_num) = 0;
54};
Base class for graph optimizers.
BaseOptimizer & operator=(const BaseOptimizer &other)
virtual ~BaseOptimizer()=default
BaseOptimizer(const BaseOptimizer &other)
BaseOptimizer(const SLAMParameters &params)
const SLAMParameters & _params_
virtual std::shared_ptr< BaseOptimizer > clone() const =0
Clone the optimizer.
virtual gtsam::Values optimize(gtsam::NonlinearFactorGraph &factor_graph, gtsam::Values &graph_values, unsigned int pose_num, unsigned int landmark_num)=0
Optimize the graph.
Parameters for the SLAM node.