diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h
index d156fd80521b31da81975ea11f55d9949fbb3754..2bdc1d4bd002deaa4e01edfac6817618fae40951 100644
--- a/include/base/ceres_wrapper/ceres_manager.h
+++ b/include/base/ceres_wrapper/ceres_manager.h
@@ -56,7 +56,7 @@ class CeresManager : public SolverManager
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
= CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
- virtual void computeCovariances(const StateBlockPtrList& st_list) override;
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) override;
virtual bool hasConverged() override;
diff --git a/include/base/ceres_wrapper/qr_manager.h b/include/base/ceres_wrapper/qr_manager.h
index d4945e066a372d5ec95578a87552e4369eec1b2e..39d133d610fb2f497c88c70781cd3c047dba81d2 100644
--- a/include/base/ceres_wrapper/qr_manager.h
+++ b/include/base/ceres_wrapper/qr_manager.h
@@ -38,7 +38,7 @@ class QRManager : public SolverManager
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
- virtual void computeCovariances(const StateBlockPtrList& _sb_list);
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list);
private:
diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h
index 28e43acfa63426072e6883b2a3237535775d1fc8..8aefa72135e3897c6a52ac550a131db782ea7860 100644
--- a/include/base/solver/solver_manager.h
+++ b/include/base/solver/solver_manager.h
@@ -53,7 +53,7 @@ public:
virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
- virtual void computeCovariances(const StateBlockPtrList& st_list) = 0;
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0;
virtual bool hasConverged() = 0;
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 5871b8fa12ee6f9ce5c3aff8741e80444ab9e166..c8cebf9f4279c3e1801c93a2a7e2cccf14493ce5 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -62,7 +62,7 @@ std::string CeresManager::solveImpl(const ReportVerbosity report_level)
}
void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
-{
+{
// update problem
update();
@@ -194,17 +194,16 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
// STORE DESIRED COVARIANCES
for (unsigned int i = 0; i < double_pairs.size(); i++)
{
- Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
- covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize());
+ covariance_->GetCovarianceBlockInTangentSpace(double_pairs[i].first, double_pairs[i].second, cov.data());
wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
- //std::cout << "getted covariance " << std::endl << cov << std::endl;
+ // std::cout << "covariance got switch: " << std::endl << cov << std::endl;
}
}
else
std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
}
-
-void CeresManager::computeCovariances(const StateBlockPtrList& st_list)
+void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list)
{
//std::cout << "CeresManager: computing covariances..." << std::endl;
@@ -234,10 +233,10 @@ void CeresManager::computeCovariances(const StateBlockPtrList& st_list)
// STORE DESIRED COVARIANCES
for (unsigned int i = 0; i < double_pairs.size(); i++)
{
- Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getSize(),state_block_pairs[i].second->getSize());
- covariance_->GetCovarianceBlock(double_pairs[i].first, double_pairs[i].second, cov.data());
+ Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize());
+ covariance_->GetCovarianceBlockInTangentSpace(double_pairs[i].first, double_pairs[i].second, cov.data());
wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
- //std::cout << "getted covariance " << std::endl << cov << std::endl;
+ // std::cout << "covariance got from st_list: " << std::endl << cov << std::endl;
}
else
std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 484fa4458dcc256c3ac002459ef700bace62b1c0..ecc363161e3fd128dce561321a659b0df48f5ea7 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -450,8 +450,8 @@ void Problem::clearCovariance()
void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov)
{
- assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
- assert(_state2->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
+ assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
+ assert(_state2->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
@@ -459,8 +459,8 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, c
void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _cov)
{
- assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
- assert(_state1->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
+ assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
+ assert(_state1->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_[std::make_pair(_state1, _state1)] = _cov;
@@ -472,23 +472,23 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
//std::cout << "entire cov to be filled:" << std::endl << _cov << std::endl;
//std::cout << "_row " << _row << std::endl;
//std::cout << "_col " << _col << std::endl;
- //std::cout << "_state1 size: " << _state1->getSize() << std::endl;
- //std::cout << "_state2 size: " << _state2->getSize() << std::endl;
- //std::cout << "part of cov to be filled:" << std::endl << _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) << std::endl;
+ //std::cout << "_state1 tangent space size: " << _state1->getLocalSize() << std::endl;
+ //std::cout << "_state2 tangent space size: " << _state2->getLocalSize() << std::endl;
+ //std::cout << "part of cov to be filled:" << std::endl << _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) << std::endl;
//if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
// std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] << std::endl;
//else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
// std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose() << std::endl;
- assert(_row + _state1->getSize() <= _cov.rows() && _col + _state2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+ assert(_row + _state1->getLocalSize() <= _cov.rows() && _col + _state2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
std::lock_guard<std::mutex> lock(mut_covariances_);
if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
- _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) =
+ _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)];
else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
- _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) =
+ _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose();
else
{
@@ -515,23 +515,23 @@ bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx
// search st1 & st2
if (covariances_.find(pair_12) != covariances_.end())
{
- assert(_sb_2_idx[sb1] + sb1->getSize() <= _cov.rows() &&
- _sb_2_idx[sb2] + sb2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
- assert(_sb_2_idx[sb2] + sb2->getSize() <= _cov.rows() &&
- _sb_2_idx[sb1] + sb1->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+ assert(_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.rows() &&
+ _sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+ assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() &&
+ _sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
- _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getSize(), sb2->getSize()) = covariances_[pair_12];
- _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getSize(), sb1->getSize()) = covariances_[pair_12].transpose();
+ _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_[pair_12];
+ _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_[pair_12].transpose();
}
else if (covariances_.find(pair_21) != covariances_.end())
{
- assert(_sb_2_idx[sb1] + sb1->getSize() <= _cov.rows() &&
- _sb_2_idx[sb2] + sb2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
- assert(_sb_2_idx[sb2] + sb2->getSize() <= _cov.rows() &&
- _sb_2_idx[sb1] + sb1->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+ assert(_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.rows() &&
+ _sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+ assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() &&
+ _sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
- _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getSize(), sb2->getSize()) = covariances_[pair_21].transpose();
- _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getSize(), sb1->getSize()) = covariances_[pair_21];
+ _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_[pair_21].transpose();
+ _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_[pair_21];
}
else
return false;
@@ -556,7 +556,7 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
SizeEigen sz = 0;
for (const auto& sb : state_bloc_vec)
if (sb)
- sz += sb->getSize();
+ sz += sb->getLocalSize();
// resizing
_covariance = Eigen::MatrixXs(sz, sz);
@@ -572,10 +572,10 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
if (sb_j)
{
success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
- j += sb_j->getSize();
+ j += sb_j->getLocalSize();
}
}
- i += sb_i->getSize();
+ i += sb_i->getLocalSize();
}
}
return success;
@@ -598,7 +598,7 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
SizeEigen sz = 0;
for (const auto& sb : state_bloc_vec)
if (sb)
- sz += sb->getSize();
+ sz += sb->getLocalSize();
// resizing
_covariance = Eigen::MatrixXs(sz, sz);
@@ -615,10 +615,10 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
if (sb_j)
{
success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
- j += sb_j->getSize();
+ j += sb_j->getLocalSize();
}
}
- i += sb_i->getSize();
+ i += sb_i->getLocalSize();
}
}
return success;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 213e29b9e8acb4c4af36da0c6365d1a538aa2d69..b65bea49219ff2fa5e2c29233d3e4d21a8e35704 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -260,18 +260,16 @@ TEST(Problem, Covariances)
// set covariance (they are not computed without a solver)
P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());
- P->addCovarianceBlock(F->getO(), Eigen::Matrix4s::Identity());
- P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix<Scalar,3,4>::Zero());
+ P->addCovarianceBlock(F->getO(), Eigen::Matrix3s::Identity());
+ P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix3s::Zero());
// get covariance
Eigen::MatrixXs Cov;
ASSERT_TRUE(P->getFrameCovariance(F, Cov));
- // FIXME Frame covariance should be 6x6, but it is actually 7x7 (the state of the state blocks, not of the local parametrizations)
- // JV: The local parameterization projects the covariance to the state size.
- ASSERT_EQ(Cov.cols() , 7);
- ASSERT_EQ(Cov.rows() , 7);
- ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix7s::Identity(), 1e-12);
+ ASSERT_EQ(Cov.cols() , 6);
+ ASSERT_EQ(Cov.rows() , 6);
+ ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix6s::Identity(), 1e-12);
}
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 9de5032b38af32cd564429fae2d0833c60817ef6..1928d5a30fc7cd7c5a63cfcefb684b17fa675709 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -61,7 +61,7 @@ class SolverManagerWrapper : public SolverManager
};
virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
- virtual void computeCovariances(const StateBlockPtrList& st_list){};
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
// The following are dummy implementations
bool hasConverged() { return true; }