[fix] example of dr/dz grad calculated using sparse matrix product -a…

…lpha*B.T*W*B

src/FGraph/mrob/factor_graph.hpp

@@ -97,7 +97,7 @@ class FGraph{
     * get_node returns the Eigen factor given the node id key, now a position on the data structure
     */
     std::shared_ptr<EigenFactor>& get_eigen_factor(factor_id_t key);
-    void print(bool complete = false) const;
+    virtual void print(bool complete = false) const;
 
 
     /**

src/FGraphDiff/examples/example_solver_2d.cpp

@@ -64,13 +64,20 @@ int main ()
         std::shared_ptr<mrob::DiffFactor> f2(new mrob::Factor2Poses2d_diff(obs,n2,n1,obsInformation));
         diff_factor_idx.emplace_back(graph.add_factor(f2));
 
+        obs << 1, 1, 0;
+        std::shared_ptr<mrob::DiffFactor> gnss_2(new mrob::Factor1Pose2d_diff(obs,n2, obsInformation*1e4));
+        diff_factor_idx.emplace_back(graph.add_factor(gnss_2));
 
         std::shared_ptr<mrob::Node> n3(new mrob::NodePose2d(x));
         graph.add_node(n3);
 
         obs << -1 , -1 , 0;
         std::shared_ptr<mrob::DiffFactor> f3(new mrob::Factor2Poses2d_diff(obs,n3,n2,obsInformation));
         diff_factor_idx.emplace_back(graph.add_factor(f3));
+
+        obs << 2, 2, 0;
+        std::shared_ptr<mrob::DiffFactor> gnss_3(new mrob::Factor1Pose2d_diff(obs,n3,obsInformation*1e4));
+        diff_factor_idx.emplace_back(graph.add_factor(gnss_3));
     }
 
     // solve the Gauss Newton optimization
@@ -108,48 +115,30 @@ int main ()
 
     Eigen::SimplicialLDLT<mrob::SMatCol,Eigen::UpLoType::Lower, Eigen::AMDOrdering<mrob::SMatCol::StorageIndex>> alpha_solve;
     alpha_solve.compute(A.transpose()*W*A);
-    SMatCol rhs(A.rows(),A.cols());
+    SMatCol rhs(A.cols(),A.cols());
     rhs.setIdentity();
     std::cout << rhs << std::endl;
 
-    MatX alpha = alpha_solve.solve(rhs); //
-    std::cout << "\n alpha =\n" << alpha << std::endl;
-
-
-    MatX errors_grads;
-    errors_grads.resize(graph.get_dimension_state(), graph.get_dimension_obs());
+    MatX alpha = alpha_solve.solve(rhs); // get information matrix graph - should be the same #TODO
+    std::cout << "\nalpha =\n" << alpha << std::endl;
 
-    int f_index = 0;
+    MatX info_matrix = graph.get_information_matrix();
+    std::cout << "\ninfo matrix =\n" << info_matrix << std::endl;
 
-    for (uint_t i = 0; i < diff_factor_idx.size(); ++i)
-    {
-        auto f = graph.get_factor(diff_factor_idx[i]);
-        f->evaluate_jacobians();
-        f->evaluate_residuals();
-        f->evaluate_dr_dz();
+    std::cout << "\nA = \n" << MatX(graph.get_adjacency_matrix()) << std::endl;
 
-        auto dr_dz = f->get_dr_dz();
-        std::cout<< "\ndr_dz = " << dr_dz << std::endl;
+    graph.build_dr_dz();
 
-        auto dr_dx = Mat3(f->get_jacobian().block(0,0,3,3));
-        std::cout << "\ndr_dx = " << dr_dx << std::endl;
+    std::cout << "\nA = \n" << MatX(graph.get_adjacency_matrix()) << std::endl;
 
-        auto Wf = f->get_information_matrix();
-        std::cout << "\nW = " << Wf << std::endl;
 
-        auto r = f->get_residual();
-        std::cout << "\nresidual = " << r << std::endl;
+    SMatRow dr_dz_full = graph.get_dr_dz();
+    std::cout << "\nMatrix B aka dr_dz matrix =\n" << MatX(dr_dz_full) << std::endl;
 
-        std::cout << dr_dx*Wf*dr_dz << std::endl;
-
-        auto error = MatX(dr_dx*Wf*dr_dz);
-        std::cout << "\nError value: " << error << std::endl; 
-
-        errors_grads.block(f_index, f_index, f->get_dim_obs(), f->get_dim_obs()) <<  error;
-        f_index += f->get_dim_obs();
-    }
+    MatX errors_grads;
+    errors_grads.resize(graph.get_dimension_state(), graph.get_dimension_obs());
 
-    errors_grads = -alpha*errors_grads;
+    errors_grads = -alpha*dr_dz_full.transpose()*W*dr_dz_full;
 
     std::cout << "\nError_grads = \n" << errors_grads << std::endl;
 

src/FGraphDiff/factor_graph_diff.cpp

@@ -49,4 +49,23 @@ std::shared_ptr<DiffFactor> &mrob::FGraphDiff::get_factor(factor_id_t key)
 {
     assert(key < diff_factors_.size() && "FGraphDiff::get_factor: incorrect key");
     return diff_factors_[key];
+}
+
+void FGraphDiff::print(bool complete) const
+{
+    std::cout << "Status of graph: " <<
+            " Nodes = " << nodes_.size()  <<
+            ", Factors = " << factors_.size() <<
+            ", Diff Factors = " << diff_factors_.size() <<
+            ", Eigen Factors = " << eigen_factors_.size() << std::endl;
+
+    if(complete)
+    {
+        for (auto &&n : nodes_)
+            n->print();
+        for (auto &&f : factors_)
+            f->print();
+        for (auto &&f : diff_factors_)
+            f->print();
+    }
 }

src/FGraphDiff/factor_graph_diff_solve.cpp

@@ -255,6 +255,100 @@ void FGraphDiffSolve::build_index_nodes_matrix()
     }
 }
 
+void FGraphDiffSolve::build_dr_dz()
+{
+
+    indNodesMatrix_.clear();
+    this->build_index_nodes_matrix();
+    assert(N_ == stateDim_ && "FGraphDiffSolve::buildAdjacency: State Dimensions are not coincident\n");
+
+
+    // 2.1) Check for consistency. With 0 observations the problem does not need to be build, EF may still build it
+    if (obsDim_ == 0)
+    {
+        buildAdjacencyFlag_ = false;
+        return;
+    }
+    buildAdjacencyFlag_ = true;
+
+    // 2) resize properly matrices (if needed)
+    // r_.resize(obsDim_,1);//dense vector TODO is it better to reserve and push_back??
+    // A_.resize(obsDim_, stateDim_);//Sparse matrix clears data, but keeps the prev reserved space
+    // W_.resize(obsDim_, obsDim_);//TODO should we reinitialize this all the time? an incremental should be fairly easy
+    //===============================================
+    B_.resize(obsDim_, stateDim_);
+
+    std::vector<uint_t> reservationB;
+    reservationB.reserve( obsDim_ );
+    std::vector<uint_t> reservationW;
+    // reservationW.reserve( obsDim_ );
+    std::vector<factor_id_t> indFactorsMatrix;
+    indFactorsMatrix.reserve(diff_factors_.size());
+    M_ = 0;
+
+    for (uint_t i = 0; i < diff_factors_.size(); ++i)
+    {
+        auto f = diff_factors_[i];
+        f->evaluate_residuals();
+        f->evaluate_jacobians();
+        f->evaluate_chi2();
+        f->evaluate_dr_dz();
+
+        // calculate dimensions for reservation and bookeping vector
+        uint_t dim = f->get_dim_obs();
+        uint_t allDim = f->get_all_nodes_dim();
+        for (uint_t j = 0; j < dim; ++j)
+        {
+            reservationB.push_back(allDim);
+            // reservationW.push_back(dim-j);//XXX this might be allocating more elements than necessary, check
+        }
+        indFactorsMatrix.push_back(M_);
+        M_ += dim;
+    }
+    assert(M_ == obsDim_ && "FGraphDiffSolve::buildAdjacency: Observation dimensions are not coincident\n");
+    B_.reserve(reservationB); //Exact allocation for elements.
+    // W_.reserve(reservationW); //same
+
+    for (factor_id_t i = 0; i < diff_factors_.size(); ++i)
+    {
+        auto f = diff_factors_[i];
+
+        // 4) Get the calculated residual
+        r_.block(indFactorsMatrix[i], 0, f->get_dim_obs(), 1) <<  f->get_residual();
+
+        // 5) build Adjacency matrix as a composition of rows
+        // 5.1) Get the number of nodes involved. It is a vector of nodes
+        auto neighNodes = f->get_neighbour_nodes();
+        // Iterates over the Jacobian row
+        for (uint_t l=0; l < f->get_dim_obs() ; ++l)
+        {
+            uint_t totalK = 0;
+            // Iterates over the number of neighbour Nodes (ordered by construction)
+            for (uint_t j=0; j < neighNodes->size(); ++j)
+            {
+                uint_t dimNode = (*neighNodes)[j]->get_dim();
+                // check for node if it is an anchor node, then skip emplacement of Jacobian in the Adjacency
+                if ((*neighNodes)[j]->get_node_mode() == Node::nodeMode::ANCHOR)
+                {
+                    totalK += dimNode;// we need to account for the dim in the Jacobian, to read the next block
+                    continue;//skip this loop
+                }
+                factor_id_t id = (*neighNodes)[j]->get_id();
+                for(uint_t k = 0; k < dimNode; ++k)
+                {
+                    // order according to the permutation vector
+                    uint_t iRow = indFactorsMatrix[i] + l;
+                    // In release mode, indexes outside will not trigger an exception
+                    uint_t iCol = indNodesMatrix_[id] + k;
+                    // This is an ordered insertion
+                    B_.insert(iRow,iCol) = f->get_dr_dz()(l, k);
+                }
+                totalK += dimNode;
+            }
+        }
+    }
+}
+
 void FGraphDiffSolve::build_adjacency()
 {
     // 1) Node indexes bookkept. We use a map to ensure the index from nodes to the current active_node
@@ -275,12 +369,13 @@ void FGraphDiffSolve::build_adjacency()
     r_.resize(obsDim_,1);//dense vector TODO is it better to reserve and push_back??
     A_.resize(obsDim_, stateDim_);//Sparse matrix clears data, but keeps the prev reserved space
     W_.resize(obsDim_, obsDim_);//TODO should we reinitialize this all the time? an incremental should be fairly easy
-
-
+    B_.resize(obsDim_, stateDim_);
 
     // 3) Evaluate every factor given the current state and bookeeping of DiffFactor indices
     std::vector<uint_t> reservationA;
     reservationA.reserve( obsDim_ );
+    std::vector<uint_t> reservationB;
+    reservationB.reserve( obsDim_ );
     std::vector<uint_t> reservationW;
     reservationW.reserve( obsDim_ );
     std::vector<factor_id_t> indFactorsMatrix;
@@ -292,20 +387,23 @@ void FGraphDiffSolve::build_adjacency()
         f->evaluate_residuals();
         f->evaluate_jacobians();
         f->evaluate_chi2();
+        f->evaluate_dr_dz();
 
         // calculate dimensions for reservation and bookeping vector
         uint_t dim = f->get_dim_obs();
         uint_t allDim = f->get_all_nodes_dim();
         for (uint_t j = 0; j < dim; ++j)
         {
             reservationA.push_back(allDim);
+            reservationB.push_back(allDim);
             reservationW.push_back(dim-j);//XXX this might be allocating more elements than necessary, check
         }
         indFactorsMatrix.push_back(M_);
         M_ += dim;
     }
     assert(M_ == obsDim_ && "FGraphDiffSolve::buildAdjacency: Observation dimensions are not coincident\n");
     A_.reserve(reservationA); //Exact allocation for elements.
+    B_.reserve(reservationB);
     W_.reserve(reservationW); //same
 
 
@@ -343,6 +441,7 @@ void FGraphDiffSolve::build_adjacency()
                     uint_t iCol = indNodesMatrix_[id] + k;
                     // This is an ordered insertion
                     A_.insert(iRow,iCol) = f->get_jacobian()(l, k + totalK);
+                    B_.insert(iRow,iCol) = f->get_dr_dz()(l, k);
                 }
                 totalK += dimNode;
             }
@@ -365,8 +464,6 @@ void FGraphDiffSolve::build_adjacency()
             }
         }
     } //end factors loop
-
-
 }
 
 void FGraphDiffSolve::build_info_adjacency()

src/FGraphDiff/factors/factor1Pose2d_diff.cpp

@@ -54,7 +54,7 @@ void Factor1Pose2d_diff::evaluate_chi2()
 
 void Factor1Pose2d_diff::evaluate_dr_dz()
 {
-    dr_dz_.setIdentity();
+    dr_dz_.setIdentity(3,3);
     dr_dz_ *= -1;
 }
 

src/FGraphDiff/factors/factor2Poses2d_diff.cpp

@@ -97,7 +97,7 @@ void Factor2Poses2d_diff::evaluate_chi2()
 
 void mrob::Factor2Poses2d_diff::evaluate_dr_dz()
 {
-    dr_dz_.setIdentity();
+    dr_dz_.setIdentity(3,3);
     dr_dz_ *= -1;
 }
 

src/FGraphDiff/mrob/factor_graph_diff.hpp

@@ -79,6 +79,7 @@ class FGraphDiff : public FGraph{
 
     factor_id_t number_diff_factors() {return diff_factors_.size();};
 
+    void print(bool complete = false) const override;
 protected:
     std::deque<std::shared_ptr<DiffFactor> > diff_factors_; // no specific order needed
 };

src/FGraphDiff/mrob/factor_graph_diff_solve.hpp

@@ -136,6 +136,9 @@ class FGraphDiffSolve: public FGraphDiff
      * TODO If true, it re-evaluates the problem
      */
     SMatCol get_adjacency_matrix() { return A_;}
+    void build_dr_dz();
+    SMatRow get_dr_dz() {return B_;} 
+
     /**
      * Returns a copy to the W matrix.
      * There is a conversion (implies copy) from Row to Col-convention (which is what np.array needs)
@@ -187,7 +190,6 @@ class FGraphDiffSolve: public FGraphDiff
      */
     void build_info_adjacency();
     void build_schur(); // TODO
-
     /**
      * Once the matrix L is generated, it solves the linearized LSQ
      * by using the Gauss-Newton algorithm
@@ -256,6 +258,7 @@ class FGraphDiffSolve: public FGraphDiff
     std::unordered_map<factor_id_t, factor_id_t > indNodesMatrix_;
 
     SMatRow A_; //Adjacency matrix, as a Row sparse matrix. The reason is for filling in row-fashion for each factor
+    SMatRow B_; // block matrix for dr_dz jacobians of factors
     SMatRow W_; //A block diagonal information matrix. For types Adjacency it calculates its block transposed squared root
     MatX1 r_; // Residuals as given by the factors