Skip to content

Commit

Permalink
Adding get_adjacency and help comments
Browse files Browse the repository at this point in the history
  • Loading branch information
@g-ferrer-sk @anastasiia-kornilova
g-ferrer-sk authored and anastasiia-kornilova committed Apr 20, 2021
1 parent b69bf78 commit 9838b16
Show file tree
Hide file tree
Showing 3 changed files with 57 additions and 17 deletions.
51 changes: 37 additions & 14 deletions mrobpy/FGraphPy.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand All @@ -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"),
Expand All @@ -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,
Expand Down
4 changes: 3 additions & 1 deletion src/FGraph/factor_graph_solve.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
19 changes: 17 additions & 2 deletions src/FGraph/mrob/factor_graph_solve.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*
Expand All @@ -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.
*/
Expand Down

0 comments on commit 9838b16

Please sign in to comment.