added covariance block enum for GAUSS

include/core/solver/solver_manager.h

@@ -56,7 +56,8 @@ class SolverManager
         {
             ALL, ///< All blocks and all cross-covariances
             ALL_MARGINALS, ///< All marginals
-            ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
+            ROBOT_LANDMARKS, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
+            GAUSS
         };
 
         /**

src/ceres_wrapper/solver_ceres.cpp

@@ -205,6 +205,66 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
             }
             break;
         }
+        case CovarianceBlocksToBeComputed::GAUSS:
+        {
+            // State blocks:
+            // - Last KF: PO
+            // - CaptureGnss T
+
+            std::vector<StateBlockPtr> gauss_state_blocks;
+
+            // last KF
+            FrameBasePtr last_frame = wolf_problem_->getLastFrame();
+            if (not last_frame)
+                return false;
+
+            // get capture gnss
+            StateBlockPtr sb_gnss_T;
+            for (CaptureBasePtr cap : last_frame->getCaptureList())
+                if (cap->getType() == "CaptureGnss" and cap->hasStateBlock('T'))
+                {
+                    sb_gnss_T = cap->getStateBlock('T');
+                    break;
+                }
+
+            // Error
+            if (not last_frame->hasStateBlock('P') or
+                not isStateBlockRegistered(last_frame->getStateBlock('P')) or
+                not last_frame->hasStateBlock('O') or
+                not isStateBlockRegistered(last_frame->getStateBlock('O')) or
+                not sb_gnss_T or
+                not isStateBlockRegistered(sb_gnss_T))
+            {
+                WOLF_WARN("SolverCeres::computeCovariances: last KF have null or unregistered state blocks P O or T, returning...");
+                WOLF_WARN_COND(not last_frame->getStateBlock('P'), "SolverCeres::computeCovariances: KF state block 'P' not found");
+                WOLF_WARN_COND(last_frame->getStateBlock('P') and not isStateBlockRegistered(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 isStateBlockRegistered(last_frame->getStateBlock('O')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P'));
+                WOLF_WARN_COND(not sb_gnss_T, "SolverCeres::computeCovariances: KF capture state block 'T' not found");
+                WOLF_WARN_COND(sb_gnss_T and not isStateBlockRegistered(sb_gnss_T), "SolverCeres::computeCovariances: KF capture state block 'T' not registered ", sb_gnss_T);
+
+                return false;
+            }
+
+            // Fill vector of state blocks
+            gauss_state_blocks = std::vector<StateBlockPtr>{last_frame->getStateBlock('P'),
+                                                            last_frame->getStateBlock('O'),
+                                                            sb_gnss_T};
+
+            // double loop all against all (without repetitions)
+            for (unsigned int i = 0; i < gauss_state_blocks.size(); i++)
+                for  (unsigned int j = i; j < gauss_state_blocks.size(); j++)
+                {
+                    state_block_pairs.emplace_back(gauss_state_blocks[i],gauss_state_blocks[j]);
+                    double_pairs.emplace_back(getAssociatedMemBlockPtr(gauss_state_blocks[i]),
+                                              getAssociatedMemBlockPtr(gauss_state_blocks[j]));
+                }
+
+            break;
+        }
+        default:
+            throw std::runtime_error("SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value");
+
     }
     //std::cout << "pairs... " << double_pairs.size() << std::endl;