[WIP] Init frame covariance

Depends on !135 (merged) .

Aims at Resolves #137 (closed).

@joanvallve I tried to somewhat follow your suggestion here regarding #137 (closed). I'd like your opinion as I'm not quite sure of what I'm doing xD.

src/frame_base.cpp

@@ -89,11 +89,18 @@ void FrameBase::remove()
 
 void FrameBase::registerNewStateBlocks()
 {
-    if (getProblem() != nullptr)
+  const auto& prob_ptr = getProblem();
+
+    if (prob_ptr != nullptr)
     {
         for (StateBlockPtr sbp : getStateBlockVec())
             if (sbp != nullptr)
-                getProblem()->addStateBlock(sbp);
+            {
+                prob_ptr->addStateBlock(sbp);
+
+                const Eigen::MatrixXs& cov = getConstraintOf(prob_ptr->getProcessorMotionPtr())->getFeaturePtr()->getMeasurementCovariance();
+                prob_ptr->addCovarianceBlock(sbp, sbp, cov);
+            }
     }
 }