Helper perturbation function

test/gtest_factor_force_torque.cpp

@@ -106,6 +106,21 @@ Matrix9d computeKinCov(const Matrix3d& Qp,
 }
 
 
+void perturbateIfUnFixed(const FrameBasePtr& KF, std::string sb_name){
+    if(!KF->getStateBlock(sb_name)->isFixed()) 
+    {
+        if (sb_name == "O")
+        {
+            KF->getStateBlock("O")->setState(KF->getStateBlock("O")->getState().reverse());
+        }
+        else 
+        {   
+            Eigen::Vector3d pert = Eigen::Vector3d::Random();
+            KF->getStateBlock(sb_name)->setState(KF->getStateBlock(sb_name)->getState() + pert);
+        }
+    } 
+}
+
 
 class FactorInertialKinematics_1KF : public testing::Test
 {
@@ -316,27 +331,24 @@ TEST_F(FactorInertialKinematics_1KF,ZeroMvt)
     Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero(); 
     Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero(); 
 
-    Vector3d errP = Vector3d::Random();
-    Vector3d errV = Vector3d::Random();
-    Vector3d errC = Vector3d::Random();
-    Vector3d errD = Vector3d::Random();
-    Vector3d errL = Vector3d::Random();
-    Vector3d errBp = Vector3d::Random();
-
     string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
-    // throw std::invalid_argument( "received negative value" );
-    if(!KFA_->getP()->isFixed()) {KFA_->getP()->setState(KFA_->getP()->getState() + errP);}
-    if(!KFA_->getO()->isFixed()) {KFA_->getO()->setState(KFA_->getO()->getState().reverse());} 
-    if(!KFA_->getV()->isFixed()) {KFA_->getV()->setState(KFA_->getV()->getState() + errV);} 
-    if(!KFA_->getStateBlock("C")->isFixed()) {KFA_->getStateBlock("C")->setState(KFA_->getStateBlock("C")->getState() + errC);} 
-    if(!KFA_->getStateBlock("D")->isFixed()) {KFA_->getStateBlock("D")->setState(KFA_->getStateBlock("D")->getState() + errD);} 
-    if(!KFA_->getStateBlock("L")->isFixed()) {KFA_->getStateBlock("L")->setState(KFA_->getStateBlock("L")->getState() + errL);} 
-    if(!KFB_->getStateBlock("C")->isFixed()) {KFB_->getStateBlock("C")->setState(KFB_->getStateBlock("C")->getState() + errC);} 
-    if(!KFB_->getStateBlock("D")->isFixed()) {KFB_->getStateBlock("D")->setState(KFB_->getStateBlock("D")->getState() + errD);} 
-    if(!KFB_->getStateBlock("L")->isFixed()) {KFB_->getStateBlock("L")->setState(KFB_->getStateBlock("L")->getState() + errL);} 
-    if(!KFB_->getStateBlock("B")->isFixed()) {KFA_->getStateBlock("B")->setState(KFA_->getStateBlock("B")->getState() + errBp);} 
-    
+    perturbateIfUnFixed(KFA_, "P");
+    perturbateIfUnFixed(KFA_, "O");
+    perturbateIfUnFixed(KFA_, "V");
+    perturbateIfUnFixed(KFA_, "C");
+    perturbateIfUnFixed(KFA_, "D");
+    perturbateIfUnFixed(KFA_, "L");
+    perturbateIfUnFixed(KFA_, "B");
+
+    perturbateIfUnFixed(KFB_, "P");
+    perturbateIfUnFixed(KFB_, "O");
+    perturbateIfUnFixed(KFB_, "V");
+    perturbateIfUnFixed(KFB_, "C");
+    perturbateIfUnFixed(KFB_, "D");
+    perturbateIfUnFixed(KFB_, "L");
+    perturbateIfUnFixed(KFB_, "B");
+
     problem_->print(4,true,true,true);
     report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
     std::cout << report << std::endl;