Skip to content
Snippets Groups Projects
Commit baff8d06 authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Remove forgotten files related to apriltag

parent 2650a66f
No related branches found
No related tags found
1 merge request!4Remove forgotten files related to apriltag
Expand all Hide whitespace changes
Inline Side-by-side
src/feature/feature_apriltag.cpp deleted 100644 → 0
+ 0
− 66
View file @ 2650a66f
#include "base/feature/feature_apriltag.h"
namespace wolf {
FeatureApriltag::FeatureApriltag(const Eigen::Vector7s & _measurement,
const Eigen::Matrix6s & _meas_uncertainty,
const int _tag_id,
const apriltag_detection_t & _det,
Scalar _rep_error1,
Scalar _rep_error2,
bool _use_rotation,
UncertaintyType _uncertainty_type) :
FeatureBase("APRILTAG", _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);
for (int i = 0; i < 4; i++)
{
tag_corners_[i].x = detection_.p[i][0];
tag_corners_[i].y = detection_.p[i][1];
}
}
FeatureApriltag::~FeatureApriltag()
{
//
}
Scalar 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_;
}
Scalar FeatureApriltag::getRepError1() const
{
return rep_error1_;
}
Scalar FeatureApriltag::getRepError2() const
{
return rep_error2_;
}
bool FeatureApriltag::getUserotation() const
{
return use_rotation_;
}
} // namespace wolf
src/landmark/landmark_apriltag.cpp deleted 100644 → 0
+ 0
− 93
View file @ 2650a66f
#include "base/landmark/landmark_apriltag.h"
#include "base/common/factory.h"
#include "base/math/rotations.h"
#include "base/yaml/yaml_conversion.h"
namespace wolf {
LandmarkApriltag::LandmarkApriltag(Eigen::Vector7s& pose, const int& _tagid, const Scalar& _tag_width) :
LandmarkBase("APRILTAG", std::make_shared<StateBlock>(pose.head(3)), std::make_shared<StateQuaternion>(pose.tail(4))),
tag_id_(_tagid),
tag_width_(_tag_width)
{
setDescriptor(Eigen::VectorXs::Constant(1,_tagid)); //change tagid to int ? do not use descriptor vector ?
setId(_tagid); // overwrite lmk ID to match tag_id.
}
LandmarkApriltag::~LandmarkApriltag()
{
//
}
Scalar LandmarkApriltag::getTagWidth() const
{
return tag_width_;
}
int LandmarkApriltag::getTagId() const
{
return tag_id_;
}
// LANDMARK SAVE AND LOAD FROM YAML
// static
LandmarkBasePtr LandmarkApriltag::create(const YAML::Node& _lmk_node)
{
// Parse YAML node with lmk info and data
unsigned int id = _lmk_node["id"] .as<unsigned int>();
unsigned int tag_id = _lmk_node["tag id"] .as<unsigned int>();
Scalar tag_width = _lmk_node["tag width"] .as<Scalar>();
Eigen::Vector3s pos = _lmk_node["position"] .as<Eigen::Vector3s>();
bool pos_fixed = _lmk_node["position fixed"] .as<bool>();
Eigen::Vector4s vquat;
if (_lmk_node["orientation"].size() == 3)
{
// we have been given 3 Euler angles in degrees
Eigen::Vector3s euler = M_TORAD * ( _lmk_node["orientation"] .as<Eigen::Vector3s>() );
Eigen::Matrix3s R = e2R(euler);
Eigen::Quaternions quat = R2q(R);
vquat = quat.coeffs();
}
else if (_lmk_node["orientation"].size() == 4)
{
// we have been given a quaternion
vquat = _lmk_node["orientation"] .as<Eigen::Vector4s>();
}
bool ori_fixed = _lmk_node["orientation fixed"] .as<bool>();
Eigen::Vector7s pose; pose << pos, vquat;
// Create a new landmark
LandmarkApriltagPtr lmk_ptr = std::make_shared<LandmarkApriltag>(pose, tag_id, tag_width);
lmk_ptr->setId(id);
lmk_ptr->getP()->setFixed(pos_fixed);
lmk_ptr->getO()->setFixed(ori_fixed);
return lmk_ptr;
}
YAML::Node LandmarkApriltag::saveToYaml() const
{
// First base things
YAML::Node node = LandmarkBase::saveToYaml();
// Then Apriltag specific things
node["tag id"] = getTagId();
node["tag width"] = getTagWidth();
return node;
}
// Register landmark creator
namespace
{
const bool WOLF_UNUSED registered_lmk_apriltag = LandmarkFactory::get().registerCreator("APRILTAG", LandmarkApriltag::create);
}
} // namespace wolf
src/processor/processor_tracker_landmark_apriltag.cpp deleted 100644 → 0
+ 0
− 669
View file @ 2650a66f
This diff is collapsed.
src/yaml/processor_tracker_landmark_apriltag_yaml.cpp deleted 100644 → 0
+ 0
− 92
View file @ 2650a66f
/**
* \file processor_tracker_landmark_apriltag_yaml.cpp
*
* Created on: Dec 6, 2018
* \author: jsola
*/
// wolf
#include "base/processor/processor_tracker_landmark_apriltag.h"
#include "base/yaml/yaml_conversion.h"
#include "base/common/factory.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
namespace wolf
{
namespace
{
static ProcessorParamsBasePtr createProcessorParamsLandmarkApriltag(const std::string & _filename_dot_yaml)
{
YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
if (config.IsNull())
{
WOLF_ERROR("Invalid YAML file!");
return nullptr;
}
else if (config["processor type"].as<std::string>() == "TRACKER LANDMARK APRILTAG")
{
ProcessorParamsTrackerLandmarkApriltagPtr params = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
YAML::Node detector_parameters = config["detector parameters"];
params->quad_decimate_ = detector_parameters["quad_decimate"] .as<Scalar>();
params->quad_sigma_ = detector_parameters["quad_sigma"] .as<Scalar>();
params->nthreads_ = detector_parameters["nthreads"] .as<int>();
params->debug_ = detector_parameters["debug"] .as<bool>();
params->refine_edges_ = detector_parameters["refine_edges"] .as<bool>();
params->ippe_min_ratio_ = detector_parameters["ippe_min_ratio"] .as<Scalar>();
params->ippe_max_rep_error_ = detector_parameters["ippe_max_rep_error"] .as<Scalar>();
YAML::Node tag_parameters = config["tag parameters"];
params->tag_family_ = tag_parameters["tag_family"] .as<std::string>();
// params->tag_black_border_ = tag_parameters["tag_black_border"] .as<int>();
params->tag_width_default_ = tag_parameters["tag_width_default"] .as<Scalar>();
// read list of tag widths
YAML::Node tag_widths = config["tag widths"];
for (auto pair_id_width : tag_widths)
{
int tag_id = pair_id_width.first .as<int>();
Scalar tag_width = pair_id_width.second .as<Scalar>();
params->tag_widths_.emplace(tag_id, tag_width);
}
YAML::Node noise = config["noise"];
params->std_xy_ = noise["std_xy"] .as<Scalar>();
params->std_z_ = noise["std_z"] .as<Scalar>();
params->std_rpy_ = M_TORAD * noise["std_rpy_degrees"] .as<Scalar>();
params->std_pix_ = noise["std_pix"] .as<Scalar>();
YAML::Node vote = config["vote"];
params->voting_active = vote["voting active"] .as<bool>();
params->min_time_vote_ = vote["min_time_vote"] .as<Scalar>();
params->max_time_vote_ = vote["max_time_vote"] .as<Scalar>();
params->min_features_for_keyframe = vote["min_features_for_keyframe"] .as<unsigned int>();
params->max_features_diff_ = vote["max_features_diff"] .as<int>();
params->nb_vote_for_every_first_ = vote["nb_vote_for_every_first"] .as<int>();
params->enough_info_necessary_ = vote["enough_info_necessary"] .as<bool>();
params->reestimate_last_frame_ = config["reestimate_last_frame"] .as<bool>();
params->add_3D_cstr_ = config["add_3D_cstr"] .as<bool>();
return params;
}
else
{
WOLF_ERROR("Wrong processor type! Should be \"TRACKER LANDMARK APRILTAG\"");
return nullptr;
}
return nullptr;
}
// Register in the SensorFactory
const bool WOLF_UNUSED registered_prc_apriltag = ProcessorParamsFactory::get().registerCreator("TRACKER LANDMARK APRILTAG", createProcessorParamsLandmarkApriltag);
const bool WOLF_UNUSED registered_prc_apriltag_wrapper = ProcessorParamsFactory::get().registerCreator("TRACKER LANDMARK APRILTAG WRAPPER", createProcessorParamsLandmarkApriltag);
} // namespace [unnamed]
} // namespace wolf
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