Commit 0a35c06c authored by mederic_fourmy's avatar mederic_fourmy
Browse files

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

parent a6e9948f
......@@ -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"
......
......@@ -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)
......
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment