Adapted QR solver to manifold use + small fixes

include/core/ceres_wrapper/qr_manager.h

@@ -14,14 +14,19 @@
 namespace wolf
 {
 
+WOLF_PTR_TYPEDEFS(QRManager);
+
+
 class QRManager : public SolverManager
 {
     protected:
         Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::COLAMDOrdering<int>> solver_;
         Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::NaturalOrdering<int>> covariance_solver_;
-        Eigen::SparseMatrixd A_;
+        Eigen::SparseMatrix<double, Eigen::RowMajor> A_;
         Eigen::VectorXd b_;
+        // Stores the first column number in the Jacobian matrix of the block associated to a state block 
         std::map<StateBlockPtr, unsigned int> sb_2_col_;
+        // Stores the first row number in the Jacobian matrix of the block associated to a factor 
         std::map<FactorBasePtr, unsigned int> fac_2_row_;
         bool any_state_block_removed_;
         unsigned int new_state_blocks_;
@@ -39,6 +44,8 @@ class QRManager : public SolverManager
         virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
 
         virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list) override;
+        
+        Eigen::SparseMatrixd getHessian();
 
     private:
 

src/ceres_wrapper/qr_manager.cpp

@@ -42,7 +42,22 @@ std::string QRManager::solve(const unsigned int& _report_level)
 
     // update state blocks
     for (auto sb_pair : sb_2_col_)
-        sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()), false); // FIXME use LocalParam->Plus()
+    {
+        StateBlockPtr sb = sb;
+        Eigen::VectorXd sb_x_incr = x_incr.segment(sb_pair.second, sb->getLocalSize());
+        if (sb->hasLocalParametrization()){
+            Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize());
+            Eigen::Map<const Eigen::VectorXd> sb_x_incr_map(sb_x_incr.data(), sb->getLocalSize());
+            Eigen::VectorXd new_sb_state(sb->getSize());
+            Eigen::Map<Eigen::VectorXd> new_sb_state_map(new_sb_state.data(), sb->getSize());
+            
+            sb->getLocalParametrization()->plus(sb_state_map, sb_x_incr_map, new_sb_state_map);
+            sb->setState(new_sb_state , false);
+        }
+        else {
+            sb->setState(sb->getState() + sb_x_incr, false);
+        }
+    }
 
     if (_report_level == 1)
         return std::string("Success!\n");
@@ -52,6 +67,10 @@ std::string QRManager::solve(const unsigned int& _report_level)
     return std::string();
 }
 
+Eigen::SparseMatrixd QRManager::getHessian(){
+    return A_.transpose() * A_;
+}
+
 void QRManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
 {
     // TODO
@@ -102,14 +121,16 @@ bool QRManager::computeDecomposition()
             for (auto sb_pair : sb_2_col_)
             {
                 sb_2_col_[sb_pair.first] = state_size;
-                state_size += sb_pair.first->getSize(); // FIXME use getLocalSize()
+                // column size is the same dimension as the state block tangent space
+                state_size += sb_pair.first->getLocalSize();
             }
 
             unsigned int meas_size = 0;
             for (auto fac_pair : fac_2_row_)
             {
                 fac_2_row_[fac_pair.first] = meas_size;
-                meas_size += fac_pair.first->getSize(); // FIXME use getLocalSize()
+                // factor residual are so far all euclidean quantities
+                meas_size += fac_pair.first->getSize();
             }
 
             // resize and setZero A, b
@@ -204,51 +225,46 @@ void QRManager::updateStateBlockStatus(const StateBlockPtr& _st_ptr)
 
 void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr) // FIXME consider the Jacobian 'M' of the local param of each SB in the factor
 {
-    /* Things to do:
-     *
-     * 1. For each SB, obtain or create, then store in a vector, this data:
-     *     - state vector
-     *     - size, and local size
-     *     - if local param, get Jacobian M
-     *     - else, M = I, or set a convenient flag sb_has_jac = false
-     * 2. Evaluate factor, obtain residual and Jacobians
-     *     - evaluate(fac_states_ptr,residual,jacobians)
-     *     - for each SB i
-     *         - if sb_has_jac[i] then J[i] = M[i] * jacobians[i]
-     *     - else
-     *         - J[i] = jacobians[i]
-     * 3. Fill A and b
-     *     - for each SB i
-     *         - insert J[i] in matrix A, in row A_block_row
-     *     - insert residual in vector b
-     */
-
-
-
-
     // evaluate factor
     std::vector<const double*> fac_states_ptr;
     for (auto sb : _fac_ptr->getStateBlockPtrVector()){
         fac_states_ptr.push_back(sb->getStateData());
     }
 
+    // retrieve residual value
     Eigen::VectorXd residual(_fac_ptr->getSize());
-    std::vector<Eigen::MatrixXd> jacobians;
-    _fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
 
-    // Fill jacobians
+    // retrieve jacobian in group size, not local size
+    std::vector<Eigen::MatrixXd> jacobians;
+    
+    _fac_ptr->evaluate(fac_states_ptr, residual, jacobians);
+    // Fill the factor block jacobians
     Eigen::SparseMatrixd A_block_row(_fac_ptr->getSize(), A_.cols());
     for (auto i = 0; i < jacobians.size(); i++)
     {
-    if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed())
+        StateBlockPtr sb = _fac_ptr->getStateBlockPtrVector()[i];
+        if (!sb->isFixed())
         {
-            assert(sb_2_col_.find(_fac_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "factor involving a state block not added");
-            assert(A_.cols() >= sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && "bad A number of cols");
+            assert(sb_2_col_.find(sb) != sb_2_col_.end() && "factor involving a state block not added");
+            assert(A_.cols() >= sb_2_col_[sb] + jacobians[i].cols() - 1 && "bad A number of cols");
+
             // insert since A_block_row has just been created so it's empty for sure
-            insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]]);
+            if (sb->hasLocalParametrization()){
+                // if the state block has a local parameterization, we need to right multiply by the manifold element / tangent element jacobian
+                Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize());
+                Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(J_manif_tang.data(), sb->getSize(), sb->getLocalSize());
+                Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize());
+
+                sb->getLocalParametrization()->computeJacobian(sb_state_map, J_manif_tang_map);
+                insertSparseBlock(jacobians[i] * J_manif_tang, A_block_row, 0, sb_2_col_[sb]);  // (to_insert, matrix_to_fill, row, col)
+            }
+            else {
+                insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[sb]);
+            }
         }
     }
 
+    // void assignBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const Eigen::SparseMatrix<double, Eigen::RowMajor>& ins, const unsigned int& _row)
     assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]);
 
     // Fill residual