WIP: Resolve "Fixing and improving QR manager"

Closes #295

src/ceres_wrapper/qr_manager.cpp

@@ -204,33 +204,55 @@ 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
 {
-//    // evaluate factor
-//    std::vector<const double*> fac_states_ptr;
-//    for (auto sb : _fac_ptr->getStateBlockPtrVector()){
-//        fac_states_ptr.push_back(sb->getStateData());
-//    }
-//
-//    Eigen::VectorXd residual(_fac_ptr->getSize());
-//    std::vector<Eigen::MatrixXd> jacobians;
-//    _fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
-//
-//    // Fill 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())
-//        {
-//            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");
-//            // 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]]);
-//        }
-//    }
-//
-//    assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]);
-//
-//    // Fill residual
-//    b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()) = residual;
+    /* 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());
+    }
+
+    Eigen::VectorXd residual(_fac_ptr->getSize());
+    std::vector<Eigen::MatrixXd> jacobians;
+    _fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
+
+    // Fill 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())
+        {
+            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");
+            // 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]]);
+        }
+    }
+
+    assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]);
+
+    // Fill residual
+    b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()) = residual;
 }
 
 } /* namespace wolf */