diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp index 6322b1d547efd6efe1b7d2a2e340e5292a5ff964..5ee0b91d829af1a495b57d935c6ae14ce94d6702 100644 --- a/src/ceres_wrapper/qr_manager.cpp +++ b/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 */