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
Hide whitespace changes
Inline Side-by-side
include/core/ceres_wrapper/qr_manager.h
+ 7
− 7
View file @ b4cb9a90
...@@ -36,23 +36,23 @@ class QRManager : public SolverManager ...@@ -36,23 +36,23 @@ class QRManager : public SolverManager
virtual std::string solve(const unsigned int& _report_level); 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: private:
bool computeDecomposition(); 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); void relinearizeFactor(FactorBasePtr _fac_ptr);
}; };
......
include/core/solver_suitesparse/sparse_utils.h
+ 2
− 2
View file @ b4cb9a90
...@@ -16,12 +16,12 @@ namespace wolf ...@@ -16,12 +16,12 @@ namespace wolf
void eraseBlockRow(Eigen::SparseMatrixd& A, const unsigned int& _row, const unsigned int& _n_rows) 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) 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) 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
View file @ b4cb9a90
...@@ -42,7 +42,7 @@ std::string QRManager::solve(const unsigned int& _report_level) ...@@ -42,7 +42,7 @@ std::string QRManager::solve(const unsigned int& _report_level)
// update state blocks // update state blocks
for (auto sb_pair : sb_2_col_) 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) if (_report_level == 1)
return std::string("Success!\n"); return std::string("Success!\n");
...@@ -102,14 +102,14 @@ bool QRManager::computeDecomposition() ...@@ -102,14 +102,14 @@ bool QRManager::computeDecomposition()
for (auto sb_pair : sb_2_col_) for (auto sb_pair : sb_2_col_)
{ {
sb_2_col_[sb_pair.first] = state_size; 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; unsigned int meas_size = 0;
for (auto fac_pair : fac_2_row_) for (auto fac_pair : fac_2_row_)
{ {
fac_2_row_[fac_pair.first] = meas_size; 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 // resize and setZero A, b
...@@ -138,7 +138,7 @@ bool QRManager::computeDecomposition() ...@@ -138,7 +138,7 @@ bool QRManager::computeDecomposition()
return true; return true;
} }
void QRManager::addFactor(FactorBasePtr _fac_ptr) void QRManager::addFactor(const FactorBasePtr& _fac_ptr)
{ {
//std::cout << "add factor " << _fac_ptr->id() << std::endl; //std::cout << "add factor " << _fac_ptr->id() << std::endl;
assert(fac_2_row_.find(_fac_ptr) == fac_2_row_.end() && "adding existing factor"); assert(fac_2_row_.find(_fac_ptr) == fac_2_row_.end() && "adding existing factor");
...@@ -154,7 +154,7 @@ void QRManager::addFactor(FactorBasePtr _fac_ptr) ...@@ -154,7 +154,7 @@ void QRManager::addFactor(FactorBasePtr _fac_ptr)
pending_changes_ = true; 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; //std::cout << "remove factor " << _fac_ptr->id() << std::endl;
assert(fac_2_row_.find(_fac_ptr) != fac_2_row_.end() && "removing unknown factor"); assert(fac_2_row_.find(_fac_ptr) != fac_2_row_.end() && "removing unknown factor");
...@@ -164,7 +164,7 @@ void QRManager::removeFactor(FactorBasePtr _fac_ptr) ...@@ -164,7 +164,7 @@ void QRManager::removeFactor(FactorBasePtr _fac_ptr)
pending_changes_ = true; 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; //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"); 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) ...@@ -175,7 +175,7 @@ void QRManager::addStateBlock(StateBlockPtr _st_ptr)
pending_changes_ = true; 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; //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"); 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) ...@@ -189,7 +189,7 @@ void QRManager::removeStateBlock(StateBlockPtr _st_ptr)
pending_changes_ = true; 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; //std::cout << "update state block " << _st_ptr.get() << std::endl;
if (_st_ptr->isFixed()) if (_st_ptr->isFixed())
...@@ -202,35 +202,35 @@ void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr) ...@@ -202,35 +202,35 @@ void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
addStateBlock(_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 // // evaluate factor
std::vector<const double*> fac_states_ptr; // std::vector<const double*> fac_states_ptr;
for (auto sb : _fac_ptr->getStateBlockPtrVector()){ // for (auto sb : _fac_ptr->getStateBlockPtrVector()){
fac_states_ptr.push_back(sb->getStateData()); // fac_states_ptr.push_back(sb->getStateData());
} // }
//
Eigen::VectorXd residual(_fac_ptr->getSize()); // Eigen::VectorXd residual(_fac_ptr->getSize());
std::vector<Eigen::MatrixXd> jacobians; // std::vector<Eigen::MatrixXd> jacobians;
_fac_ptr->evaluate(fac_states_ptr,residual,jacobians); // _fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
//
// Fill jacobians // // Fill jacobians
Eigen::SparseMatrixd A_block_row(_fac_ptr->getSize(), A_.cols()); // Eigen::SparseMatrixd A_block_row(_fac_ptr->getSize(), A_.cols());
for (auto i = 0; i < jacobians.size(); i++) // for (auto i = 0; i < jacobians.size(); i++)
{ // {
if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed()) // 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(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(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 // // 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]]); // insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]]);
} // }
} // }
//
assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]); // assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]);
//
// Fill residual // // Fill residual
b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()) = residual; // b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()) = residual;
} }
} /* namespace wolf */ } /* 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