diff --git a/src/FGraph/mrob/factor_graph.hpp b/src/FGraph/mrob/factor_graph.hpp index b11ccc4..dbb6c6b 100644 --- a/src/FGraph/mrob/factor_graph.hpp +++ 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& get_eigen_factor(factor_id_t key); - void print(bool complete = false) const; + virtual void print(bool complete = false) const; /** diff --git a/src/FGraphDiff/examples/example_solver_2d.cpp b/src/FGraphDiff/examples/example_solver_2d.cpp index 6286a52..f19b9e5 100644 --- a/src/FGraphDiff/examples/example_solver_2d.cpp +++ b/src/FGraphDiff/examples/example_solver_2d.cpp @@ -64,6 +64,9 @@ int main () std::shared_ptr 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 gnss_2(new mrob::Factor1Pose2d_diff(obs,n2, obsInformation*1e4)); + diff_factor_idx.emplace_back(graph.add_factor(gnss_2)); std::shared_ptr n3(new mrob::NodePose2d(x)); graph.add_node(n3); @@ -71,6 +74,10 @@ int main () obs << -1 , -1 , 0; std::shared_ptr 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 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> 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; diff --git a/src/FGraphDiff/factor_graph_diff.cpp b/src/FGraphDiff/factor_graph_diff.cpp index 3b69cb1..d7b1a48 100644 --- a/src/FGraphDiff/factor_graph_diff.cpp +++ b/src/FGraphDiff/factor_graph_diff.cpp @@ -49,4 +49,23 @@ std::shared_ptr &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(); + } } \ No newline at end of file diff --git a/src/FGraphDiff/factor_graph_diff_solve.cpp b/src/FGraphDiff/factor_graph_diff_solve.cpp index 1aff1fe..75b33d6 100644 --- a/src/FGraphDiff/factor_graph_diff_solve.cpp +++ b/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 reservationB; + reservationB.reserve( obsDim_ ); + std::vector reservationW; + // reservationW.reserve( obsDim_ ); + std::vector 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 reservationA; reservationA.reserve( obsDim_ ); + std::vector reservationB; + reservationB.reserve( obsDim_ ); std::vector reservationW; reservationW.reserve( obsDim_ ); std::vector indFactorsMatrix; @@ -292,6 +387,7 @@ 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(); @@ -299,6 +395,7 @@ void FGraphDiffSolve::build_adjacency() 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_); @@ -306,6 +403,7 @@ void FGraphDiffSolve::build_adjacency() } 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() diff --git a/src/FGraphDiff/factors/factor1Pose2d_diff.cpp b/src/FGraphDiff/factors/factor1Pose2d_diff.cpp index 641aebd..0cdbd8d 100644 --- a/src/FGraphDiff/factors/factor1Pose2d_diff.cpp +++ b/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; } diff --git a/src/FGraphDiff/factors/factor2Poses2d_diff.cpp b/src/FGraphDiff/factors/factor2Poses2d_diff.cpp index 4733ea3..15f11b1 100644 --- a/src/FGraphDiff/factors/factor2Poses2d_diff.cpp +++ b/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; } diff --git a/src/FGraphDiff/mrob/factor_graph_diff.hpp b/src/FGraphDiff/mrob/factor_graph_diff.hpp index 8517034..1249188 100644 --- a/src/FGraphDiff/mrob/factor_graph_diff.hpp +++ b/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 > diff_factors_; // no specific order needed }; diff --git a/src/FGraphDiff/mrob/factor_graph_diff_solve.hpp b/src/FGraphDiff/mrob/factor_graph_diff_solve.hpp index 13193be..4fd090d 100644 --- a/src/FGraphDiff/mrob/factor_graph_diff_solve.hpp +++ b/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 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