Skip to content
Snippets Groups Projects
Commit 74332b0a authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Add TD help

parent b4cb9a90
No related branches found
No related tags found
1 merge request!344WIP: Resolve "Fixing and improving QR manager"
Pipeline #5015 failed
This commit is part of merge request !344. Comments created here will be created in the context of that merge request.
...@@ -204,33 +204,55 @@ void QRManager::updateStateBlockStatus(const StateBlockPtr& _st_ptr) ...@@ -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 void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr) // FIXME consider the Jacobian 'M' of the local param of each SB in the factor
{ {
// // evaluate factor /* Things to do:
// std::vector<const double*> fac_states_ptr; *
// for (auto sb : _fac_ptr->getStateBlockPtrVector()){ * 1. For each SB, obtain or create, then store in a vector, this data:
// fac_states_ptr.push_back(sb->getStateData()); * - state vector
// } * - size, and local size
// * - if local param, get Jacobian M
// Eigen::VectorXd residual(_fac_ptr->getSize()); * - else, M = I, or set a convenient flag sb_has_jac = false
// std::vector<Eigen::MatrixXd> jacobians; * 2. Evaluate factor, obtain residual and Jacobians
// _fac_ptr->evaluate(fac_states_ptr,residual,jacobians); * - evaluate(fac_states_ptr,residual,jacobians)
// * - for each SB i
// // Fill jacobians * - if sb_has_jac[i] then J[i] = M[i] * jacobians[i]
// Eigen::SparseMatrixd A_block_row(_fac_ptr->getSize(), A_.cols()); * - else
// for (auto i = 0; i < jacobians.size(); i++) * - J[i] = jacobians[i]
// { * 3. Fill A and b
// if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed()) * - for each SB i
// { * - insert J[i] in matrix A, in row A_block_row
// assert(sb_2_col_.find(_fac_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "factor involving a state block not added"); * - insert residual in vector b
// 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]]);
// }
// }
// // evaluate factor
// assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]); std::vector<const double*> fac_states_ptr;
// for (auto sb : _fac_ptr->getStateBlockPtrVector()){
// // Fill residual fac_states_ptr.push_back(sb->getStateData());
// b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()) = residual; }
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 */ } /* namespace wolf */
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment