clean up: updated authors in comments, removed redundant headers and …

…eigen_factor_diff class/ methods

src/FGraphDiff/CMakeLists.txt

@@ -1,18 +1,12 @@
-# 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
 )
 
 # extra source files
 SET(sources
-    ../FGraph/node.cpp
     factor_diff.cpp
     factor_graph_diff.cpp
     factor_graph_diff_solve.cpp

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;
 

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 
  */

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<DiffFactor> &factor)
     return factor->get_id();
 }
 
-factor_id_t FGraphDiff::add_eigen_factor(std::shared_ptr<DiffEigenFactor> &factor)
-{
-    factor->set_id(eigen_factors_.size());
-    eigen_factors_.emplace_back(factor);
-    return factor->get_id();
-}

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 <iostream>
 #include <Eigen/SparseCore>
@@ -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<Eigen::Upper>() * A_);
         b_ = A_.transpose() * W_.selfadjointView<Eigen::Upper>() * r_;
     }
-
-    // If any EF, we should combine both solutions
-    if (eigen_factors_.size() > 0 )
-    {
-        if (buildAdjacencyFlag_)
-        {
-            L_ += hessianEF_.selfadjointView<Eigen::Upper>();
-            b_ += gradientEF_;
-        }
-        // case when there are pure EF and no other factor
-        else
-        {
-            L_ = hessianEF_.selfadjointView<Eigen::Upper>();
-            b_ = gradientEF_;
-        }
-    }
-}
-
-
-void FGraphDiffSolve::build_info_EF()
-{
-    gradientEF_.resize(stateDim_,1);
-    gradientEF_.setZero();
-    std::vector<Triplet> 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<bool> 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<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> FGraphDiffSolve::get_factors_robust_mask()
 {
     std::vector<bool> results;

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<MatRefConst> mrob::Factor1Pose2d_diff::get_d2r_dx_dz() const
+// {
+//     std::vector<MatRefConst> 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();

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 <iostream>
+#include <vector>
 #include <mrob/factors/factor2Poses2d_diff.hpp>
 
 
@@ -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<MatRefConst> Factor2Poses2d_diff::get_d2r_dx_dz()
+// {
+//     std::vector<Mat3> 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
 {