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

Fix some API issues and add FIXMEs about state sizes

parent b9dc68dc
No related branches found
No related tags found
1 merge request!344WIP: Resolve "Fixing and improving QR manager"
Pipeline #5014 passed
This commit is part of merge request !344. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
include/core/ceres_wrapper/qr_manager.h
+ 7
− 7
Edit View file @ b4cb9a90
......@@ -36,23 +36,23 @@ class QRManager : public SolverManager
virtual std::string solve(const unsigned int& _report_level);
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS);
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list);
virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list) override;
private:
bool computeDecomposition();
virtual void addFactor(FactorBasePtr _fac_ptr);
virtual void addFactor(const FactorBasePtr& _fac_ptr) override;
virtual void removeFactor(FactorBasePtr _fac_ptr);
virtual void removeFactor(const FactorBasePtr& _fac_ptr) override;
virtual void addStateBlock(StateBlockPtr _st_ptr);
virtual void addStateBlock(const StateBlockPtr& _st_ptr) override;
virtual void removeStateBlock(StateBlockPtr _st_ptr);
virtual void removeStateBlock(const StateBlockPtr& _st_ptr) override;
virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
virtual void updateStateBlockStatus(const StateBlockPtr& _st_ptr) override;
void relinearizeFactor(FactorBasePtr _fac_ptr);
};
......
include/core/solver_suitesparse/sparse_utils.h
+ 2
− 2
Edit View file @ b4cb9a90
......@@ -16,12 +16,12 @@ namespace wolf
void eraseBlockRow(Eigen::SparseMatrixd& A, const unsigned int& _row, const unsigned int& _n_rows)
{
A.prune([](int i, int, double) { return i >= _row && i < _row + _n_rows; });
A.prune([&](int i, int, double) { return i >= _row && i < _row + _n_rows; });
}
void eraseBlockCol(Eigen::SparseMatrixd& A, const unsigned int& _col, const unsigned int& _n_cols)
{
A.prune([](int, int j, double) { return j >= _col && j < _col + _n_cols; });
A.prune([&](int, int j, double) { return j >= _col && j < _col + _n_cols; });
}
void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
......
src/ceres_wrapper/qr_manager.cpp
+ 36
− 36
Edit View file @ b4cb9a90
......@@ -42,7 +42,7 @@ 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);
sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()), false); // FIXME use LocalParam->Plus()
if (_report_level == 1)
return std::string("Success!\n");
......@@ -102,14 +102,14 @@ bool QRManager::computeDecomposition()
for (auto sb_pair : sb_2_col_)
{
sb_2_col_[sb_pair.first] = state_size;
state_size += sb_pair.first->getSize();
state_size += sb_pair.first->getSize(); // FIXME use 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();
meas_size += fac_pair.first->getSize(); // FIXME use getLocalSize()
}
// resize and setZero A, b
......@@ -138,7 +138,7 @@ bool QRManager::computeDecomposition()
return true;
}
void QRManager::addFactor(FactorBasePtr _fac_ptr)
void QRManager::addFactor(const FactorBasePtr& _fac_ptr)
{
//std::cout << "add factor " << _fac_ptr->id() << std::endl;
assert(fac_2_row_.find(_fac_ptr) == fac_2_row_.end() && "adding existing factor");
......@@ -154,7 +154,7 @@ void QRManager::addFactor(FactorBasePtr _fac_ptr)
pending_changes_ = true;
}
void QRManager::removeFactor(FactorBasePtr _fac_ptr)
void QRManager::removeFactor(const FactorBasePtr& _fac_ptr)
{
//std::cout << "remove factor " << _fac_ptr->id() << std::endl;
assert(fac_2_row_.find(_fac_ptr) != fac_2_row_.end() && "removing unknown factor");
......@@ -164,7 +164,7 @@ void QRManager::removeFactor(FactorBasePtr _fac_ptr)
pending_changes_ = true;
}
void QRManager::addStateBlock(StateBlockPtr _st_ptr)
void QRManager::addStateBlock(const StateBlockPtr& _st_ptr)
{
//std::cout << "add state block " << _st_ptr.get() << std::endl;
assert(sb_2_col_.find(_st_ptr) == sb_2_col_.end() && "adding existing state block");
......@@ -175,7 +175,7 @@ void QRManager::addStateBlock(StateBlockPtr _st_ptr)
pending_changes_ = true;
}
void QRManager::removeStateBlock(StateBlockPtr _st_ptr)
void QRManager::removeStateBlock(const StateBlockPtr& _st_ptr)
{
//std::cout << "remove state block " << _st_ptr.get() << std::endl;
assert(sb_2_col_.find(_st_ptr) != sb_2_col_.end() && "removing unknown state block");
......@@ -189,7 +189,7 @@ void QRManager::removeStateBlock(StateBlockPtr _st_ptr)
pending_changes_ = true;
}
void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
void QRManager::updateStateBlockStatus(const StateBlockPtr& _st_ptr)
{
//std::cout << "update state block " << _st_ptr.get() << std::endl;
if (_st_ptr->isFixed())
......@@ -202,35 +202,35 @@ void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
addStateBlock(_st_ptr);
}
void QRManager::relinearizeFactor(FactorBasePtr _fac_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;
// // 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 */
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment