Commit 291abed2 authored by mederic_fourmy's avatar mederic_fourmy
Browse files

Fix almost all gtests -> processor fails because of getSensorO

parent 383956b4
......@@ -38,6 +38,7 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureApriltag);
//class FeatureApriltag
class FeatureApriltag : public FeatureBase
{
......@@ -46,7 +47,7 @@ class FeatureApriltag : public FeatureBase
FeatureApriltag(const Eigen::Vector7d & _measurement,
const Eigen::Matrix6d & _meas_covariance,
const int _tag_id,
const apriltag_detection_t & _det,
const Vector8d & _corners_vec,
double _rep_error1,
double _rep_error2,
bool _use_rotation,
......@@ -60,8 +61,6 @@ class FeatureApriltag : public FeatureBase
**/
double getTagId() const;
const apriltag_detection_t& getDetection() const;
const std::vector<cv::Point2d>& getTagCorners() const;
double getRepError1() const;
......@@ -72,7 +71,6 @@ class FeatureApriltag : public FeatureBase
private:
int tag_id_;
std::vector<cv::Point2d> tag_corners_;
apriltag_detection_t detection_;
double rep_error1_;
double rep_error2_;
bool use_rotation_;
......
......@@ -46,7 +46,6 @@ class FeatureApriltagProj : public FeatureBase
const Eigen::Matrix8d & _meas_covariance,
const int _tag_id,
const double _tag_width,
const apriltag_detection_t & _det,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
~FeatureApriltagProj() override;
......@@ -59,15 +58,12 @@ class FeatureApriltagProj : public FeatureBase
int getTagId() const;
double getTagWidth() const;
const apriltag_detection_t& getDetection() const;
const std::vector<cv::Point2d>& getTagCorners() const;
private:
int tag_id_;
double tag_width_;
std::vector<cv::Point2d> tag_corners_;
apriltag_detection_t detection_;
};
......
......@@ -26,7 +26,7 @@ namespace wolf {
FeatureApriltag::FeatureApriltag(const Eigen::Vector7d & _measurement,
const Eigen::Matrix6d & _meas_uncertainty,
const int _tag_id,
const apriltag_detection_t & _det,
const Vector8d & _corners_vec,
double _rep_error1,
double _rep_error2,
bool _use_rotation,
......@@ -34,17 +34,16 @@ FeatureApriltag::FeatureApriltag(const Eigen::Vector7d & _measurement,
FeatureBase("FeatureApriltag", _measurement, _meas_uncertainty, _uncertainty_type),
tag_id_ (_tag_id),
tag_corners_(4),
detection_ (_det),
rep_error1_(_rep_error1),
rep_error2_(_rep_error2),
use_rotation_(_use_rotation)
{
assert(_det.id == _tag_id && "Tag ID and detection ID do not match!");
setTrackId(_tag_id);
setTrackId(_tag_id); // assuming there is a single landmark with this id in the scene
for (int i = 0; i < 4; i++)
{
tag_corners_[i].x = detection_.p[i][0];
tag_corners_[i].y = detection_.p[i][1];
tag_corners_[i].x = _corners_vec[2*i];
tag_corners_[i].y = _corners_vec[2*i+1];
}
}
......@@ -58,11 +57,6 @@ double FeatureApriltag::getTagId() const
return tag_id_;
}
const apriltag_detection_t& FeatureApriltag::getDetection() const
{
return detection_;
}
const std::vector<cv::Point2d>& FeatureApriltag::getTagCorners() const
{
return tag_corners_;
......
......@@ -27,20 +27,18 @@ FeatureApriltagProj::FeatureApriltagProj(const Eigen::Vector8d & _measurement,
const Eigen::Matrix8d & _meas_uncertainty,
const int _tag_id,
const double _tag_width,
const apriltag_detection_t & _det,
UncertaintyType _uncertainty_type) :
FeatureBase("FeatureApriltagProj", _measurement, _meas_uncertainty, _uncertainty_type),
tag_id_ (_tag_id),
tag_width_ (_tag_width),
tag_corners_(4),
detection_ (_det)
tag_corners_(4)
{
assert(_det.id == _tag_id && "Tag ID and detection ID do not match!");
setTrackId(_tag_id);
setTrackId(_tag_id); // assuming there is a single landmark with this id in the scene
for (int i = 0; i < 4; i++)
{
tag_corners_[i].x = detection_.p[i][0];
tag_corners_[i].y = detection_.p[i][1];
tag_corners_[i].x = _measurement[2*i];
tag_corners_[i].y = _measurement[2*i+1];
}
}
......@@ -59,11 +57,6 @@ double FeatureApriltagProj::getTagWidth() const
return tag_width_;
}
const apriltag_detection_t& FeatureApriltagProj::getDetection() const
{
return detection_;
}
const std::vector<cv::Point2d>& FeatureApriltagProj::getTagCorners() const
{
return tag_corners_;
......
......@@ -135,8 +135,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
// IPPE (Infinitesimal Plane-based Pose Estimation)
//////////////////
Eigen::Isometry3d M_ippe1, M_ippe2;
double rep_error1;
double rep_error2;
double rep_error1, rep_error2;
ippePoseEstimation(det, cv_K_, tag_width, M_ippe1, rep_error1, M_ippe2, rep_error2);
// If not so sure about whether we have the right solution or not, do not create a feature
bool use_rotation = ((rep_error2 / rep_error1 > ippe_min_ratio_) && rep_error1 < ippe_max_rep_error_);
......@@ -159,11 +158,16 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
info.bottomRightCorner(3,3) = M_1_PI*M_1_PI * Eigen::Matrix3d::Identity(); // 180 degrees standar deviation
}
Vector8d corners_vec; corners_vec << det->p[0][0], det->p[0][1],
det->p[1][0], det->p[1][1],
det->p[2][0], det->p[2][1],
det->p[3][0], det->p[3][1];
// add to detected features list
detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose,
info,
tag_id,
*det,
corners_vec,
rep_error1,
rep_error2,
use_rotation,
......@@ -192,6 +196,7 @@ void ProcessorTrackerLandmarkApriltag::ippePoseEstimation(apriltag_detection_t *
corners_pix[i].y = _det->p[i][1];
}
cv::Mat rvec1, tvec1, rvec2, tvec2;
float err1, err2;
IPPE::PoseSolver pose_solver;
......
......@@ -41,6 +41,7 @@ using namespace Eigen;
using namespace wolf;
using std::static_pointer_cast;
Isometry3d pose2iso(const Vector3d& posi, const Quaterniond& quat)
{
return Translation<double,3>(posi) * quat;
......@@ -251,18 +252,6 @@ class FactorApriltagProj_class : public testing::Test{
proc_apriltag->setLastPtr(C1);
meas_cov = Eigen::Matrix8d::Identity(); // pixel noise
int tag_id = 1;
// unused
det.id = tag_id;
det.p[0][0] = 1.0;
det.p[0][1] = -1.0;
det.p[1][0] = 1.0;
det.p[1][1] = 1.0;
det.p[2][0] = -1.0;
det.p[2][1] = 1.0;
det.p[3][0] = -1.0;
det.p[3][1] = -1.0;
tag_width = 0.2;
lmk1 = LandmarkBase::emplace<LandmarkApriltag>(problem->getMap(), pose_w_l, 42, tag_width);
......@@ -291,8 +280,7 @@ class FactorApriltagProj_class : public testing::Test{
TEST_F(FactorApriltagProj_class, Constructor)
{
Vector8d meas = Vector8d::Zero();
f1 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C1, meas, meas_cov, det.id, tag_width, det));
f1 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C1, meas1, meas_cov, 1, tag_width));
FactorApriltagProjPtr factor = std::make_shared<FactorApriltagProj>(
camera,
F1,
......@@ -310,7 +298,7 @@ TEST_F(FactorApriltagProj_class, Constructor)
TEST_F(FactorApriltagProj_class, problem_1KF)
{
f1 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C1, meas1, meas_cov, det.id, tag_width, det));
f1 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C1, meas1, meas_cov, 1, tag_width));
//emplace feature and landmark
auto factor = FactorBase::emplace<FactorApriltagProj>(f1, camera, F1, lmk1, f1, nullptr, false, FAC_ACTIVE);
......@@ -351,8 +339,8 @@ TEST_F(FactorApriltagProj_class, problem_2KF)
FrameBasePtr F2 = problem->emplaceFrame(2, posiquat2pose(p_w_r2, q_w_r2));
CaptureImagePtr C2 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F2, 1, camera, cv::Mat(2,2,CV_8UC1)));
f1 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C1, meas1, meas_cov, det.id, tag_width, det));
auto f2 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C2, meas2, meas_cov, det.id, tag_width, det));
f1 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C1, meas1, meas_cov, 1, tag_width));
auto f2 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C2, meas2, meas_cov, 2, tag_width));
//emplace feature and landmark
auto factor1 = FactorBase::emplace<FactorApriltagProj>(f1, camera, F1, lmk1, f1, nullptr, false, FAC_ACTIVE);
......
......@@ -38,8 +38,8 @@ class FeatureApriltag_test : public testing::Test
public:
Eigen::Vector7d pose;
Eigen::Matrix6d cov;
Eigen::Vector8d corners_vec;
int tag_id;
apriltag_detection_t det;
double rep_error1;
double rep_error2;
bool use_rotation;
......@@ -49,16 +49,11 @@ class FeatureApriltag_test : public testing::Test
pose << 1,2,3,4,5,6,7;
cov.setIdentity() * 2.0;
det.id = 1;
tag_id = det.id;
det.p[0][0] = 1.0;
det.p[0][1] = -1.0;
det.p[1][0] = 1.0;
det.p[1][1] = 1.0;
det.p[2][0] = -1.0;
det.p[2][1] = 1.0;
det.p[3][0] = -1.0;
det.p[3][1] = -1.0;
tag_id = 1;
corners_vec << 1.0, -1.0,
1.0, 1.0,
-1.0, 1.0,
-1.0, -1.0;
rep_error1 = 0.01;
rep_error2 = 0.1;
......@@ -68,21 +63,21 @@ class FeatureApriltag_test : public testing::Test
TEST_F(FeatureApriltag_test, type)
{
FeatureApriltagPtr f = std::make_shared<FeatureApriltag>(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
FeatureApriltagPtr f = std::make_shared<FeatureApriltag>(pose, cov, tag_id, corners_vec, rep_error1, rep_error2, use_rotation);
ASSERT_EQ(f->getType(), "FeatureApriltag");
}
TEST_F(FeatureApriltag_test, getTagId)
{
FeatureApriltagPtr f = std::make_shared<FeatureApriltag>(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
FeatureApriltagPtr f = std::make_shared<FeatureApriltag>(pose, cov, tag_id, corners_vec, rep_error1, rep_error2, use_rotation);
ASSERT_EQ(f->getTagId(), 1);
}
TEST_F(FeatureApriltag_test, getCorners)
{
FeatureApriltagPtr f = std::make_shared<FeatureApriltag>(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
FeatureApriltagPtr f = std::make_shared<FeatureApriltag>(pose, cov, tag_id, corners_vec, rep_error1, rep_error2, use_rotation);
ASSERT_EQ(f->getTagCorners().size(), 4);
......@@ -96,34 +91,9 @@ TEST_F(FeatureApriltag_test, getCorners)
ASSERT_EQ(f->getTagCorners()[3].y, -1.0);
}
TEST_F(FeatureApriltag_test, getDetection)
{
FeatureApriltagPtr f = std::make_shared<FeatureApriltag>(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
ASSERT_EQ(f->getDetection().id, 1);
}
TEST_F(FeatureApriltag_test, tagid_detid_equality)
{
FeatureApriltagPtr f = std::make_shared<FeatureApriltag>(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
ASSERT_EQ(f->getDetection().id, f->getTagId());
}
TEST_F(FeatureApriltag_test, tagCorners_detection_equality)
{
FeatureApriltagPtr f = std::make_shared<FeatureApriltag>(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
for (int i = 0; i<f->getTagCorners().size(); i++)
{
ASSERT_EQ(f->getTagCorners()[i].x, f->getDetection().p[i][0]);
ASSERT_EQ(f->getTagCorners()[i].y, f->getDetection().p[i][1]);
}
}
TEST_F(FeatureApriltag_test, getRepErrors)
{
FeatureApriltagPtr f = std::make_shared<FeatureApriltag>(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
FeatureApriltagPtr f = std::make_shared<FeatureApriltag>(pose, cov, tag_id, corners_vec, rep_error1, rep_error2, use_rotation);
double err1 = f->getRepError1();
double err2 = f->getRepError2();
......
......@@ -33,50 +33,61 @@
using namespace wolf;
class FeatureApriltag_test : public testing::Test
class FeatureApriltagProj_test : public testing::Test
{
public:
Eigen::Vector8d meas;
Eigen::Matrix8d cov;
Eigen::Vector8d meas_;
Eigen::Matrix8d cov_;
int tag_id_;
double tag_width_;
apriltag_detection_t det_;
void SetUp() override
{
meas << 1,2,3,4,5,6,7,8;
cov.setIdentity() * 2.0;
tag_id_ = 1;
tag_width_ = 0.2;
det_.id = 1;
det_.p[0][0] = 1.0;
det_.p[0][1] = -1.0;
det_.p[1][0] = 1.0;
det_.p[1][1] = 1.0;
det_.p[2][0] = -1.0;
det_.p[2][1] = 1.0;
det_.p[3][0] = -1.0;
det_.p[3][1] = -1.0;
meas_ << 1.0, -1.0,
1.0, 1.0,
-1.0, 1.0,
-1.0, -1.0;
cov_.setIdentity();
}
};
TEST_F(FeatureApriltag_test, type)
TEST_F(FeatureApriltagProj_test, type)
{
auto f = std::make_shared<FeatureApriltagProj>(meas, cov, tag_id_, tag_width_, det_);
auto f = std::make_shared<FeatureApriltagProj>(meas_, cov_, tag_id_, tag_width_);
ASSERT_EQ(f->getType(), "FeatureApriltagProj");
}
TEST_F(FeatureApriltag_test, getters)
TEST_F(FeatureApriltagProj_test, getters)
{
auto f = std::make_shared<FeatureApriltagProj>(meas, cov, tag_id_, tag_width_, det_);
auto f = std::make_shared<FeatureApriltagProj>(meas_, cov_, tag_id_, tag_width_);
ASSERT_EQ(f->getTagId(), tag_id_);
ASSERT_EQ(f->getTagWidth(), tag_width_);
}
TEST_F(FeatureApriltagProj_test, getCorners)
{
auto f = std::make_shared<FeatureApriltagProj>(meas_, cov_, tag_id_, tag_width_);
ASSERT_EQ(f->getTagCorners().size(), 4);
ASSERT_EQ(f->getTagCorners()[0].x, 1.0);
ASSERT_EQ(f->getTagCorners()[0].y, -1.0);
ASSERT_EQ(f->getTagCorners()[1].x, 1.0);
ASSERT_EQ(f->getTagCorners()[1].y, 1.0);
ASSERT_EQ(f->getTagCorners()[2].x, -1.0);
ASSERT_EQ(f->getTagCorners()[2].y, 1.0);
ASSERT_EQ(f->getTagCorners()[3].x, -1.0);
ASSERT_EQ(f->getTagCorners()[3].y, -1.0);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
......
......@@ -86,7 +86,23 @@ WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkApriltag_Wrapper);
*/
// Use the following in case you want to initialize tests with predefined variables or methods.
class ProcessorTrackerLandmarkApriltag_class : public testing::Test{
public:
ProcessorTrackerLandmarkApriltag_WrapperPtr prc_apr;
std::string wolf_root;
ProblemPtr problem;
SensorBasePtr sen;
ProcessorBasePtr prc;
FrameBasePtr F1;
CaptureBasePtr C1;
Vector8d corners_vec_;
int tag_id_;
double rep_error1;
double rep_error2;
bool use_rotation;
Vector7d pose_default_;
Matrix6d cov_pose_;
void SetUp() override
{
wolf_root = _WOLF_APRILTAG_ROOT_DIR;
......@@ -107,32 +123,21 @@ class ProcessorTrackerLandmarkApriltag_class : public testing::Test{
prc_apr->setOriginPtr(C1);
prc_apr->setLastPtr(C1);
det.p[0][0] = 1.0;
det.p[0][1] = -1.0;
det.p[1][0] = 1.0;
det.p[1][1] = 1.0;
det.p[2][0] = -1.0;
det.p[2][1] = 1.0;
det.p[3][0] = -1.0;
det.p[3][1] = -1.0;
tag_id_ = 1;
corners_vec_ << 1.0, -1.0,
1.0, 1.0,
-1.0, 1.0,
-1.0, -1.0;
rep_error1 = 0.01;
rep_error2 = 0.1;
use_rotation = true;
// use for tests in which pose value irrelevant
pose_default_ << 0,0,0, 0,0,0,1;
cov_pose_.setIdentity();
}
public:
ProcessorTrackerLandmarkApriltag_WrapperPtr prc_apr;
std::string wolf_root;
ProblemPtr problem;
SensorBasePtr sen;
ProcessorBasePtr prc;
FrameBasePtr F1;
CaptureBasePtr C1;
apriltag_detection_t det;
double rep_error1;
double rep_error2;
bool use_rotation;
};
////////////////////////////////////////////////////////////////
......@@ -239,32 +244,14 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeaturesDuplicated)
prc_apr->detectNewFeatures(1, C1, features_out);
ASSERT_EQ(features_out.size(), 0);
// Some detected features TODO
// Some detected features, pose does not matter for this test
FeatureBasePtrList features_in;
Eigen::Vector3d pos;
Eigen::Vector3d ori; //Euler angles in rad
Eigen::Quaterniond quat;
Eigen::Vector7d pose;
Eigen::Matrix6d meas_cov( Matrix6d::Identity() );
int tag_id;
// feature 0
pos << 0,2,0;
ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
quat = e2q(ori);
pose << pos, quat.coeffs();
tag_id = 0;
det.id = tag_id;
FeatureBasePtr f0 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
tag_id_ = 0;
auto f0 = std::make_shared<FeatureApriltag>(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation);
// feature 1 (with same id of feature 0)
pos << 1,2,0;
ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
quat = e2q(ori);
pose << pos, quat.coeffs();
tag_id = 0;
det.id = tag_id;
FeatureBasePtr f1 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
auto f1 = std::make_shared<FeatureApriltag>(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation);
features_in.push_back(f0);
features_in.push_back(f1);
......@@ -278,46 +265,19 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeaturesDuplicated)
TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeatures)
{
// No detected features
FeatureBasePtrList features_out;
prc_apr->detectNewFeatures(1, C1, features_out);
ASSERT_EQ(features_out.size(), 0);
// Some detected features TODO
// Some detected features, pose does not matter for this test
FeatureBasePtrList features_in;
Eigen::Vector3d pos;
Eigen::Vector3d ori; //Euler angles in rad
Eigen::Quaterniond quat;
Eigen::Vector7d pose;
Eigen::Matrix6d meas_cov( Matrix6d::Identity() );
int tag_id;
FeatureBasePtrList features_out;
// feature 0
pos << 0,2,0;
ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
quat = e2q(ori);
pose << pos, quat.coeffs();
tag_id = 0;
det.id = tag_id;
FeatureBasePtr f0 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
tag_id_ = 0;
auto f0 = std::make_shared<FeatureApriltag>(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation);
// feature 1
pos << 1,2,0;
ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
quat = e2q(ori);
pose << pos, quat.coeffs();
tag_id = 1;
det.id = tag_id;
FeatureBasePtr f1 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
tag_id_ = 1;
auto f1 = std::make_shared<FeatureApriltag>(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation);
// feature 2
pos << 0,2,1;
ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
quat = e2q(ori);
pose << pos, quat.coeffs();
tag_id = 2;
det.id = tag_id;
FeatureBasePtr f2 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
tag_id_ = 2;
auto f2 = std::make_shared<FeatureApriltag>(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation);
//we add different features in the list
features_in.push_back(f0);
......@@ -349,12 +309,12 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeatures)
TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceLandmark)
{
Vector7d pose_landmark((Vector7d()<<0,0,0,0,0,0,1).finished());
det.id = 1;
FeatureBasePtr f1 = FeatureBase::emplace<FeatureApriltag>(C1,pose_landmark, Matrix6d::Identity(), 1, det, rep_error1, rep_error2, use_rotation);
Vector7d pose_landmark((Vector7d()<<1,2,3,1,0,0,0).finished());
auto f1 = std::make_shared<FeatureApriltag>(pose_landmark, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation);
LandmarkBasePtr lmk = prc_apr->emplaceLandmark(f1);
LandmarkApriltagPtr lmk_april = std::static_pointer_cast<LandmarkApriltag>(lmk);
ASSERT_TRUE(lmk_april->getMap() != nullptr);
ASSERT_TRUE(lmk_april->getType() == "LandmarkApriltag");
ASSERT_MATRIX_APPROX(lmk_april->getState().vector("PO"), pose_landmark, 1e-6);
......@@ -362,8 +322,7 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceLandmark)
TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceFactor)
{
det.id = 1;
FeatureBasePtr f1 = FeatureBase::emplace<FeatureApriltag>(C1,(Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), 1, det, rep_error1, rep_error2, use_rotation);
auto f1 = std::make_shared<FeatureApriltag>(pose_default_, cov_pose_, tag_id_, corners_vec_, rep_error1, rep_error2, use_rotation);
LandmarkBasePtr lmk = prc_apr->emplaceLandmark(f1);
LandmarkApriltagPtr lmk_april = std::static_pointer_cast<LandmarkApriltag>(lmk);
......