added cov enum for GAUSS Tucan

include/core/solver/solver_manager.h

@@ -57,7 +57,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
-            GAUSS
+            GAUSS,
+            GAUSS_TUCAN
         };
 
         /**

src/ceres_wrapper/solver_ceres.cpp

@@ -264,6 +264,59 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
 
             break;
         }
+        case CovarianceBlocksToBeComputed::GAUSS_TUCAN:
+        {
+            // State blocks:
+            // - Last KF: PV
+
+            // last KF
+            FrameBasePtr last_frame = wolf_problem_->getLastFrame();
+            if (not last_frame)
+                return false;
+
+            // Error
+            if (not last_frame->hasStateBlock('P') or
+                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 last_frame->hasStateBlock('T') or
+                not isStateBlockRegisteredDerived(last_frame->getStateBlock('T')))
+            {
+                WOLF_WARN("SolverCeres::computeCovariances: last KF have null or unregistered state blocks P, O, V 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 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 last_frame->getStateBlock('T'), "SolverCeres::computeCovariances: KF state block 'T' not found");
+                WOLF_WARN_COND(last_frame->getStateBlock('T') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('T')), "SolverCeres::computeCovariances: KF state block 'T' not registered ", last_frame->getStateBlock('T'));
+
+                return false;
+            }
+
+            // only marginals of P, O, V and T
+            state_block_pairs.emplace_back(last_frame->getStateBlock('P'),
+                                           last_frame->getStateBlock('P'));
+            double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('P')),
+                                      getAssociatedMemBlockPtr(last_frame->getStateBlock('P')));
+            state_block_pairs.emplace_back(last_frame->getStateBlock('O'),
+                                           last_frame->getStateBlock('O'));
+            double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('O')),
+                                      getAssociatedMemBlockPtr(last_frame->getStateBlock('O')));
+            state_block_pairs.emplace_back(last_frame->getStateBlock('V'),
+                                           last_frame->getStateBlock('V'));
+            double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('V')),
+                                      getAssociatedMemBlockPtr(last_frame->getStateBlock('V')));
+            state_block_pairs.emplace_back(last_frame->getStateBlock('T'),
+                                           last_frame->getStateBlock('T'));
+            double_pairs.emplace_back(getAssociatedMemBlockPtr(last_frame->getStateBlock('T')),
+                                      getAssociatedMemBlockPtr(last_frame->getStateBlock('T')));
+
+            break;
+        }
         default:
             throw std::runtime_error("SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value");