From 9838b16a092e5e5448425ab1e9ffdfdf1817aebc Mon Sep 17 00:00:00 2001 From: Gonzalo Ferrer Date: Sun, 28 Feb 2021 19:24:56 +0300 Subject: [PATCH] Adding get_adjacency and help comments --- mrobpy/FGraphPy.cpp | 51 +++++++++++++++++++------- src/FGraph/factor_graph_solve.cpp | 4 +- src/FGraph/mrob/factor_graph_solve.hpp | 19 +++++++++- 3 files changed, 57 insertions(+), 17 deletions(-) diff --git a/mrobpy/FGraphPy.cpp b/mrobpy/FGraphPy.cpp index 63e67a45..265da87e 100644 --- a/mrobpy/FGraphPy.cpp +++ b/mrobpy/FGraphPy.cpp @@ -206,16 +206,35 @@ void init_FGraph(py::module &m) .def(py::init<>(), "Constructor, solveType default is ADJ and GN.") .def("solve", &FGraphSolve::solve, - "Solves the corresponding FG", + "Solves the corresponding FG.\n" + "Options:\n method = mrob.GN (Gauss Newton), by default option. It carries out a SINGLE iteration.\n" + " = mrob.LM (Levenberg-Marquard), it has several parameters:\n" + " - marIters = 30 (by default). Only for LM\n" + " - lambda = 1-6, LM paramter for the size of the update\n" + " - solutionTolerance: convergence criteria.", py::arg("method") = FGraphSolve::optimMethod::GN, - py::arg("maxIters") = 30) + py::arg("maxIters") = 30, + py::arg("lambda") = 1e-6, + py::arg("solutionTolerance") = 1e-2) .def("chi2", &FGraphSolve::chi2, - "Calculated the chi2 of the problem. By default re-evaluates residuals, set to false if doesn't", + "Calculated the chi2 of the problem.\n" + "By default re-evaluates residuals, \n" + "if set to false if doesn't: evaluateResidualsFlag = False", py::arg("evaluateResidualsFlag") = true) .def("get_estimated_state", &FGraphSolve::get_estimated_state, - "returns the list of states ordered according to ids. Some of these elements might be matrices if the are 3D poses") + "returns the list of states ordered according to ids.\n" + "Each state can be of different size and some of these elements might be matrices if the are 3D poses") .def("get_information_matrix", &FGraphSolve::get_information_matrix, - "Returns the information matrix. It requires to be calculated -> solved the problem", + "Returns the information matrix (sparse matrix). It requires to be calculated -> solved the problem", + py::return_value_policy::copy) + .def("get_adjacency_matrix", &FGraphSolve::get_adjacency_matrix, + "Returns the adjacency matrix (sparse matrix). It requires to be calculated -> solved the problem", + py::return_value_policy::copy) + .def("get_W_matrix", &FGraphSolve::get_W_matrix, + "Returns the W matrix of observation noises(sparse matrix). It requires to be calculated -> solved the problem", + py::return_value_policy::copy) + .def("get_vector_b", &FGraphSolve::get_vector_b, + "Returns the vector b = A'Wr, from residuals. It requires to be calculated -> solved the problem", py::return_value_policy::copy) .def("get_chi2_array", &FGraphSolve::get_chi2_array, "Returns the vector of chi2 values for each factor. It requires to be calculated -> solved the problem", @@ -225,10 +244,13 @@ void init_FGraph(py::module &m) .def("print", &FGraph::print, "By default False: does not print all the information on the Fgraph", py::arg("completePrint") = false) // ----------------------------------------------------------------------------- // Specific call to 2D - .def("add_node_pose_2d", &FGraphPy::add_node_pose_2d) + .def("add_node_pose_2d", &FGraphPy::add_node_pose_2d, + " - arguments, initial estimate (np.zeros(3)\n" + "output, node id, for later usage") .def("add_factor_1pose_2d", &FGraphPy::add_factor_1pose_2d) .def("add_factor_2poses_2d", &FGraphPy::add_factor_2poses_2d, - "Factors connecting 2 poses. If last input set to true (by default false), also updates the value of the target Node according to the new obs + origin node", + "Factors connecting 2 poses. If last input set to true (by default false), also updates " + "the value of the target Node according to the new obs + origin node", py::arg("obs"), py::arg("nodeOriginId"), py::arg("nodeTargetId"), @@ -244,14 +266,15 @@ void init_FGraph(py::module &m) py::arg("obsInvCov")) // 2d Landmkarks .def("add_node_landmark_2d", &FGraphPy::add_node_landmark_2d, - "Ladmarks are 2D points, in [x,y]") + "Landmarks are 2D points, in [x,y]. It requries initialization, " + "although factor 1pose 1land 2d can initialize with the inverse observation function") .def("add_factor_1pose_1landmark_2d", &FGraphPy::add_factor_1pose_1landmark_2d, - "Factor connecting 1 pose and 1 point (landmark).", - py::arg("obs"), - py::arg("nodePoseId"), - py::arg("nodeLandmarkId"), - py::arg("obsInvCov"), - py::arg("initializeLandmark") = false) + "Factor connecting 1 pose and 1 point (landmark).", + py::arg("obs"), + py::arg("nodePoseId"), + py::arg("nodeLandmarkId"), + py::arg("obsInvCov"), + py::arg("initializeLandmark") = false) // ----------------------------------------------------------------------------- // Specific call to 3D .def("add_node_pose_3d", &FGraphPy::add_node_pose_3d, diff --git a/src/FGraph/factor_graph_solve.cpp b/src/FGraph/factor_graph_solve.cpp index e3e37af6..5174d114 100644 --- a/src/FGraph/factor_graph_solve.cpp +++ b/src/FGraph/factor_graph_solve.cpp @@ -46,7 +46,7 @@ FGraphSolve::FGraphSolve(matrixMethod method, optimMethod optim): FGraphSolve::~FGraphSolve() = default; -void FGraphSolve::solve(optimMethod method, uint_t maxIters) +void FGraphSolve::solve(optimMethod method, uint_t maxIters, matData_t lambda, matData_t solutionTolerance) { /** * 2800 2D nodes on M3500 @@ -58,6 +58,8 @@ void FGraphSolve::solve(optimMethod method, uint_t maxIters) * */ optimMethod_ = method; // updates the optimization method + lambda_ = lambda; + solutionTolerance_ = solutionTolerance; time_profiles_.reset(); // Optimization diff --git a/src/FGraph/mrob/factor_graph_solve.hpp b/src/FGraph/mrob/factor_graph_solve.hpp index 7fdae584..0d26ae49 100644 --- a/src/FGraph/mrob/factor_graph_solve.hpp +++ b/src/FGraph/mrob/factor_graph_solve.hpp @@ -95,7 +95,7 @@ class FGraphSolve: public FGraph * ultimately on the function input, * by default optim method is Gauss Newton */ - void solve(optimMethod method = GN, uint_t maxIters = 20); + void solve(optimMethod method = GN, uint_t maxIters = 20, matData_t lambda = 1e-6, matData_t solutionTolerance = 1e-2); /** * Evaluates the current solution chi2. * @@ -117,10 +117,25 @@ class FGraphSolve: public FGraph matrixMethod get_build_matrix_method() { return matrixMethod_;}; /** - * Returns a reference to the information matrix. + * Returns a copy to the information matrix. * TODO If true, it re-evaluates the problem */ SMatCol get_information_matrix() { return L_;} + /** + * Returns a copy to the information matrix. + * TODO If true, it re-evaluates the problem + */ + SMatCol get_adjacency_matrix() { return A_;} + /** + * Returns a copy to the W matrix. + * TODO If true, it re-evaluates the problem + */ + SMatCol get_W_matrix() { return W_;} + /** + * Returns a copy to the processed residuals in state space b = A'Wr. + * TODO If true, it re-evaluates the problem + */ + MatX1 get_vector_b() { return b_;} /** * Returns a vector of chi2 values for each of the factors. */