gtest constraintBlockAbs -> test p

src/test/gtest_constraint_absolute.cpp

@@ -122,6 +122,37 @@ TEST(ConstraintAbsV, ctr_abs_v_solve)
     ASSERT_MATRIX_APPROX(frm0->getState().tail<3>(), pose10.tail<3>(), 1e-6);
 }
 
+TEST(ConstraintBlockAbs, ctr_bloc_abs_p_check)
+{
+    FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
+    ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
+        fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr(),
+                                                                    fea0->getMeasurement(),
+                                                                    data_cov.block<3,3>(3,3)))
+        );
+    ASSERT_TRUE(problem->check(0));
+}
+
+TEST(ConstraintBlockAbs, ctr_bloc_abs_p_solve)
+{
+    FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
+    ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
+        fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr(),
+                                                                    fea0->getMeasurement(),
+                                                                    data_cov.block<3,3>(3,3)))
+        );
+    
+    // Fix frame 0, perturb frm1
+    frm0->unfix();
+    frm0->setState(x0);
+
+    // solve for frm0
+    std::string brief_report = ceres_mgr.solve(1);
+
+    //only orientation is constrained
+    ASSERT_MATRIX_APPROX(frm0->getState().segment<4>(3), pose10.segment<4>(3), 1e-6);
+}
+
 TEST(ConstraintBlockAbs, ctr_bloc_abs_v_check)
 {
     FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));