Commit 582cc2c4 authored by mederic_fourmy's avatar mederic_fourmy
Browse files

Refactored feature classes with an intermediate FeatureApriltag class

parent aac67f15
......@@ -104,6 +104,7 @@ SET(HDRS_FACTOR
include/${PROJECT_NAME}/factor/factor_apriltag_proj.h
)
SET(HDRS_FEATURE
include/${PROJECT_NAME}/feature/feature_apriltag.h
include/${PROJECT_NAME}/feature/feature_apriltag_pose.h
include/${PROJECT_NAME}/feature/feature_apriltag_proj.h
)
......@@ -117,8 +118,6 @@ include/${PROJECT_NAME}/processor/ippe.h
# ============ SOURCES ============
SET(SRCS_FEATURE
src/feature/feature_apriltag_pose.cpp
src/feature/feature_apriltag_proj.cpp
)
SET(SRCS_LANDMARK
src/landmark/landmark_apriltag.cpp
......
......@@ -19,24 +19,62 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#include "apriltag/feature/feature_apriltag_pose.h"
#ifndef FEATURE_APRILTAG_H_
#define FEATURE_APRILTAG_H_
//Wolf includes
#include "core/feature/feature_base.h"
// opencv
#include <opencv2/features2d.hpp>
namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureApriltag);
//class FeatureApriltag
class FeatureApriltag : public FeatureBase
{
public:
FeatureApriltag(
const std::string _type,
const Eigen::VectorXd & _measurement,
const Eigen::MatrixXd & _meas_covariance,
const int _tag_id,
const Vector8d & _corners_vec,
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO);
~FeatureApriltag() override;
/** \brief Return tag id
*
**/
int getTagId() const;
/** \brief Return vector of tag corners
*
**/
const std::vector<cv::Point2d>& getTagCorners() const;
private:
int tag_id_;
std::vector<cv::Point2d> tag_corners_;
};
FeatureApriltagPose::FeatureApriltagPose(const Eigen::Vector7d & _measurement,
const Eigen::Matrix6d & _meas_uncertainty,
const int _tag_id,
const Vector8d & _corners_vec,
double _rep_error1,
double _rep_error2,
bool _use_rotation,
UncertaintyType _uncertainty_type) :
FeatureBase("FeatureApriltagPose", _measurement, _meas_uncertainty, _uncertainty_type),
tag_id_ (_tag_id),
tag_corners_(4),
rep_error1_(_rep_error1),
rep_error2_(_rep_error2),
use_rotation_(_use_rotation)
inline FeatureApriltag::FeatureApriltag(
const std::string _type,
const Eigen::VectorXd & _measurement,
const Eigen::MatrixXd & _meas_uncertainty,
const int _tag_id,
const Vector8d & _corners_vec,
UncertaintyType _uncertainty_type) :
FeatureBase(_type, _measurement, _meas_uncertainty, _uncertainty_type),
tag_id_(_tag_id),
tag_corners_(4)
{
setTrackId(_tag_id); // assuming there is a single landmark with this id in the scene
......@@ -47,34 +85,22 @@ FeatureApriltagPose::FeatureApriltagPose(const Eigen::Vector7d & _measurement,
}
}
FeatureApriltagPose::~FeatureApriltagPose()
inline FeatureApriltag::~FeatureApriltag()
{
//
}
double FeatureApriltagPose::getTagId() const
inline int FeatureApriltag::getTagId() const
{
return tag_id_;
}
const std::vector<cv::Point2d>& FeatureApriltagPose::getTagCorners() const
inline const std::vector<cv::Point2d>& FeatureApriltag::getTagCorners() const
{
return tag_corners_;
}
double FeatureApriltagPose::getRepError1() const
{
return rep_error1_;
}
double FeatureApriltagPose::getRepError2() const
{
return rep_error2_;
}
bool FeatureApriltagPose::getUserotation() const
{
return use_rotation_;
}
} // namespace wolf
#endif
......@@ -19,17 +19,18 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef FEATURE_APRILTAG_H_
#define FEATURE_APRILTAG_H_
#ifndef FEATURE_APRILTAG_POSE_H_
#define FEATURE_APRILTAG_POSE_H_
//Wolf includes
#include "core/feature/feature_base.h"
// Wolf apriltag
#include "apriltag/feature/feature_apriltag.h"
//std includes
// Wolf core
#include <core/feature/feature_base.h>
//external library incudes
#include "apriltag/apriltag.h"
#include "apriltag/common/zarray.h"
// UMich apriltag library
#include <apriltag/apriltag.h>
#include <apriltag/common/zarray.h>
// opencv
#include <opencv2/features2d.hpp>
......@@ -40,12 +41,13 @@ WOLF_PTR_TYPEDEFS(FeatureApriltagPose);
//class FeatureApriltagPose
class FeatureApriltagPose : public FeatureBase
class FeatureApriltagPose : public FeatureApriltag
{
public:
FeatureApriltagPose(const Eigen::Vector7d & _measurement,
const Eigen::Matrix6d & _meas_covariance,
FeatureApriltagPose(
const Eigen::Vector7d & _measurement,
const Eigen::Matrix6d & _meas_info,
const int _tag_id,
const Vector8d & _corners_vec,
double _rep_error1,
......@@ -54,29 +56,50 @@ class FeatureApriltagPose : public FeatureBase
UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO);
~FeatureApriltagPose() override;
/** \brief Returns tag id
*
* Returns tag id
*
**/
double getTagId() const;
const std::vector<cv::Point2d>& getTagCorners() const;
double getRepError1() const;
double getRepError2() const;
bool getUserotation() const;
private:
int tag_id_;
std::vector<cv::Point2d> tag_corners_;
double rep_error1_;
double rep_error2_;
bool use_rotation_;
};
inline FeatureApriltagPose::FeatureApriltagPose(
const Eigen::Vector7d & _measurement,
const Eigen::Matrix6d & _meas_info,
const int _tag_id,
const Vector8d & _corners_vec,
double _rep_error1,
double _rep_error2,
bool _use_rotation,
UncertaintyType _uncertainty_type) :
FeatureApriltag("FeatureApriltagPose", _measurement, _meas_info, _tag_id, _corners_vec, _uncertainty_type),
rep_error1_(_rep_error1),
rep_error2_(_rep_error2),
use_rotation_(_use_rotation)
{
}
inline FeatureApriltagPose::~FeatureApriltagPose()
{
//
}
inline double FeatureApriltagPose::getRepError1() const
{
return rep_error1_;
}
inline double FeatureApriltagPose::getRepError2() const
{
return rep_error2_;
}
inline bool FeatureApriltagPose::getUserotation() const
{
return use_rotation_;
}
} // namespace wolf
#endif
......@@ -22,14 +22,15 @@
#ifndef FEATURE_APRILTAG_PROJ_H_
#define FEATURE_APRILTAG_PROJ_H_
//Wolf includes
#include "core/feature/feature_base.h"
// Wolf apriltag
#include "apriltag/feature/feature_apriltag.h"
//std includes
// Wolf core
#include <core/feature/feature_base.h>
//external library incudes
#include "apriltag/apriltag.h"
#include "apriltag/common/zarray.h"
// UMich apriltag library
#include <apriltag/apriltag.h>
#include <apriltag/common/zarray.h>
// opencv
#include <opencv2/features2d.hpp>
......@@ -39,37 +40,58 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureApriltagProj);
//class FeatureApriltagProj
class FeatureApriltagProj : public FeatureBase
class FeatureApriltagProj : public FeatureApriltag
{
public:
FeatureApriltagProj(const Eigen::Vector8d & _measurement,
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(
const Eigen::Vector8d & _measurement,
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;
/** \brief Returns tag id
*
* Returns tag id
*
**/
int getTagId() const;
double getTagWidth() const;
const Eigen::Vector7d& getPosePnp() const;
const std::vector<cv::Point2d>& getTagCorners() const;
double getTagWidth() const;
private:
int tag_id_;
double tag_width_;
std::vector<cv::Point2d> tag_corners_;
Eigen::Vector7d pose_pnp_;
double tag_width_;
};
inline FeatureApriltagProj::FeatureApriltagProj(
const Eigen::Vector8d & _measurement,
const Eigen::Matrix8d & _meas_covariance,
const int _tag_id,
const double _tag_width,
const Eigen::Vector7d& _pose_pnp,
UncertaintyType _uncertainty_type) :
FeatureApriltag("FeatureApriltagProj", _measurement, _meas_covariance, _tag_id, _measurement, _uncertainty_type),
pose_pnp_(_pose_pnp),
tag_width_(_tag_width)
{
}
inline FeatureApriltagProj::~FeatureApriltagProj()
{
//
}
inline const Eigen::Vector7d& FeatureApriltagProj::getPosePnp() const
{
return pose_pnp_;
}
inline double FeatureApriltagProj::getTagWidth() const
{
return tag_width_;
}
} // namespace wolf
#endif
......@@ -23,6 +23,8 @@
#define _PROCESSOR_TRACKER_LANDMARK_APRILTAG_H_
// Wolf apriltag includes
#include "apriltag/feature/feature_apriltag.h"
#include "apriltag/feature/feature_apriltag_proj.h"
#include "apriltag/feature/feature_apriltag_pose.h"
#include "apriltag/landmark/landmark_apriltag.h"
#include "apriltag/factor/factor_apriltag_proj.h"
......
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#include "apriltag/feature/feature_apriltag_proj.h"
namespace wolf {
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),
pose_pnp_(_pose_pnp)
{
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 = _measurement[2*i];
tag_corners_[i].y = _measurement[2*i+1];
}
}
FeatureApriltagProj::~FeatureApriltagProj()
{
//
}
int FeatureApriltagProj::getTagId() const
{
return tag_id_;
}
double FeatureApriltagProj::getTagWidth() const
{
return tag_width_;
}
const std::vector<cv::Point2d>& FeatureApriltagProj::getTagCorners() const
{
return tag_corners_;
}
const Eigen::Vector7d& FeatureApriltagProj::getPosePnp() const
{
return pose_pnp_;
}
} // namespace wolf
......@@ -41,7 +41,6 @@ ProcessorTrackerLandmarkApriltag::ProcessorTrackerLandmarkApriltag( ParamsProces
min_features_for_keyframe_(_params_tracker_landmark_apriltag->min_features_for_keyframe),
nb_vote_for_every_first_(_params_tracker_landmark_apriltag->nb_vote_for_every_first_),
nb_vote_(0)
{
// configure apriltag detector
std::string famname(_params_tracker_landmark_apriltag->tag_family_);
......@@ -179,20 +178,20 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
info.bottomRightCorner(3,3) = M_1_PI*M_1_PI * Eigen::Matrix3d::Identity(); // 180 degrees standar deviation
}
// add to detected features list
detections_incoming_.push_back(std::make_shared<FeatureApriltagPose>(pose,
info,
tag_id,
corners_vec,
rep_error1,
rep_error2,
use_rotation,
FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
info,
tag_id,
corners_vec,
rep_error1,
rep_error2,
use_rotation,
FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
}
}
WOLF_INFO("\ndetections_incoming_: ", detections_incoming_.size())
apriltag_detections_destroy(detections);
}
......@@ -409,7 +408,7 @@ unsigned int ProcessorTrackerLandmarkApriltag::findLandmarks(const LandmarkBaseP
{
for (auto feature_in_image : detections_incoming_)
{
int tag_id(std::static_pointer_cast<FeatureApriltagPose>(feature_in_image)->getTagId());
int tag_id(std::static_pointer_cast<FeatureApriltag>(feature_in_image)->getTagId());
for (auto landmark_in_ptr : _landmarks_in)
{
......
......@@ -8,9 +8,7 @@ add_subdirectory(gtest)
# #
###########################################################
wolf_add_gtest(gtest_feature_apriltag gtest_feature_apriltag_pose.cpp)
wolf_add_gtest(gtest_feature_apriltag_proj gtest_feature_apriltag_proj.cpp)
wolf_add_gtest(gtest_features_apriltag gtest_features_apriltag.cpp)
wolf_add_gtest(gtest_landmark_apriltag gtest_landmark_apriltag.cpp)
......
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/**
* \file gtest_feature_apriltag_proj.cpp
*
* Created on: May 30, 2022
* \author: mfourmy
*/
#include <core/utils/utils_gtest.h>
#include "apriltag/feature/feature_apriltag_proj.h"
using namespace wolf;
class FeatureApriltagProj_test : public testing::Test
{
public:
Eigen::Vector8d meas_;
Eigen::Matrix8d cov_;
int tag_id_;
double tag_width_;
apriltag_detection_t det_;
Eigen::Vector7d pose_pnp_;
void SetUp() override
{
tag_id_ = 1;
tag_width_ = 0.2;
meas_ << 1.0, -1.0,
1.0, 1.0,
-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_, pose_pnp_);
ASSERT_EQ(f->getType(), "FeatureApriltagProj");
}
TEST_F(FeatureApriltagProj_test, getters)
{
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_, pose_pnp_);
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);
return RUN_ALL_TESTS();
}
......@@ -19,27 +19,24 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/**
* \file gtest_feature_apriltag_pose.cpp
*
* Created on: Dec 22, 2018
* \author: jsola
*/
#include <core/utils/utils_gtest.h>
#include "apriltag/feature/feature_apriltag.h"
#include "apriltag/feature/feature_apriltag_pose.h"
#include "apriltag/feature/feature_apriltag_proj.h"
using namespace wolf;
class FeatureApriltag_test : public testing::Test
class FeatureApriltag_fixture : public testing::Test
{
public:
Eigen::Vector7d pose;
Eigen::Matrix6d cov;
Eigen::Matrix6d info_pose;
Eigen::Vector8d corners_vec;