Feature/ef alternating (#64)

* adding matrix form utilities for EFs, creating dense EF and minor updates. WIP

* Solved isue with undefined symbol (on destructor not default one one). Now the behaviour we get is not exactly the EF center. WIP

* Solved Jacobian and Hessian, now equal (slighly better) than EFcenter.

* EF dense, crossterm added on block diagonal

* Update on classes EF nodes interface. WIP

* Changing Bareg to EFplaneBase class

* Changing EF center. Removing deprecated data structure and EF factors

* Getting the hessian block logic modify the Fgraph solve. WIP

* common hessian interface for EF, now as a function of the two id nodes generating it (some EF only calculate the diagonal, but this generalizes)

* Hessian build problem, needs testing. WIP

* Modified method get_hessian to return a success_flag and the block matrix by reference

* Adding full hessian, solving issue with storing gradient Qxpi per pose. WIP

* Inverse of Q matrix. Dense solution not being better than block diagonal. Needs more investigation. WIP

* Fix of error on EF's with empty points

* Version of EF with centered data and block gradients

* EF center block gradients, Now considering hessian d pi d Q. Minimal impact, it is needed to measure

* Update on the optim for robust on EF, needs now setting of params and configs

* Adding robust factors to EF, checking the sum of information from all points vs current error

* adding function to detect robust masks

* fix to whindows wheels, copied in here from PR#61

* fix to grad pi dx, problem with sign. Now dense provides overall best restuls.

* cleaning plane estimation and quasi-inverse

* Hessian updated, now equals numerical one for the dense case and the alternating methods (block diagonal)

* final function names for EF: Dense and Alternating. Default is (best performer) Alternating

.github/workflows/wheels.yml

@@ -13,7 +13,7 @@ jobs:
       - uses: actions/checkout@v3
 
       - name: Build wheels
-        uses: pypa/cibuildwheel@v2.14.1
+        uses: pypa/cibuildwheel@v2.16.5
         with:
           package-dir: mrobpy
 

src/FGraph/factor.cpp

@@ -27,7 +27,8 @@
 using namespace mrob;
 
 Factor::Factor(uint_t dim, uint_t allNodesDim, robustFactorType factor_type, uint_t potNumberNodes):
-		id_(0), dim_(dim), allNodesDim_(allNodesDim), chi2_(0), robust_type_(factor_type), robust_weight_(0.0)
+		id_(0), dim_(dim), allNodesDim_(allNodesDim), chi2_(0), robust_type_(factor_type), robust_weight_(0.0),
+        robust_mask_(false)
 {
     // Child factor must specify dimensions
     neighbourNodes_.reserve( potNumberNodes );
@@ -39,34 +40,50 @@ Factor::~Factor()
 }
 
 
-matData_t Factor::evaluate_robust_weight(matData_t u, matData_t params)
+matData_t Factor::evaluate_robust_weight(matData_t u, matData_t threshold)
 {
+    matData_t params;
     switch(robust_type_)
     {
         case HUBER:
             // p(u) = 1/2u^2    if u < d
             //        d(u-1/2d)
-            if (u < 1.0) //XXX should this be set param?
+            if (u < threshold)
             {
                 robust_weight_ = 1.0;
+                robust_mask_ = false;
             }
             else
             {
                 robust_weight_ = 1.0/u;
+                robust_mask_ = true;
             }
             break;
         case CAUCHY:
             robust_weight_ = 1.0/(1+u*u);
+            robust_mask_ = true;//TODO this would require a check on the threshold?
             break;
         case MCCLURE:
             params = 1+u*u;
             robust_weight_ = 1.0/params/params;
+            robust_mask_ = true;
             break;
         case RANSAC:
+            if (u < threshold)
+            {
+                robust_weight_ = 1.0;
+                robust_mask_ = false;
+            }
+            else
+            {
+                robust_weight_ = 0.0;
+                robust_mask_ = true;
+            }
             break;
         case QUADRATIC:
         default:
             robust_weight_ = 1.0;
+            robust_mask_ = false;
             break;
     }
     return robust_weight_;

src/FGraph/factor_graph_solve.cpp

@@ -360,6 +360,7 @@ void FGraphSolve::build_adjacency()
             {
                 uint_t iRow = indFactorsMatrix[i] + l;
                 uint_t iCol = indFactorsMatrix[i] + k;
+                // Weights are then applied both to the residual and the Hessian by modifying the information matrix.
                 robust_weight = f->evaluate_robust_weight(std::sqrt(f->get_chi2()));
                 W_.insert(iRow,iCol) = robust_weight * f->get_information_matrix()(l,k);
             }
@@ -422,28 +423,74 @@ void FGraphSolve::build_info_EF()
         for (auto node : *neighNodes)
         {
             uint_t indNode = node->get_id();
-            if ( node->get_node_mode() == Node::nodeMode::ANCHOR)
+            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;//TODO robust weight would go here
+            gradientEF_.block<6,1>(indNodesMatrix_[indNode],0) += J;
 
-            // Updating the Hessian
-            Mat6 H = f->get_hessian(indNode);
+            // get the neighboiring nodes TODO and for over them
             uint_t startingIndex = indNodesMatrix_[indNode];
-            // 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 (auto node2 : *neighNodes)
             {
-                for (uint_t j = i; j<6; j++)
+                // 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++)
                 {
-                    // 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+ j, H(i,j)));
+                    // 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)));
+                    }
                 }
+
             }
+
+
         }
     }
 
