From 08e6d9163f3c50a9bb399d57eab999d531d1358c Mon Sep 17 00:00:00 2001 From: Aleksei Panchenko Date: Wed, 29 May 2024 15:59:19 +0300 Subject: [PATCH] clean up: updated authors in comments, removed redundant headers and eigen_factor_diff class/ methods --- src/FGraphDiff/CMakeLists.txt | 6 - src/FGraphDiff/examples/example_solver_2d.cpp | 13 +- src/FGraphDiff/factor_diff.cpp | 8 +- src/FGraphDiff/factor_graph_diff.cpp | 14 +- src/FGraphDiff/factor_graph_diff_solve.cpp | 145 +----------------- src/FGraphDiff/factors/factor1Pose2d_diff.cpp | 28 +++- .../factors/factor2Poses2d_diff.cpp | 26 +++- src/FGraphDiff/mrob/factor_diff.hpp | 45 ++---- src/FGraphDiff/mrob/factor_graph_diff.hpp | 17 +- .../mrob/factor_graph_diff_solve.hpp | 24 +-- .../mrob/factors/factor1Pose2d_diff.hpp | 8 +- .../mrob/factors/factor2Poses2d_diff.hpp | 10 +- src/pybind/FGraphDiffPy.cpp | 45 ++---- 13 files changed, 112 insertions(+), 277 deletions(-) diff --git a/src/FGraphDiff/CMakeLists.txt b/src/FGraphDiff/CMakeLists.txt index 23e965e..291e55c 100644 --- a/src/FGraphDiff/CMakeLists.txt +++ b/src/FGraphDiff/CMakeLists.txt @@ -1,10 +1,5 @@ -# locate the necessary dependencies, if any - -project(FGraphDiff) - # extra header files SET(headers - ../FGraph/mrob/node.hpp mrob/factor_diff.hpp mrob/factor_graph_diff.hpp mrob/factor_graph_diff_solve.hpp @@ -12,7 +7,6 @@ SET(headers # extra source files SET(sources - ../FGraph/node.cpp factor_diff.cpp factor_graph_diff.cpp factor_graph_diff_solve.cpp diff --git a/src/FGraphDiff/examples/example_solver_2d.cpp b/src/FGraphDiff/examples/example_solver_2d.cpp index fa4a034..cef87a0 100644 --- a/src/FGraphDiff/examples/example_solver_2d.cpp +++ b/src/FGraphDiff/examples/example_solver_2d.cpp @@ -13,18 +13,15 @@ * limitations under the License. * * - * example_solver.cpp + * example_solver_2d.cpp * - * Created on: April 10, 2019 - * Author: Gonzalo Ferrer - * g.ferrer@skoltech.ru + * Created on: May 28, 2024 + * Author: Aleksei Panchenko + * aleksei.panchenko@skoltech.ru * Mobile Robotics Lab, Skoltech */ - - - #include "mrob/factor_graph_diff_solve.hpp" #include "mrob/factors/factor1Pose2d_diff.hpp" #include "mrob/factors/factor2Poses2d_diff.hpp" @@ -72,7 +69,7 @@ int main () // solve the Gauss Newton optimization graph.print(true); - graph.solve(mrob::FGraphDiffSolve::LM); + graph.solve(mrob::FGraphDiffSolve::GN); std::cout << "\nSolved, chi2 = " << graph.chi2() << std::endl; diff --git a/src/FGraphDiff/factor_diff.cpp b/src/FGraphDiff/factor_diff.cpp index 062fda7..b760cf4 100644 --- a/src/FGraphDiff/factor_diff.cpp +++ b/src/FGraphDiff/factor_diff.cpp @@ -13,10 +13,12 @@ * limitations under the License. * * - * factor.cpp + * factor_diff.cpp * - * Created on: Feb 27, 2018 - * Author: Gonzalo Ferrer + * Created on: May 28, 2024 + * Author: Aleksei Panchenko + * aleksei.panchenko@skoltech.ru + * Gonzalo Ferrer * g.ferrer@skoltech.ru * Mobile Robotics Lab, Skoltech */ diff --git a/src/FGraphDiff/factor_graph_diff.cpp b/src/FGraphDiff/factor_graph_diff.cpp index 98c39ec..8bc9f38 100644 --- a/src/FGraphDiff/factor_graph_diff.cpp +++ b/src/FGraphDiff/factor_graph_diff.cpp @@ -13,10 +13,12 @@ * limitations under the License. * * - * factor_graph.cpp + * factor_graph_diff.cpp * - * Created on: Feb 12, 2018 - * Author: Gonzalo Ferrer + * Created on: May 28, 2024 + * Author: Aleksei Panchenko + * aleksei.panchenko@skoltech.ru + * Gonzalo Ferrer * g.ferrer@skoltech.ru * Mobile Robotics Lab, Skoltech */ @@ -45,9 +47,3 @@ factor_id_t FGraphDiff::add_factor(std::shared_ptr &factor) return factor->get_id(); } -factor_id_t FGraphDiff::add_eigen_factor(std::shared_ptr &factor) -{ - factor->set_id(eigen_factors_.size()); - eigen_factors_.emplace_back(factor); - return factor->get_id(); -} diff --git a/src/FGraphDiff/factor_graph_diff_solve.cpp b/src/FGraphDiff/factor_graph_diff_solve.cpp index 2e00119..d0253f9 100644 --- a/src/FGraphDiff/factor_graph_diff_solve.cpp +++ b/src/FGraphDiff/factor_graph_diff_solve.cpp @@ -13,17 +13,18 @@ * limitations under the License. * * - * factor_graph_solve.cpp + * factor_graph_diff_solve.cpp * - * Created on: Mar 23, 2018 - * Author: Gonzalo Ferrer + * Created on: May 28, 2024 + * Author: Aleksei Panchenko + * aleksei.panchenko@skoltech.ru + * Gonzalo Ferrer * g.ferrer@skoltech.ru * Mobile Robotics Lab, Skoltech */ #include "mrob/factor_graph_diff_solve.hpp" -//#include "mrob/CustomCholesky.hpp" #include #include @@ -104,13 +105,6 @@ void FGraphDiffSolve::build_problem(bool useLambda) this->build_adjacency(); time_profiles_.stop("Adjacency"); - if (eigen_factors_.size()>0) - { - time_profiles_.start(); - this->build_info_EF(); - time_profiles_.stop("EFs Jacobian and Hessian"); - } - // 1.2) builds specifically the information switch(matrixMethod_) { @@ -391,117 +385,6 @@ void FGraphDiffSolve::build_info_adjacency() L_ = (A_.transpose() * W_.selfadjointView() * A_); b_ = A_.transpose() * W_.selfadjointView() * r_; } - - // If any EF, we should combine both solutions - if (eigen_factors_.size() > 0 ) - { - if (buildAdjacencyFlag_) - { - L_ += hessianEF_.selfadjointView(); - b_ += gradientEF_; - } - // case when there are pure EF and no other factor - else - { - L_ = hessianEF_.selfadjointView(); - b_ = gradientEF_; - } - } -} - - -void FGraphDiffSolve::build_info_EF() -{ - gradientEF_.resize(stateDim_,1); - gradientEF_.setZero(); - std::vector hessianData; - // TODO if EF ever connected a node that is not 6D, then this will not hold. - hessianData.reserve(eigen_factors_.size()*21);//For each EF we reserve the uppder triangular view => 6+5+..+1 = 21 - - for (size_t id = 0; id < eigen_factors_.size(); ++id) - { - auto f = eigen_factors_[id]; - f->evaluate_residuals(); - f->evaluate_jacobians();//and Hessian - f->evaluate_chi2(); - auto neighNodes = f->get_neighbour_nodes(); - for (auto node : *neighNodes) - { - uint_t indNode = node->get_id(); - if (node->get_node_mode() == Node::nodeMode::ANCHOR) - { - continue; - } - // Updating Jacobian, b should has been previously calculated - Mat61 J = f->get_jacobian(indNode); - - // Calculate the robust factor contribution, similar than in the adjacency - // Now the weight fator should be introduced in the Hessian block and in the gradient s.t. - // (wH)^-1* wgrad - // When using the aggregated matrix of all EFs, this operation is not trivila (for one EF is trival, it cancels out ofc) - matData_t robust_weight = 1.0; - // The vlaue needs to be normalized by - robust_weight = f->evaluate_robust_weight(); - J *= robust_weight; - - // It requires previous calculation of indNodesMatrix (in build adjacency) - gradientEF_.block<6,1>(indNodesMatrix_[indNode],0) += J; - - // get the neighboiring nodes TODO and for over them - uint_t startingIndex = indNodesMatrix_[indNode]; - for (auto node2 : *neighNodes) - { - // getting second index, rows in the hessian matrix - uint_t indNode_2 = node2->get_id(); - uint_t startingIndex_2 = indNodesMatrix_[indNode_2]; - - // Check if the indexes (col,row) is in the upper triangular part or skip if not - if (startingIndex_2 < startingIndex) - continue; - - // Calculate hessian, this is a lookup - Mat6 H; - // If there is no such crosterms, the methods returns false and the block embeding into H is skipped - if (!f->get_hessian(H,indNode,indNode_2)) - { - continue; - } - //std::cout << "Hessian node (i,j) = (" << indNode << ", " << indNode_2 << ")\n"; - - // Robust factors, for the same EF, we must account for the weight factor in the block hessian as well: - H *= robust_weight; - - - // Calculate the variable that allows to control diagonal/crossterms in the for() below - // If it is a crossterm, it needs all elements of the 6x6 matrix, so it does not enable the for start in diag (=0) - // If it is diagonal, it can start at the current row for the upper triangular view (therefore =1) - uint_t cross_term_reset = 0; - if (indNode == indNode_2) - cross_term_reset = 1; - - - // Updating the Full Hessian - // XXX if EF ever connected a node that is not 6D, then this will not hold. TODO - for (uint_t i = 0; i < 6; i++) - { - // for diagonal terms, this will start at j=i, which give the uppter triang view - for (uint_t j = i*cross_term_reset; j<6; j++) - { - // convert the hessian to triplets, duplicated ones will be summed - // https://eigen.tuxfamily.org/dox/classEigen_1_1SparseMatrix.html#a8f09e3597f37aa8861599260af6a53e0 - hessianData.emplace_back(Triplet(startingIndex+ i, startingIndex_2+ j, H(i,j))); - } - } - - } - - - } - } - - // create a Upper-view sparse matrix from the triplets: - hessianEF_.resize(stateDim_,stateDim_); - hessianEF_.setFromTriplets(hessianData.begin(), hessianData.end()); } matData_t FGraphDiffSolve::chi2(bool evaluateResidualsFlag) @@ -587,24 +470,6 @@ MatX1 FGraphDiffSolve::get_chi2_array() return results; } - -std::vector FGraphDiffSolve::get_eigen_factors_robust_mask() -{ - // it will create a copy on the python side, that is why scope here is just defined in here - // XXX:one day we should improve the interface with python... - std::vector results; - results.reserve(eigen_factors_.size()); - - bool mask; - for (auto ef : eigen_factors_) - { - mask = ef->get_robust_mask(); - results.push_back(mask); - } - - return results; -} - std::vector FGraphDiffSolve::get_factors_robust_mask() { std::vector results; diff --git a/src/FGraphDiff/factors/factor1Pose2d_diff.cpp b/src/FGraphDiff/factors/factor1Pose2d_diff.cpp index 8a51c0f..b056f5d 100644 --- a/src/FGraphDiff/factors/factor1Pose2d_diff.cpp +++ b/src/FGraphDiff/factors/factor1Pose2d_diff.cpp @@ -13,9 +13,9 @@ * limitations under the License. * * - * Created on: Jan 14, 2019 - * Author: Konstantin Pakulev - * konstantin.pakulev@skoltech.ru + * Created on: May 28, 2024 + * Author: Aleksei Panchenko + * aleksei.panchenko@skoltech.ru * Gonzalo Ferrer * g.ferrer@skoltech.ru * Mobile Robotics Lab, Skoltech @@ -52,11 +52,31 @@ void Factor1Pose2d_diff::evaluate_chi2() chi2_ = 0.5 * r_.dot(W_ * r_); } -void mrob::Factor1Pose2d_diff::evaluate_dr_dz() +void Factor1Pose2d_diff::evaluate_dr_dz() { dr_dz_ = - Mat3::Identity(); } +MatRefConst Factor1Pose2d_diff::get_dr_dz() const +{ + Mat3 result; + result = this->dr_dz_; + + return result; +} + +// std::vector mrob::Factor1Pose2d_diff::get_d2r_dx_dz() const +// { +// std::vector result[3]; + +// result[0] = MatRef(this->d2r_dx_dz_[0]); +// result[1] = this->d2r_dx_dz_[1]; +// result[2] = this->d2r_dx_dz_[2]; + +// return result; +// } + + void mrob::Factor1Pose2d_diff::evaluate_d2r_dx_dz() { d2r_dx_dz_[0].setZero(); diff --git a/src/FGraphDiff/factors/factor2Poses2d_diff.cpp b/src/FGraphDiff/factors/factor2Poses2d_diff.cpp index 2bfa73a..121fe1d 100644 --- a/src/FGraphDiff/factors/factor2Poses2d_diff.cpp +++ b/src/FGraphDiff/factors/factor2Poses2d_diff.cpp @@ -13,15 +13,16 @@ * limitations under the License. * * - * Created on: Jan 14, 2019 - * Author: Konstantin Pakulev - * konstantin.pakulev@skoltech.ru + * Created on: May 28, 2024 + * Author: Aleksei Panchenko + * aleksei.panchenko@skoltech.ru * Gonzalo Ferrer * g.ferrer@skoltech.ru * Mobile Robotics Lab, Skoltech */ #include +#include #include @@ -100,6 +101,14 @@ void mrob::Factor2Poses2d_diff::evaluate_dr_dz() std::cout << "mrob::Factor2Poses2d_diff::evaluate_dr_dz - not implemented!" << std::endl; } +MatRefConst Factor2Poses2d_diff::get_dr_dz() const +{ + Mat3 result; + result = this->dr_dz_; + + return result; +} + void mrob::Factor2Poses2d_diff::evaluate_d2r_dx_dz() { d2r_dx_dz_[0].setZero(); @@ -108,6 +117,17 @@ void mrob::Factor2Poses2d_diff::evaluate_d2r_dx_dz() std::cout << "mrob::Factor2Poses2d_diff::evaluate_d2r_dx_dz - not implemented!" << std::endl; } +// std::vector Factor2Poses2d_diff::get_d2r_dx_dz() +// { +// std::vector result[3]; + +// result[0] = this->d2r_dx_dz_[0]; +// result[1] = this->d2r_dx_dz_[1]; +// result[2] = this->d2r_dx_dz_[2]; + +// return result; +// } + void Factor2Poses2d_diff::print() const { diff --git a/src/FGraphDiff/mrob/factor_diff.hpp b/src/FGraphDiff/mrob/factor_diff.hpp index 2c87cb0..3d053f0 100644 --- a/src/FGraphDiff/mrob/factor_diff.hpp +++ b/src/FGraphDiff/mrob/factor_diff.hpp @@ -13,11 +13,11 @@ * limitations under the License. * * - * factor.hpp + * factor_diff.hpp * - * Created on: Feb 12, 2018 - * Author: Gonzalo Ferrer - * g.ferrer@skoltech.ru + * Created on: May 28, 2024 + * Author: Aleksei Panchenko + * aleksei.panchenko@skoltech.ru * Mobile Robotics Lab, Skoltech */ @@ -27,11 +27,14 @@ #include #include + +#include "mrob/matrix_base.hpp" #include "../FGraph/mrob/factor.hpp" namespace mrob{ +class Factor; /** * DiffFactor class is a base pure abstract class defining factors, * the second type of vertexes on factor graphs (bipartite). @@ -46,7 +49,6 @@ namespace mrob{ * otherwise the optimization will not work properly. */ - class DiffFactor : public Factor { public: @@ -61,37 +63,10 @@ class DiffFactor : public Factor * */ virtual void evaluate_d2r_dx_dz() =0; -}; -/** - * Abstract class DiffEigenFactor. This is a factor with extra methods than DiffFactor - * which requires a new base abstract class. - * - * The Eigen factor connects different poses that have observed the same geometric entity. - * It is not required an explicit parametrization of the current state, which is a geometric entity - * estimated a priory on each iteration, e.g. a plane. The resultant topology - * is N nodes connecting to the eigen factor. - * - * Hence, the new method get state, for instance a plane, but this state is outside the FGraphDiff optimization, that - * is why we can consider this approach non-parametric - * - get_state() - * - * NOTE: due to its nature, multiple observation can be added to the same EF, - * meaning we need to create a constructor PLUS an additional method - * - add_point() - * - * In order to build the problem we would follow the interface specifications by FGraphDiff - * but we need extra methods and variables to keep track of the neighbours. For instance, we need - * to get Jacobians and Hessian matrices - * - * This class assumes that matrices S = sum p*p' are calculated before since they are directly inputs - * XXX should we store all points? - */ -class DiffEigenFactor : public EigenFactor -{ -public: - DiffEigenFactor(robustFactorType factor_type = QUADRATIC, uint_t potNumberNodes = 5); - virtual ~DiffEigenFactor() = default; + virtual MatRefConst get_dr_dz() const = 0; + + // virtual std::vector get_d2r_dx_dz() const = 0; }; } diff --git a/src/FGraphDiff/mrob/factor_graph_diff.hpp b/src/FGraphDiff/mrob/factor_graph_diff.hpp index d30e05b..ecf6f16 100644 --- a/src/FGraphDiff/mrob/factor_graph_diff.hpp +++ b/src/FGraphDiff/mrob/factor_graph_diff.hpp @@ -13,11 +13,11 @@ * limitations under the License. * * - * factor_graph.hpp + * factor_graph_diff.hpp * - * Created on: Feb 12, 2018 - * Author: Gonzalo Ferrer - * g.ferrer@skoltech.ru + * Created on: May 28, 2024 + * Author: Aleksei Panchenko + * aleksei.panchenko@skoltech.ru * Mobile Robotics Lab, Skoltech */ @@ -72,13 +72,6 @@ class FGraphDiff : public FGraph{ * returns factor id */ factor_id_t add_factor(std::shared_ptr &factor); - /** - * Adds an Eigen Factor, the special factor that is not formulated - * as a sum of residuals, but directly as a real value (eigenvalue) - * and therefore it requires a different processing, apart from the - * standard residual factors from above. - */ - factor_id_t add_eigen_factor(std::shared_ptr &factor); /** * get_node returns the node given the node id key, now a position on the data structure */ @@ -89,4 +82,4 @@ class FGraphDiff : public FGraph{ } -#endif /* FACTOR_Graph_DIFF_HPP_ */ +#endif /* FACTOR_GRAPH_DIFF_HPP_ */ diff --git a/src/FGraphDiff/mrob/factor_graph_diff_solve.hpp b/src/FGraphDiff/mrob/factor_graph_diff_solve.hpp index d55ac2c..386f2a0 100644 --- a/src/FGraphDiff/mrob/factor_graph_diff_solve.hpp +++ b/src/FGraphDiff/mrob/factor_graph_diff_solve.hpp @@ -13,10 +13,12 @@ * limitations under the License. * * - * factor_graph_solve.hpp + * factor_graph_diff_solve.hpp * - * Created on: Mar 23, 2018 - * Author: Gonzalo Ferrer + * Created on: May 28, 2024 + * Author: Aleksei Panchenko + * aleksei.panchenko@skoltech.ru + * Gonzalo Ferrer * g.ferrer@skoltech.ru * Mobile Robotics Lab, Skoltech */ @@ -149,14 +151,6 @@ class FGraphDiffSolve: public FGraphDiff * Returns a vector of chi2 values for each of the factors. */ MatX1 get_chi2_array(); - /** - * Returns a vector (python list) of Eigen factors robust functions: - * - True if the robust mask was applied - * - False if the robust factor had not effect. - * - * The index in the graph is the Eigen DiffFactor Id. - */ - std::vector get_eigen_factors_robust_mask(); /** * Returns a vector (python list) * - True if the robust mask was applied @@ -190,12 +184,6 @@ class FGraphDiffSolve: public FGraphDiff * The residuals are also calculated as b = A^T * W *r */ void build_info_adjacency(); - /** - * Builds the information matrix directly from Eigen Factors. - * It follows a different approach than build adjacency, it will only create - * a Hessian and Jacobian when at least one EF is present. - */ - void build_info_EF(); void build_schur(); // TODO /** @@ -282,8 +270,6 @@ class FGraphDiffSolve: public FGraphDiff // Methods for handling Eigen factors. If not used, no problem - SMatCol hessianEF_; - MatX1 gradientEF_; bool buildAdjacencyFlag_; // time profiling diff --git a/src/FGraphDiff/mrob/factors/factor1Pose2d_diff.hpp b/src/FGraphDiff/mrob/factors/factor1Pose2d_diff.hpp index 57f7f16..3d95e9f 100644 --- a/src/FGraphDiff/mrob/factors/factor1Pose2d_diff.hpp +++ b/src/FGraphDiff/mrob/factors/factor1Pose2d_diff.hpp @@ -13,9 +13,9 @@ * limitations under the License. * * - * Created on: Jan 14, 2019 - * Author: Konstantin Pakulev - * konstantin.pakulev@skoltech.ru + * Created on: May 28, 2024 + * Author: Aleksei Panchenko + * aleksei.panchenko@skoltech.ru * Gonzalo Ferrer * g.ferrer@skoltech.ru * Mobile Robotics Lab, Skoltech @@ -54,6 +54,8 @@ namespace mrob{ void evaluate_chi2() override; void evaluate_dr_dz() override; void evaluate_d2r_dx_dz() override; + MatRefConst get_dr_dz() const override; + // std::vector get_d2r_dx_dz() const override; void print() const override; diff --git a/src/FGraphDiff/mrob/factors/factor2Poses2d_diff.hpp b/src/FGraphDiff/mrob/factors/factor2Poses2d_diff.hpp index ea4c832..f8c18ab 100644 --- a/src/FGraphDiff/mrob/factors/factor2Poses2d_diff.hpp +++ b/src/FGraphDiff/mrob/factors/factor2Poses2d_diff.hpp @@ -13,11 +13,11 @@ * limitations under the License. * * - * Created on: Jan 14, 2019 - * Author: Gonzalo Ferrer + * Created on: May 28, 2024 + * Author: Aleksei Panchenko + * aleksei.panchenko@skoltech.ru + * Gonzalo Ferrer * g.ferrer@skoltech.ru - * konstantin.pakulev@skoltech.ru - * Konstantin Pakulev * Mobile Robotics Lab, Skoltech */ #ifndef MROB_FACTOR2POSES2D_DIFF_H @@ -62,6 +62,8 @@ namespace mrob{ void evaluate_chi2() override; void evaluate_dr_dz() override; void evaluate_d2r_dx_dz() override; + MatRefConst get_dr_dz() const override; + // std::vector get_d2r_dx_dz() override; MatRefConst get_obs() const override {return obs_;}; VectRefConst get_residual() const override {return r_;}; diff --git a/src/pybind/FGraphDiffPy.cpp b/src/pybind/FGraphDiffPy.cpp index 56db0f6..f864398 100644 --- a/src/pybind/FGraphDiffPy.cpp +++ b/src/pybind/FGraphDiffPy.cpp @@ -15,8 +15,10 @@ * * FGraphDiffPy.cpp * - * Created on: Apr 5, 2019 - * Author: Gonzalo Ferrer + * Created on: May 28, 2024 + * Author: Aleksei Panchenko + * aleksei.panchenko@skoltech.ru + * Gonzalo Ferrer * g.ferrer@skoltech.ru * Mobile Robotics Lab, Skoltech */ @@ -31,22 +33,6 @@ #include "mrob/factors/factor1Pose2d_diff.hpp" #include "mrob/factors/factor2Poses2d_diff.hpp" -#include "mrob/factors/factor1PosePoint2Plane.hpp" -#include "mrob/factors/factor1PosePoint2Point.hpp" - -#include "mrob/factors/factor1Pose1Plane4d.hpp" -#include "mrob/factors/nodePlane4d.hpp" -#include "mrob/factors/BaregEFPlane.hpp" -#include "mrob/factors/PiFactorPlane.hpp" -#include "mrob/factors/EigenFactorPlaneCenter.hpp" -#include "mrob/factors/EigenFactorPlaneCenter2.hpp" -#include "mrob/factors/EigenFactorPoint.hpp" -#include "mrob/factors/EigenFactorPlaneDense.hpp" -#include "mrob/factors/EigenFactorPlaneDenseHomog.hpp" - -// #include "mrob/factors/factorCameraProj3dPoint.hpp" -// #include "mrob/factors/factorCameraProj3dLine.hpp" - //#include namespace py = pybind11; @@ -82,13 +68,13 @@ class FGraphDiffPy : public FGraphDiffSolve this->add_node(n); return n->get_id(); } - void add_factor_1pose_2d(const py::EigenDRef obs, uint_t nodeId, const py::EigenDRef obsInvCov) + void add_factor_1pose_2d_diff(const py::EigenDRef obs, uint_t nodeId, const py::EigenDRef obsInvCov) { auto n1 = this->get_node(nodeId); std::shared_ptr f(new mrob::Factor1Pose2d_diff(obs,n1,obsInvCov,robust_type_)); this->add_factor(f); } - void add_factor_2poses_2d(const py::EigenDRef obs, uint_t nodeOriginId, uint_t nodeTargetId, + void add_factor_2poses_2d_diff(const py::EigenDRef obs, uint_t nodeOriginId, uint_t nodeTargetId, const py::EigenDRef obsInvCov, bool updateNodeTarget) { auto nO = this->get_node(nodeOriginId); @@ -110,12 +96,12 @@ class FGraphDiffPy : public FGraphDiffSolve void init_FGraphDiff(py::module &m) { - py::enum_(m, "FGraphDiff.optimMethod") - .value("GN", FGraphDiffSolve::optimMethod::GN) - .value("LM", FGraphDiffSolve::optimMethod::LM) - .value("LM_ELLIPS", FGraphDiffSolve::optimMethod::LM_ELLIPS) - .export_values() - ; +// py::enum_(m, "FGraphDiff.optimMethod") +// .value("GN", FGraphDiffSolve::optimMethod::GN) +// .value("LM", FGraphDiffSolve::optimMethod::LM) +// .value("LM_ELLIPS", FGraphDiffSolve::optimMethod::LM_ELLIPS) +// .export_values() +// ; // py::enum_(m, "FGraphDiff.robustFactorType") // .value("QUADRATIC", DiffFactor::robustFactorType::QUADRATIC) // .value("CAUCHY", DiffFactor::robustFactorType::CAUCHY) @@ -171,9 +157,6 @@ void init_FGraphDiff(py::module &m) .def("get_chi2_array", &FGraphDiffSolve::get_chi2_array, "Returns the vector of chi2 values for each factor. It requires to be calculated -> solved the problem", py::return_value_policy::copy) - .def("get_eigen_factors_robust_mask", &FGraphDiffSolve::get_eigen_factors_robust_mask, - "Returns a vector (python list) of Eigen factors robust functions: - True if the robust mask was applied - False if the robust factor had not effect", - py::return_value_policy::copy) .def("get_factors_robust_mask", &FGraphDiffSolve::get_factors_robust_mask, "Returns a vector (python list) of factors robust functions: - True if the robust mask was applied - False if the robust factor had not effect", py::return_value_policy::copy) @@ -190,8 +173,8 @@ void init_FGraphDiff(py::module &m) "output, node id, for later usage", py::arg("x"), py::arg("mode") = Node::nodeMode::STANDARD) - .def("add_factor_1pose_2d", &FGraphDiffPy::add_factor_1pose_2d) - .def("add_factor_2poses_2d", &FGraphDiffPy::add_factor_2poses_2d, + .def("add_factor_1pose_2d", &FGraphDiffPy::add_factor_1pose_2d_diff) + .def("add_factor_2poses_2d", &FGraphDiffPy::add_factor_2poses_2d_diff, "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"),