Skip to content
GitLab
Commit 6fb1121f authored by mederic_fourmy's avatar mederic_fourmy
Browse files

Add a pose attribute to feature proj

parent 4bca1654
Hide whitespace changes
Inline Side-by-side
include/apriltag/feature/feature_apriltag_proj.h
View file @ 6fb1121f
......@@ -46,6 +46,7 @@ class FeatureApriltagProj : public FeatureBase
const Eigen::Matrix8d & _meas_covariance,
const int _tag_id,
const double _tag_width,
const Eigen::Vector7d& _pose_pnp,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
~FeatureApriltagProj() override;
......@@ -56,7 +57,8 @@ class FeatureApriltagProj : public FeatureBase
*
**/
int getTagId() const;
double getTagWidth() const;
double getTagWidth() const;
const Eigen::Vector7d& getPosePnp() const;
const std::vector<cv::Point2d>& getTagCorners() const;
......@@ -64,6 +66,7 @@ class FeatureApriltagProj : public FeatureBase
int tag_id_;
double tag_width_;
std::vector<cv::Point2d> tag_corners_;
Eigen::Vector7d pose_pnp_;
};
......
src/feature/feature_apriltag_proj.cpp
View file @ 6fb1121f
......@@ -27,11 +27,13 @@ FeatureApriltagProj::FeatureApriltagProj(const Eigen::Vector8d & _measurement,
const Eigen::Matrix8d & _meas_uncertainty,
const int _tag_id,
const double _tag_width,
const Eigen::Vector7d& _pose_pnp,
UncertaintyType _uncertainty_type) :
FeatureBase("FeatureApriltagProj", _measurement, _meas_uncertainty, _uncertainty_type),
tag_id_ (_tag_id),
tag_width_ (_tag_width),
tag_corners_(4)
tag_corners_(4),
pose_pnp_(_pose_pnp)
{
setTrackId(_tag_id); // assuming there is a single landmark with this id in the scene
......@@ -62,4 +64,9 @@ const std::vector<cv::Point2d>& FeatureApriltagProj::getTagCorners() const
return tag_corners_;
}
const Eigen::Vector7d& FeatureApriltagProj::getPosePnp() const
{
return pose_pnp_;
}
} // namespace wolf
src/processor/processor_tracker_landmark_apriltag.cpp
View file @ 6fb1121f
......@@ -132,6 +132,9 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
//////////////////
// IPPE (Infinitesimal Plane-based Pose Estimation)
// TODO: Quite bad for reprojection factor: we only need to do this question once
// when the landmark is newly discovered. Would require to change the whole logic
// or to check if this landmark is already in the map HERE (which is usually not the logic of preProcess)
//////////////////
Eigen::Isometry3d M_ippe1, M_ippe2;
double rep_error1, rep_error2;
......@@ -160,7 +163,8 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
detections_incoming_.push_back(std::make_shared<FeatureApriltagProj>(corners_vec,
std_pix_*std_pix_*Eigen::Matrix8d::Identity(),
tag_id,
tag_width));
tag_width,
pose));
}
else
{
......@@ -287,8 +291,17 @@ LandmarkBasePtr ProcessorTrackerLandmarkApriltag::emplaceLandmark(FeatureBasePtr
Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos.head(3)) * quat_tmp;
// camera to lmk (tag)
pos = _feature_ptr->getMeasurement().head(3);
quat_tmp.coeffs() = _feature_ptr->getMeasurement().tail(4);
if (use_proj_factor_)
{
auto feat_proj = std::dynamic_pointer_cast<FeatureApriltagProj>(_feature_ptr);
pos = feat_proj->getPosePnp().head(3);
quat_tmp.coeffs() = feat_proj->getPosePnp().tail(4);
}
else
{
pos = _feature_ptr->getMeasurement().head(3);
quat_tmp.coeffs() = _feature_ptr->getMeasurement().tail(4);
}
Eigen::Isometry3d c_M_t = Eigen::Translation<double,3>(pos) * quat_tmp;
// world to lmk (tag)
......
test/gtest_factor_apriltag_proj.cpp
View file @ 6fb1121f
......@@ -114,6 +114,7 @@ class FactorApriltagProj_class : public testing::Test{
Vector8d meas1;
Vector8d meas2;
Eigen::Matrix8d meas_cov;
Eigen::Vector7d pose_dummy;
void SetUp() override
{
......@@ -228,6 +229,9 @@ class FactorApriltagProj_class : public testing::Test{
FactorApriltagProj::pinholeProj(K, p_c2_l, q_c2_l, l_corn2),
FactorApriltagProj::pinholeProj(K, p_c2_l, q_c2_l, l_corn3),
FactorApriltagProj::pinholeProj(K, p_c2_l, q_c2_l, l_corn4);
// dummy pose, not used in the factor, only for some part of the processor
pose_dummy.setZero();
}
};
......@@ -235,7 +239,7 @@ class FactorApriltagProj_class : public testing::Test{
TEST_F(FactorApriltagProj_class, Constructor)
{
f1 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C1, meas1, meas_cov, 1, tag_width));
f1 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C1, meas1, meas_cov, 1, tag_width, pose_dummy));
FactorApriltagProjPtr factor = std::make_shared<FactorApriltagProj>(
f1,
camera,
......@@ -252,7 +256,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, 1, tag_width));
f1 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C1, meas1, meas_cov, 1, tag_width, pose_dummy));
//emplace feature and landmark
auto factor = FactorBase::emplace<FactorApriltagProj>(f1, f1, camera, F1, lmk1, nullptr, false);
......@@ -289,8 +293,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, 1, tag_width));
auto f2 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C2, meas2, meas_cov, 2, tag_width));
f1 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C1, meas1, meas_cov, 1, tag_width, pose_dummy));
auto f2 = std::static_pointer_cast<FeatureApriltagProj>(FeatureBase::emplace<FeatureApriltagProj>(C2, meas2, meas_cov, 2, tag_width, pose_dummy));
//emplace feature and landmark
auto factor1 = FactorBase::emplace<FactorApriltagProj>(f1, f1, camera, F1, lmk1, nullptr, false);
......
test/gtest_feature_apriltag_proj.cpp
View file @ 6fb1121f
......@@ -41,6 +41,7 @@ class FeatureApriltagProj_test : public testing::Test
int tag_id_;
double tag_width_;
apriltag_detection_t det_;
Eigen::Vector7d pose_pnp_;
void SetUp() override
{
......@@ -53,28 +54,30 @@ class FeatureApriltagProj_test : public testing::Test
-1.0, 1.0,
-1.0, -1.0;
cov_.setIdentity();
pose_pnp_ << 0,0,0, 0,0,0,1;
}
};
TEST_F(FeatureApriltagProj_test, type)
{
auto f = std::make_shared<FeatureApriltagProj>(meas_, cov_, tag_id_, tag_width_);
auto f = std::make_shared<FeatureApriltagProj>(meas_, cov_, tag_id_, tag_width_, pose_pnp_);
ASSERT_EQ(f->getType(), "FeatureApriltagProj");
}
TEST_F(FeatureApriltagProj_test, getters)
{
auto f = std::make_shared<FeatureApriltagProj>(meas_, cov_, tag_id_, tag_width_);
auto f = std::make_shared<FeatureApriltagProj>(meas_, cov_, tag_id_, tag_width_, pose_pnp_);
ASSERT_EQ(f->getTagId(), tag_id_);
ASSERT_EQ(f->getTagWidth(), tag_width_);
ASSERT_MATRIX_APPROX(f->getPosePnp(), pose_pnp_, 1e-6);
}
TEST_F(FeatureApriltagProj_test, getCorners)
{
auto f = std::make_shared<FeatureApriltagProj>(meas_, cov_, tag_id_, tag_width_);
auto f = std::make_shared<FeatureApriltagProj>(meas_, cov_, tag_id_, tag_width_, pose_pnp_);
ASSERT_EQ(f->getTagCorners().size(), 4);
......
test/gtest_processor_tracker_landmark_apriltag.cpp
View file @ 6fb1121f
......@@ -441,7 +441,9 @@ TEST_F(ProcessorTrackerLandmarkApriltag_class, computeInformation)
TEST_F(ProcessorTrackerLandmarkApriltag_class, emplaceFactorProj)
{
auto f1 = FeatureBase::emplace<FeatureApriltagProj>(C1, Vector8d::Zero(), Matrix8d::Identity(), tag_id_, 0.1);
// dummy pose, not used in the factor, only for some part of the processor
Vector7d pose_dummy = Vector7d::Zero();
auto f1 = FeatureBase::emplace<FeatureApriltagProj>(C1, Vector8d::Zero(), Matrix8d::Identity(), tag_id_, 0.1, pose_dummy);
LandmarkBasePtr lmk = prc_apr->emplaceLandmark(f1);
LandmarkApriltagPtr lmk_april = std::static_pointer_cast<LandmarkApriltag>(lmk);
......
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