@@ -534,3 +581,36 @@ MatX1 FGraphSolve::get_chi2_array()
 
     return results;
 }
+
+
+std::vector<bool> FGraphSolve::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<bool> 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<bool> FGraphSolve::get_factors_robust_mask()
+{
+    std::vector<bool> results;
+    results.reserve(factors_.size());
+
+    bool mask;
+    for (auto f : factors_)
+    {
+        mask = f->get_robust_mask();
+        results.push_back(mask);
+    }
+
+    return results;
+}

src/FGraph/mrob/factor.hpp

@@ -160,7 +160,14 @@ class Factor{
      *  matrix W when building the problem.
      */
 
-    matData_t evaluate_robust_weight(matData_t u, matData_t params = 0.0);
+    virtual matData_t evaluate_robust_weight(matData_t u = 0.0, matData_t threshold = 1.0);
+
+    /**
+     * Return the mask if the Robust factor used:
+     *  - False if the function was not used (inlier)
+     *  - True if the threshold was exceed and the function was applied (outlier)
+    */
+    bool get_robust_mask() const {return robust_mask_;}
 
 protected:
     factor_id_t id_;
@@ -176,6 +183,7 @@ class Factor{
     // Robust factor weighting the "iteratively weighted LSQ"
     robustFactorType robust_type_;
     matData_t robust_weight_; // dp/du 1/u or influence by the inverse of the distance u
+    bool robust_mask_;// mask set to true if the robust function is used to clip.
 
     /**
      * TODO: for ransac factors, we can think of the problem as an hypothesis rejection test
@@ -225,7 +233,15 @@ class EigenFactor : public Factor
     virtual ~EigenFactor() = default;
     virtual VectRefConst get_state() const = 0;
     virtual void add_point(const Mat31& p, std::shared_ptr<Node> &node, mrob::matData_t &W) = 0;
-    virtual MatRefConst get_hessian(mrob::factor_id_t id = 0) const = 0;
+    /**
+     * Function that calcualtes the block Hessian of the EF, corresponding to nodes i and j
+     *    Output: - bool flag indicating if there is such block matrix
+     *            - By reference matrix H, with the value
+     * 
+     *    Input:  - index i and j corresponding to the node id's the the EF is pointing to
+     * 
+    */
+    virtual bool get_hessian(MatRef H, mrob::factor_id_t id_i = 0,mrob::factor_id_t id_j = 0) const = 0;
     virtual void add_points_array(const MatX &P, std::shared_ptr<Node> &node, mrob::matData_t &W) = 0;
     virtual void add_points_S_matrix(const Mat4 &S, std::shared_ptr<Node> &node, mrob::matData_t &W) = 0;
 

src/FGraph/mrob/factor_graph_solve.hpp

@@ -149,6 +149,22 @@ class FGraphSolve: public FGraph
      * 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 Factor Id.
+    */
+    std::vector<bool> get_eigen_factors_robust_mask();
+    /**
+     * Returns a vector (python list)
+     * - True if the robust mask was applied
+     * - False if the robust factor had not effect.
+     *
+     * The index in the graph is the factor Id
+    */
+    std::vector<bool> get_factors_robust_mask();
 
 protected:
     /**

src/common/mrob/matrix_base.hpp

@@ -84,6 +84,7 @@ using Mat = Eigen::Matrix<matData_t, Rw, Col, Eigen::RowMajor>;
 //we might need this for aligment requirements: Eigen::Ref<const MatX1, Eigen::AlignmentType::Aligned16>
 using VectRefConst = const Eigen::Ref<const MatX1>;
 using MatRefConst = const Eigen::Ref<const MatX>;
+using MatRef = Eigen::Ref<MatX>;
 
 }//end of namespace
 

src/geometry/SE3.cpp

@@ -361,4 +361,4 @@ std::string SE3::toString() const
     std::stringstream ss;
     ss << this->T_;
     return ss.str();
-}
+}

src/plane-surfaces/CMakeLists.txt

@@ -7,30 +7,35 @@ SET(sources
     plane_registration.cpp
     create_points.cpp
     estimate_plane.cpp
+    utils_lie_differentiation.cpp
     factors/nodePlane4d.cpp
     factors/factor1Pose1Plane4d.cpp
-    factors/EigenFactorPlane.cpp
     factors/EigenFactorPlaneCenter.cpp
-    factors/EigenFactorPlaneRaw.cpp
     factors/EigenFactorPoint.cpp
     factors/PiFactorPlane.cpp
-    factors/EigenFactorPlaneCoordinatesAlign.cpp
+    factors/EigenFactorPlaneBase.cpp
+    factors/EigenFactorPlaneDense.cpp
+    factors/BaregEFPlane.cpp
+    factors/EigenFactorPlaneCenter2.cpp
 )
 
+
 # extra header files
 SET(headers
     mrob/plane.hpp
     mrob/plane_registration.hpp
     mrob/create_points.hpp
     mrob/estimate_plane.hpp
+    mrob/utils_lie_differentiation.hpp
     mrob/factors/nodePlane4d.hpp
     mrob/factors/factor1Pose1plane4d.hpp
-    mrob/factors/EigenFactorPlane.hpp
     mrob/factors/EigenFactorPlaneCenter.hpp
-    mrob/factors/EigenFactorPlaneRaw.hpp
     mrob/factors/EigenFactorPoint.hpp
     mrob/factors/PiFactorPlane.hpp
-    mrob/factors/EigenFactorPlaneCoordinatesAlign.hpp
+    mrob/factors/EigenFactorBase.hpp
+    mrob/factors/EigenFactorDense.hpp
+    mrob/factors/BaregEFPlane.hpp
+    mrob/factors/EigenFactorPlaneCenter2.hpp
 )
 
 # create library

src/plane-surfaces/README.md

@@ -9,6 +9,12 @@ There are also some routines dedicated to generate sinthetic planes or estimate_
 ## Dependencies
 C++'14, Eigen
 
+## Current implementations
+* Eigen-Factor
+* Eigen-Factor-Centered (to be deprecated)
+* Eigen-Factor-Raw (to be deprecated)
+* pi-factor
+* BAREG
 
 ## To be deprecated (now solved by the FGraph):
 plane_registration.cpp