Switching created cov sizes to tangent space size

src/problem/problem.cpp

@@ -462,16 +462,16 @@ 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");
 
     covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
 }
 
 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");
 
     covariances_[std::make_pair(_state1, _state1)] = _cov;
 }
@@ -482,9 +482,9 @@ 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())
@@ -493,10 +493,10 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
     assert(_row + _state1->getLocalSize() <= _cov.rows() && _col + _state2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
 
     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
     {
@@ -521,23 +521,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;