Emplace factor creates a factor based on the feature type (Apriltag or ApriltagProj)

include/apriltag/processor/processor_tracker_landmark_apriltag.h

@@ -25,6 +25,7 @@
 // Wolf apriltag includes
 #include "apriltag/feature/feature_apriltag.h"
 #include "apriltag/landmark/landmark_apriltag.h"
+#include "apriltag/factor/factor_apriltag_proj.h"
 
 // IPPE (copy from https://github.com/tobycollins/IPPE)
 #include "ippe.h"

src/processor/processor_tracker_landmark_apriltag.cpp

@@ -231,11 +231,27 @@ void ProcessorTrackerLandmarkApriltag::postProcess()
 FactorBasePtr ProcessorTrackerLandmarkApriltag::emplaceFactor(FeatureBasePtr _feature_ptr,
                                                               LandmarkBasePtr _landmark_ptr)
 {
-    return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature_ptr,
-                                               _feature_ptr,
-                                               _landmark_ptr,
-                                               shared_from_this(),
-                                               params_->apply_loss_function);
+    auto feat_pose = std::dynamic_pointer_cast<FeatureApriltag>(_feature_ptr);
+    if (feat_pose)
+    {
+        return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature_ptr,
+                                                _feature_ptr,
+                                                _landmark_ptr,
+                                                shared_from_this(),
+                                                params_->apply_loss_function);
+    }
+    else
+    {
+        auto feat_proj = std::dynamic_pointer_cast<FeatureApriltagProj>(_feature_ptr);
+        return FactorBase::emplace<FactorApriltagProj>(feat_proj,
+                                        feat_proj,
+                                        getSensor(),
+                                        feat_proj->getFrame(),
+                                        _landmark_ptr,
+                                        shared_from_this(),
+                                        params_->apply_loss_function);
+    }
+
 }
 
 LandmarkBasePtr ProcessorTrackerLandmarkApriltag::emplaceLandmark(FeatureBasePtr _feature_ptr)

test/gtest_processor_tracker_landmark_apriltag.cpp

@@ -331,10 +331,10 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceFactor)
     std::cout << prc_apr->getSensor()->getStructure() << std::endl;
     std::cout << st.transpose() << std::endl;
 
-    FactorBasePtr ctr = prc_apr->emplaceFactor(f1, lmk);
+    FactorBasePtr fac = prc_apr->emplaceFactor(f1, lmk);
 
-    ASSERT_TRUE(ctr->getFeature() == f1);
-    ASSERT_TRUE(ctr->getType() == "FactorRelativePose3dWithExtrinsics");
+    ASSERT_TRUE(fac->getFeature() == f1);
+    ASSERT_TRUE(fac->getType() == "FactorRelativePose3dWithExtrinsics");
 }
 
 TEST_F(ProcessorTrackerLandmarkApriltag_class, computeInformation)
@@ -423,6 +423,37 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, computeInformation)
 
 }
 
+
+
+
+
+
+
+
+
+////////////////////////////////
+////////////////////////////////
+// Projection based Factor
+////////////////////////////////
+////////////////////////////////
+
+
+
+TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceFactorProj)
+{
+    auto f1 = FeatureBase::emplace<FeatureApriltagProj>(C1, Vector8d::Zero(), Matrix8d::Identity(), tag_id_, 0.1);
+
+    LandmarkBasePtr lmk = prc_apr->emplaceLandmark(f1);
+    LandmarkApriltagPtr lmk_april = std::static_pointer_cast<LandmarkApriltag>(lmk);
+
+    auto st = prc_apr->getSensor()->getO()->getState();
+
+    FactorBasePtr fac = prc_apr->emplaceFactor(f1, lmk);
+
+    ASSERT_TRUE(fac->getFeature() == f1);
+    ASSERT_TRUE(fac->getType() == "FactorApriltagProj");
+}
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);