diff --git a/include/core/ceres_wrapper/qr_manager.h b/include/core/ceres_wrapper/qr_manager.h
index 855b5dab5691c7a951de72a169e44ec279a5f348..8608d894d5f5536c28adef9c23dc40eeeba249c6 100644
--- a/include/core/ceres_wrapper/qr_manager.h
+++ b/include/core/ceres_wrapper/qr_manager.h
@@ -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);
};
diff --git a/include/core/solver_suitesparse/sparse_utils.h b/include/core/solver_suitesparse/sparse_utils.h
index 7f5e0a0d0d2a56c9c798509c8b092eba0a35d855..ba372158622f37a5c962ee7f5041c344e4ff2ee2 100644
--- a/include/core/solver_suitesparse/sparse_utils.h
+++ b/include/core/solver_suitesparse/sparse_utils.h
@@ -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)
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index 2a7c21aa363f364a294528dd7a1a1abed7c71177..6322b1d547efd6efe1b7d2a2e340e5292a5ff964 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -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 */