GAUSS covariance ENUM modification

src/ceres_wrapper/solver_ceres.cpp

@@ -230,7 +230,7 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
         case CovarianceBlocksToBeComputed::GAUSS:
         {
             // State blocks:
-            // - Last KF: PO
+            // - Last KF: POV
             // - CaptureGnss T
 
             std::vector<StateBlockPtr> gauss_state_blocks;
@@ -254,6 +254,8 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
                 not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')) or
                 not last_frame->hasStateBlock('O') or
                 not isStateBlockRegisteredDerived(last_frame->getStateBlock('O')) or
+                not last_frame->hasStateBlock('V') or
+                not isStateBlockRegisteredDerived(last_frame->getStateBlock('V')) or
                 not sb_gnss_T or
                 not isStateBlockRegisteredDerived(sb_gnss_T))
             {
@@ -262,6 +264,8 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
                 WOLF_WARN_COND(last_frame->getStateBlock('P') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P'));
                 WOLF_WARN_COND(not last_frame->getStateBlock('O'), "SolverCeres::computeCovariances: KF state block 'O' not found");
                 WOLF_WARN_COND(last_frame->getStateBlock('O') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('O')), "SolverCeres::computeCovariances: KF state block 'O' not registered ", last_frame->getStateBlock('O'));
+                WOLF_WARN_COND(not last_frame->getStateBlock('V'), "SolverCeres::computeCovariances: KF state block 'V' not found");
+                WOLF_WARN_COND(last_frame->getStateBlock('V') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('V')), "SolverCeres::computeCovariances: KF state block 'V' not registered ", last_frame->getStateBlock('V'));
                 WOLF_WARN_COND(not sb_gnss_T, "SolverCeres::computeCovariances: KF capture state block 'T' not found");
                 WOLF_WARN_COND(sb_gnss_T and not isStateBlockRegisteredDerived(sb_gnss_T), "SolverCeres::computeCovariances: KF capture state block 'T' not registered ", sb_gnss_T);
 
@@ -271,9 +275,19 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
             // Fill vector of state blocks
             gauss_state_blocks = std::vector<StateBlockPtr>{last_frame->getStateBlock('P'),
                                                             last_frame->getStateBlock('O'),
+                                                            last_frame->getStateBlock('V'),
                                                             sb_gnss_T};
 
-            // double loop all against all (without repetitions)
+            // only marginals
+            for (unsigned int i = 0; i < gauss_state_blocks.size(); i++)
+            {
+                assert(isStateBlockRegisteredDerived(gauss_state_blocks[i]));
+
+                state_block_pairs.emplace_back(gauss_state_blocks[i],gauss_state_blocks[i]);
+                double_pairs.emplace_back(getAssociatedMemBlockPtr(gauss_state_blocks[i]),
+                                          getAssociatedMemBlockPtr(gauss_state_blocks[i]));
+            }
+            /*// double loop all against all (without repetitions)
             for (unsigned int i = 0; i < gauss_state_blocks.size(); i++)
             {
                 assert(isStateBlockRegisteredDerived(gauss_state_blocks[i]));
@@ -286,7 +300,7 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
                     double_pairs.emplace_back(getAssociatedMemBlockPtr(gauss_state_blocks[i]),
                                               getAssociatedMemBlockPtr(gauss_state_blocks[j]));
                 }
-            }
+            }*/
 
             break;
         }