add tracking position output

This commit is contained in:
lxm 2023-09-08 17:46:53 +08:00
parent 230c41a41c
commit b93d5c7381
4 changed files with 1359 additions and 1055 deletions

View File

@ -11,21 +11,18 @@
#define SV_MODEL_DIR "/SpireCV/models/"
#define SV_ROOT_DIR "/SpireCV/"
namespace sv {
namespace sv
{
using namespace cv;
using namespace cv::dnn;
void _cameraMatrix2Fov(cv::Mat camera_matrix_, int img_w_, int img_h_, double &fov_x_, double &fov_y_)
{
fov_x_ = 2 * atan(img_w_ / 2. / camera_matrix_.at<double>(0, 0)) * SV_RAD2DEG;
fov_y_ = 2 * atan(img_h_ / 2. / camera_matrix_.at<double>(1, 1)) * SV_RAD2DEG;
}
CameraAlgorithm::CameraAlgorithm()
{
// this->_value = NULL;
@ -66,20 +63,17 @@ void CameraAlgorithm::loadCameraParams(std::string yaml_fn_)
// std::cout << this->fov_x << ", " << this->fov_y << std::endl;
}
void CameraAlgorithm::loadAlgorithmParams(std::string json_fn_)
{
this->alg_params_fn = json_fn_;
}
ArucoDetector::ArucoDetector()
{
_params_loaded = false;
_dictionary = nullptr;
}
void ArucoDetector::_load()
{
JsonValue all_value;
@ -93,63 +87,79 @@ void ArucoDetector::_load()
_dictionary_id = 10;
// _detector_params = aruco::DetectorParameters::create();
_detector_params = new aruco::DetectorParameters;
for (auto i : aruco_params_value) {
if ("_dictionary_id" == std::string(i->key)) {
for (auto i : aruco_params_value)
{
if ("_dictionary_id" == std::string(i->key))
{
_dictionary_id = i->value.toNumber();
}
else if ("adaptiveThreshConstant" == std::string(i->key)) {
else if ("adaptiveThreshConstant" == std::string(i->key))
{
// std::cout << "adaptiveThreshConstant (old, new): " << _detector_params->adaptiveThreshConstant << ", " << i->value.toNumber() << std::endl;
_detector_params->adaptiveThreshConstant = i->value.toNumber();
}
else if ("adaptiveThreshWinSizeMax" == std::string(i->key)) {
else if ("adaptiveThreshWinSizeMax" == std::string(i->key))
{
// std::cout << "adaptiveThreshWinSizeMax (old, new): " << _detector_params->adaptiveThreshWinSizeMax << ", " << i->value.toNumber() << std::endl;
_detector_params->adaptiveThreshWinSizeMax = i->value.toNumber();
}
else if ("adaptiveThreshWinSizeMin" == std::string(i->key)) {
else if ("adaptiveThreshWinSizeMin" == std::string(i->key))
{
// std::cout << "adaptiveThreshWinSizeMin (old, new): " << _detector_params->adaptiveThreshWinSizeMin << ", " << i->value.toNumber() << std::endl;
_detector_params->adaptiveThreshWinSizeMin = i->value.toNumber();
}
else if ("adaptiveThreshWinSizeStep" == std::string(i->key)) {
else if ("adaptiveThreshWinSizeStep" == std::string(i->key))
{
// std::cout << "adaptiveThreshWinSizeStep (old, new): " << _detector_params->adaptiveThreshWinSizeStep << ", " << i->value.toNumber() << std::endl;
_detector_params->adaptiveThreshWinSizeStep = i->value.toNumber();
}
else if ("aprilTagCriticalRad" == std::string(i->key)) {
else if ("aprilTagCriticalRad" == std::string(i->key))
{
// std::cout << "aprilTagCriticalRad (old, new): " << _detector_params->aprilTagCriticalRad << ", " << i->value.toNumber() << std::endl;
_detector_params->aprilTagCriticalRad = i->value.toNumber();
}
else if ("aprilTagDeglitch" == std::string(i->key)) {
else if ("aprilTagDeglitch" == std::string(i->key))
{
// std::cout << "aprilTagDeglitch (old, new): " << _detector_params->aprilTagDeglitch << ", " << i->value.toNumber() << std::endl;
_detector_params->aprilTagDeglitch = i->value.toNumber();
}
else if ("aprilTagMaxLineFitMse" == std::string(i->key)) {
else if ("aprilTagMaxLineFitMse" == std::string(i->key))
{
// std::cout << "aprilTagMaxLineFitMse (old, new): " << _detector_params->aprilTagMaxLineFitMse << ", " << i->value.toNumber() << std::endl;
_detector_params->aprilTagMaxLineFitMse = i->value.toNumber();
}
else if ("aprilTagMaxNmaxima" == std::string(i->key)) {
else if ("aprilTagMaxNmaxima" == std::string(i->key))
{
// std::cout << "aprilTagMaxNmaxima (old, new): " << _detector_params->aprilTagMaxNmaxima << ", " << i->value.toNumber() << std::endl;
_detector_params->aprilTagMaxNmaxima = i->value.toNumber();
}
else if ("aprilTagMinClusterPixels" == std::string(i->key)) {
else if ("aprilTagMinClusterPixels" == std::string(i->key))
{
// std::cout << "aprilTagMinClusterPixels (old, new): " << _detector_params->aprilTagMinClusterPixels << ", " << i->value.toNumber() << std::endl;
_detector_params->aprilTagMinClusterPixels = i->value.toNumber();
}
else if ("aprilTagMinWhiteBlackDiff" == std::string(i->key)) {
else if ("aprilTagMinWhiteBlackDiff" == std::string(i->key))
{
// std::cout << "aprilTagMinWhiteBlackDiff (old, new): " << _detector_params->aprilTagMinWhiteBlackDiff << ", " << i->value.toNumber() << std::endl;
_detector_params->aprilTagMinWhiteBlackDiff = i->value.toNumber();
}
else if ("aprilTagQuadDecimate" == std::string(i->key)) {
else if ("aprilTagQuadDecimate" == std::string(i->key))
{
// std::cout << "aprilTagQuadDecimate (old, new): " << _detector_params->aprilTagQuadDecimate << ", " << i->value.toNumber() << std::endl;
_detector_params->aprilTagQuadDecimate = i->value.toNumber();
}
else if ("aprilTagQuadSigma" == std::string(i->key)) {
else if ("aprilTagQuadSigma" == std::string(i->key))
{
// std::cout << "aprilTagQuadSigma (old, new): " << _detector_params->aprilTagQuadSigma << ", " << i->value.toNumber() << std::endl;
_detector_params->aprilTagQuadSigma = i->value.toNumber();
}
else if ("cornerRefinementMaxIterations" == std::string(i->key)) {
else if ("cornerRefinementMaxIterations" == std::string(i->key))
{
// std::cout << "cornerRefinementMaxIterations (old, new): " << _detector_params->cornerRefinementMaxIterations << ", " << i->value.toNumber() << std::endl;
_detector_params->cornerRefinementMaxIterations = i->value.toNumber();
}
else if ("cornerRefinementMethod" == std::string(i->key)) {
else if ("cornerRefinementMethod" == std::string(i->key))
{
// std::cout << "cornerRefinementMethod (old, new): " << _detector_params->cornerRefinementMethod << ", " << i->value.toNumber() << std::endl;
// _detector_params->cornerRefinementMethod = i->value.toNumber();
int method = (int)i->value.toNumber();
@ -170,109 +180,139 @@ void ArucoDetector::_load()
_detector_params->cornerRefinementMethod = cv::aruco::CornerRefineMethod::CORNER_REFINE_NONE;
}
}
else if ("cornerRefinementMinAccuracy" == std::string(i->key)) {
else if ("cornerRefinementMinAccuracy" == std::string(i->key))
{
// std::cout << "cornerRefinementMinAccuracy (old, new): " << _detector_params->cornerRefinementMinAccuracy << ", " << i->value.toNumber() << std::endl;
_detector_params->cornerRefinementMinAccuracy = i->value.toNumber();
}
else if ("cornerRefinementWinSize" == std::string(i->key)) {
else if ("cornerRefinementWinSize" == std::string(i->key))
{
// std::cout << "cornerRefinementWinSize (old, new): " << _detector_params->cornerRefinementWinSize << ", " << i->value.toNumber() << std::endl;
_detector_params->cornerRefinementWinSize = i->value.toNumber();
}
else if ("detectInvertedMarker" == std::string(i->key)) {
else if ("detectInvertedMarker" == std::string(i->key))
{
bool json_tf = false;
if (i->value.getTag() == JSON_TRUE) json_tf = true;
if (i->value.getTag() == JSON_TRUE)
json_tf = true;
// std::cout << "detectInvertedMarker (old, new): " << _detector_params->detectInvertedMarker << ", " << json_tf << std::endl;
_detector_params->detectInvertedMarker = json_tf;
}
else if ("errorCorrectionRate" == std::string(i->key)) {
else if ("errorCorrectionRate" == std::string(i->key))
{
// std::cout << "errorCorrectionRate (old, new): " << _detector_params->errorCorrectionRate << ", " << i->value.toNumber() << std::endl;
_detector_params->errorCorrectionRate = i->value.toNumber();
}
else if ("markerBorderBits" == std::string(i->key)) {
else if ("markerBorderBits" == std::string(i->key))
{
// std::cout << "markerBorderBits (old, new): " << _detector_params->markerBorderBits << ", " << i->value.toNumber() << std::endl;
_detector_params->markerBorderBits = i->value.toNumber();
}
else if ("maxErroneousBitsInBorderRate" == std::string(i->key)) {
else if ("maxErroneousBitsInBorderRate" == std::string(i->key))
{
// std::cout << "maxErroneousBitsInBorderRate (old, new): " << _detector_params->maxErroneousBitsInBorderRate << ", " << i->value.toNumber() << std::endl;
_detector_params->maxErroneousBitsInBorderRate = i->value.toNumber();
}
else if ("maxMarkerPerimeterRate" == std::string(i->key)) {
else if ("maxMarkerPerimeterRate" == std::string(i->key))
{
// std::cout << "maxMarkerPerimeterRate (old, new): " << _detector_params->maxMarkerPerimeterRate << ", " << i->value.toNumber() << std::endl;
_detector_params->maxMarkerPerimeterRate = i->value.toNumber();
}
else if ("minCornerDistanceRate" == std::string(i->key)) {
else if ("minCornerDistanceRate" == std::string(i->key))
{
// std::cout << "minCornerDistanceRate (old, new): " << _detector_params->minCornerDistanceRate << ", " << i->value.toNumber() << std::endl;
_detector_params->minCornerDistanceRate = i->value.toNumber();
}
else if ("minDistanceToBorder" == std::string(i->key)) {
else if ("minDistanceToBorder" == std::string(i->key))
{
// std::cout << "minDistanceToBorder (old, new): " << _detector_params->minDistanceToBorder << ", " << i->value.toNumber() << std::endl;
_detector_params->minDistanceToBorder = i->value.toNumber();
}
else if ("minMarkerDistanceRate" == std::string(i->key)) {
else if ("minMarkerDistanceRate" == std::string(i->key))
{
// std::cout << "minMarkerDistanceRate (old, new): " << _detector_params->minMarkerDistanceRate << ", " << i->value.toNumber() << std::endl;
_detector_params->minMarkerDistanceRate = i->value.toNumber();
}
else if ("minMarkerLengthRatioOriginalImg" == std::string(i->key)) {
else if ("minMarkerLengthRatioOriginalImg" == std::string(i->key))
{
// std::cout << "minMarkerLengthRatioOriginalImg (old, new): " << _detector_params->minMarkerLengthRatioOriginalImg << ", " << i->value.toNumber() << std::endl;
_detector_params->minMarkerLengthRatioOriginalImg = i->value.toNumber();
}
else if ("minMarkerPerimeterRate" == std::string(i->key)) {
else if ("minMarkerPerimeterRate" == std::string(i->key))
{
// std::cout << "minMarkerPerimeterRate (old, new): " << _detector_params->minMarkerPerimeterRate << ", " << i->value.toNumber() << std::endl;
_detector_params->minMarkerPerimeterRate = i->value.toNumber();
}
else if ("minOtsuStdDev" == std::string(i->key)) {
else if ("minOtsuStdDev" == std::string(i->key))
{
// std::cout << "minOtsuStdDev (old, new): " << _detector_params->minOtsuStdDev << ", " << i->value.toNumber() << std::endl;
_detector_params->minOtsuStdDev = i->value.toNumber();
}
else if ("minSideLengthCanonicalImg" == std::string(i->key)) {
else if ("minSideLengthCanonicalImg" == std::string(i->key))
{
// std::cout << "minSideLengthCanonicalImg (old, new): " << _detector_params->minSideLengthCanonicalImg << ", " << i->value.toNumber() << std::endl;
_detector_params->minSideLengthCanonicalImg = i->value.toNumber();
}
else if ("perspectiveRemoveIgnoredMarginPerCell" == std::string(i->key)) {
else if ("perspectiveRemoveIgnoredMarginPerCell" == std::string(i->key))
{
// std::cout << "perspectiveRemoveIgnoredMarginPerCell (old, new): " << _detector_params->perspectiveRemoveIgnoredMarginPerCell << ", " << i->value.toNumber() << std::endl;
_detector_params->perspectiveRemoveIgnoredMarginPerCell = i->value.toNumber();
}
else if ("perspectiveRemovePixelPerCell" == std::string(i->key)) {
else if ("perspectiveRemovePixelPerCell" == std::string(i->key))
{
// std::cout << "perspectiveRemovePixelPerCell (old, new): " << _detector_params->perspectiveRemovePixelPerCell << ", " << i->value.toNumber() << std::endl;
_detector_params->perspectiveRemovePixelPerCell = i->value.toNumber();
}
else if ("polygonalApproxAccuracyRate" == std::string(i->key)) {
else if ("polygonalApproxAccuracyRate" == std::string(i->key))
{
// std::cout << "polygonalApproxAccuracyRate (old, new): " << _detector_params->polygonalApproxAccuracyRate << ", " << i->value.toNumber() << std::endl;
_detector_params->polygonalApproxAccuracyRate = i->value.toNumber();
}
else if ("useAruco3Detection" == std::string(i->key)) {
else if ("useAruco3Detection" == std::string(i->key))
{
bool json_tf = false;
if (i->value.getTag() == JSON_TRUE) json_tf = true;
if (i->value.getTag() == JSON_TRUE)
json_tf = true;
// std::cout << "useAruco3Detection (old, new): " << _detector_params->useAruco3Detection << ", " << json_tf << std::endl;
_detector_params->useAruco3Detection = json_tf;
}
else if ("markerIds" == std::string(i->key) && i->value.getTag() == JSON_ARRAY) {
else if ("markerIds" == std::string(i->key) && i->value.getTag() == JSON_ARRAY)
{
int jcnt = 0;
for (auto j : i->value) {
if (jcnt == 0 && j->value.toNumber() == -1) {
for (auto j : i->value)
{
if (jcnt == 0 && j->value.toNumber() == -1)
{
_ids_need.push_back(-1);
break;
}
else {
else
{
_ids_need.push_back(j->value.toNumber());
}
}
}
else if ("markerLengths" == std::string(i->key) && i->value.getTag() == JSON_ARRAY) {
for (auto j : i->value) {
if (_ids_need.size() > 0 && _ids_need[0] == -1) {
else if ("markerLengths" == std::string(i->key) && i->value.getTag() == JSON_ARRAY)
{
for (auto j : i->value)
{
if (_ids_need.size() > 0 && _ids_need[0] == -1)
{
_lengths_need.push_back(j->value.toNumber());
break;
}
else {
else
{
_lengths_need.push_back(j->value.toNumber());
}
}
}
}
if (_ids_need.size() == 0) _ids_need.push_back(-1);
if (_lengths_need.size() != _ids_need.size()) {
if (_ids_need.size() == 0)
_ids_need.push_back(-1);
if (_lengths_need.size() != _ids_need.size())
{
throw std::runtime_error("SpireCV (106) Parameter markerIds.length != markerLengths.length!");
}
@ -282,7 +322,6 @@ void ArucoDetector::_load()
// std::cout << "_lengths_need: " << l << std::endl;
}
void ArucoDetector::detect(cv::Mat img_, TargetsInFrame &tgts_)
{
if (!_params_loaded)
@ -311,7 +350,6 @@ void ArucoDetector::detect(cv::Mat img_, TargetsInFrame& tgts_)
// detect markers and estimate pose
aruco::detectMarkers(img_, this->_dictionary, corners, ids, _detector_params, rejected);
if (ids.size() > 0)
{
if (_ids_need[0] == -1)
@ -383,8 +421,6 @@ void ArucoDetector::detect(cv::Mat img_, TargetsInFrame& tgts_)
// waitKey(10);
}
EllipseDetector::EllipseDetector()
{
this->_ed = NULL;
@ -393,7 +429,11 @@ EllipseDetector::EllipseDetector()
}
EllipseDetector::~EllipseDetector()
{
if (_ed) { delete _ed; _ed = NULL; }
if (_ed)
{
delete _ed;
_ed = NULL;
}
}
void EllipseDetector::detect(cv::Mat img_, TargetsInFrame &tgts_)
@ -435,10 +475,8 @@ LandingMarkerDetectorBase::LandingMarkerDetectorBase()
setupImpl();
}
LandingMarkerDetectorBase::~LandingMarkerDetectorBase()
{
}
bool LandingMarkerDetectorBase::isParamsLoaded()
@ -454,7 +492,6 @@ std::vector<std::string> LandingMarkerDetectorBase::getLabelsNeed()
return this->_labels_need;
}
void LandingMarkerDetectorBase::detect(cv::Mat img_, TargetsInFrame &tgts_)
{
if (!_params_loaded)
@ -489,11 +526,16 @@ void LandingMarkerDetectorBase::detect(cv::Mat img_, TargetsInFrame& tgts_)
int y1 = rect.y;
int x2 = rect.x + rect.width;
int y2 = rect.y + rect.height;
if (x1 < 0) x1 = 0;
if (y1 < 0) y1 = 0;
if (x2 > img_.cols - 1) x2 = img_.cols - 1;
if (y2 > img_.rows - 1) y2 = img_.rows - 1;
if (x2 - x1 < 5 || y2 - y1 < 5) continue;
if (x1 < 0)
x1 = 0;
if (y1 < 0)
y1 = 0;
if (x2 > img_.cols - 1)
x2 = img_.cols - 1;
if (y2 > img_.rows - 1)
y2 = img_.rows - 1;
if (x2 - x1 < 5 || y2 - y1 < 5)
continue;
rect.x = x1;
rect.y = y1;
rect.width = x2 - x1;
@ -521,7 +563,8 @@ void LandingMarkerDetectorBase::detect(cv::Mat img_, TargetsInFrame& tgts_)
need = true;
}
}
if (!need) label = 0;
if (!need)
label = 0;
yaed::Ellipse e = ellsCned[i];
if (label > 0)
@ -541,10 +584,8 @@ bool LandingMarkerDetectorBase::setupImpl()
}
void LandingMarkerDetectorBase::roiCNN(std::vector<cv::Mat> &input_rois_, std::vector<int> &output_labels_)
{
}
void LandingMarkerDetectorBase::_loadLabels()
{
JsonValue all_value;
@ -554,13 +595,17 @@ void LandingMarkerDetectorBase::_loadLabels()
JsonValue landing_params_value;
_parser_algorithm_params("LandingMarkerDetector", all_value, landing_params_value);
for (auto i : landing_params_value) {
if ("labels" == std::string(i->key) && i->value.getTag() == JSON_ARRAY) {
for (auto j : i->value) {
for (auto i : landing_params_value)
{
if ("labels" == std::string(i->key) && i->value.getTag() == JSON_ARRAY)
{
for (auto j : i->value)
{
this->_labels_need.push_back(j->value.toString());
}
}
else if ("maxCandidates" == std::string(i->key)) {
else if ("maxCandidates" == std::string(i->key))
{
this->_max_candidates = i->value.toNumber();
// std::cout << "maxCandidates: " << this->_max_candidates << std::endl;
}
@ -568,7 +613,6 @@ void LandingMarkerDetectorBase::_loadLabels()
setupImpl();
}
void EllipseDetector::detectAllInDirectory(std::string input_img_dir_, std::string output_json_dir_)
{
if (!_params_loaded)
@ -652,88 +696,107 @@ void EllipseDetector::_load()
float fT_TCN_P;
float fThre_R;
for (auto i : ell_params_value) {
if ("preProcessingGaussKernel" == std::string(i->key)) {
for (auto i : ell_params_value)
{
if ("preProcessingGaussKernel" == std::string(i->key))
{
int sigma = i->value.toNumber();
szPreProcessingGaussKernel = cv::Size(sigma, sigma);
// std::cout << "preProcessingGaussKernel: " << sigma << std::endl;
}
else if ("preProcessingGaussSigma" == std::string(i->key)) {
else if ("preProcessingGaussSigma" == std::string(i->key))
{
dPreProcessingGaussSigma = i->value.toNumber();
// std::cout << "preProcessingGaussSigma: " << dPreProcessingGaussSigma << std::endl;
}
else if ("thPosition" == std::string(i->key)) {
else if ("thPosition" == std::string(i->key))
{
fThPosition = i->value.toNumber();
// std::cout << "thPosition: " << fThPosition << std::endl;
}
else if ("maxCenterDistance" == std::string(i->key)) {
else if ("maxCenterDistance" == std::string(i->key))
{
this->_max_center_distance_ratio = i->value.toNumber();
fMaxCenterDistance = sqrt(float(this->image_width * this->image_width + this->image_height * this->image_height)) * this->_max_center_distance_ratio;
// std::cout << "maxCenterDistance: " << this->_max_center_distance_ratio << std::endl;
}
else if ("minEdgeLength" == std::string(i->key)) {
else if ("minEdgeLength" == std::string(i->key))
{
iMinEdgeLength = i->value.toNumber();
// std::cout << "minEdgeLength: " << iMinEdgeLength << std::endl;
}
else if ("minOrientedRectSide" == std::string(i->key)) {
else if ("minOrientedRectSide" == std::string(i->key))
{
fMinOrientedRectSide = i->value.toNumber();
// std::cout << "minOrientedRectSide: " << fMinOrientedRectSide << std::endl;
}
else if ("distanceToEllipseContour" == std::string(i->key)) {
else if ("distanceToEllipseContour" == std::string(i->key))
{
fDistanceToEllipseContour = i->value.toNumber();
// std::cout << "distanceToEllipseContour: " << fDistanceToEllipseContour << std::endl;
}
else if ("minScore" == std::string(i->key)) {
else if ("minScore" == std::string(i->key))
{
fMinScore = i->value.toNumber();
// std::cout << "minScore: " << fMinScore << std::endl;
}
else if ("minReliability" == std::string(i->key)) {
else if ("minReliability" == std::string(i->key))
{
fMinReliability = i->value.toNumber();
// std::cout << "minReliability: " << fMinReliability << std::endl;
}
else if ("ns" == std::string(i->key)) {
else if ("ns" == std::string(i->key))
{
iNs = i->value.toNumber();
// std::cout << "ns: " << iNs << std::endl;
}
else if ("percentNe" == std::string(i->key)) {
else if ("percentNe" == std::string(i->key))
{
dPercentNe = i->value.toNumber();
// std::cout << "percentNe: " << dPercentNe << std::endl;
}
else if ("T_CNC" == std::string(i->key)) {
else if ("T_CNC" == std::string(i->key))
{
fT_CNC = i->value.toNumber();
// std::cout << "T_CNC: " << fT_CNC << std::endl;
}
else if ("T_TCN_L" == std::string(i->key)) {
else if ("T_TCN_L" == std::string(i->key))
{
fT_TCN_L = i->value.toNumber();
// std::cout << "T_TCN_L: " << fT_TCN_L << std::endl;
}
else if ("T_TCN_P" == std::string(i->key)) {
else if ("T_TCN_P" == std::string(i->key))
{
fT_TCN_P = i->value.toNumber();
// std::cout << "T_TCN_P: " << fT_TCN_P << std::endl;
}
else if ("thRadius" == std::string(i->key)) {
else if ("thRadius" == std::string(i->key))
{
fThre_R = i->value.toNumber();
// std::cout << "thRadius: " << fThre_R << std::endl;
}
else if ("radiusInMeter" == std::string(i->key)) {
else if ("radiusInMeter" == std::string(i->key))
{
this->_radius_in_meter = i->value.toNumber();
// std::cout << "radiusInMeter: " << this->_radius_in_meter << std::endl;
}
}
if (_ed) { delete _ed; _ed = NULL; }
if (_ed)
{
delete _ed;
_ed = NULL;
}
_ed = new yaed::EllipseDetector;
_ed->SetParameters(szPreProcessingGaussKernel, dPreProcessingGaussSigma, fThPosition, fMaxCenterDistance, iMinEdgeLength, fMinOrientedRectSide, fDistanceToEllipseContour, fMinScore, fMinReliability, iNs, dPercentNe, fT_CNC, fT_TCN_L, fT_TCN_P, fThre_R);
}
SingleObjectTrackerBase::SingleObjectTrackerBase()
{
this->_params_loaded = false;
}
SingleObjectTrackerBase::~SingleObjectTrackerBase()
{
}
bool SingleObjectTrackerBase::isParamsLoaded()
{
@ -751,6 +814,18 @@ int SingleObjectTrackerBase::getTarget()
{
return this->_target;
}
double SingleObjectTrackerBase::getObjectWs()
{
return this->_object_ws;
}
double SingleObjectTrackerBase::getObjectHs()
{
return this->_object_hs;
}
int SingleObjectTrackerBase::useWidthOrHeight()
{
return this->_use_width_or_height;
}
void SingleObjectTrackerBase::warmUp()
{
@ -796,6 +871,24 @@ void SingleObjectTrackerBase::track(cv::Mat img_, TargetsInFrame& tgts_)
tgt.setBox(rect.x, rect.y, rect.x + rect.width, rect.y + rect.height, img_.cols, img_.rows);
tgt.setTrackID(1);
tgt.setLOS(tgt.cx, tgt.cy, this->camera_matrix, img_.cols, img_.rows);
int ow = int(round(rect.width));
int oh = int(round(rect.height));
if (this->_use_width_or_height == 0)
{
double z = this->camera_matrix.at<double>(0, 0) * this->_object_ws / ow;
double x = tan(tgt.los_ax / SV_RAD2DEG) * z;
double y = tan(tgt.los_ay / SV_RAD2DEG) * z;
tgt.setPosition(x, y, z);
}
else if (this->_use_width_or_height == 1)
{
double z = this->camera_matrix.at<double>(1, 1) * this->_object_hs / oh;
double x = tan(tgt.los_ax / SV_RAD2DEG) * z;
double y = tan(tgt.los_ay / SV_RAD2DEG) * z;
tgt.setPosition(x, y, z);
}
tgts_.targets.push_back(tgt);
}
@ -807,7 +900,6 @@ bool SingleObjectTrackerBase::setupImpl()
}
void SingleObjectTrackerBase::initImpl(cv::Mat img_, const cv::Rect &bounding_box_)
{
}
bool SingleObjectTrackerBase::trackImpl(cv::Mat img_, cv::Rect &output_bbox_)
{
@ -822,23 +914,38 @@ void SingleObjectTrackerBase::_load()
JsonValue tracker_params_value;
_parser_algorithm_params("SingleObjectTracker", all_value, tracker_params_value);
for (auto i : tracker_params_value) {
if ("algorithm" == std::string(i->key)) {
for (auto i : tracker_params_value)
{
if ("algorithm" == std::string(i->key))
{
this->_algorithm = i->value.toString();
std::cout << "algorithm: " << this->_algorithm << std::endl;
}
else if ("backend" == std::string(i->key)) {
else if ("backend" == std::string(i->key))
{
this->_backend = i->value.toNumber();
}
else if ("target" == std::string(i->key)) {
else if ("target" == std::string(i->key))
{
this->_target = i->value.toNumber();
}
else if ("useWidthOrHeight" == std::string(i->key))
{
this->_use_width_or_height = i->value.toNumber();
}
else if ("sigleobjectW" == std::string(i->key))
{
this->_object_ws = i->value.toNumber();
}
else if ("sigleobjectH" == std::string(i->key))
{
this->_object_hs = i->value.toNumber();
}
}
setupImpl();
}
CommonObjectDetectorBase::CommonObjectDetectorBase() // : CameraAlgorithm()
{
this->_params_loaded = false;
@ -846,7 +953,6 @@ CommonObjectDetectorBase::CommonObjectDetectorBase() // : CameraAlgorithm()
}
CommonObjectDetectorBase::~CommonObjectDetectorBase()
{
}
bool CommonObjectDetectorBase::isParamsLoaded()
@ -958,10 +1064,14 @@ void CommonObjectDetectorBase::detect(cv::Mat img_, TargetsInFrame& tgts_, Box*
int oy = int(round(boxes_y[j]));
int ow = int(round(boxes_w[j]));
int oh = int(round(boxes_h[j]));
if (ox < 0) ox = 0;
if (oy < 0) oy = 0;
if (ox + ow >= img_.cols) ow = img_.cols - ox - 1;
if (oy + oh >= img_.rows) oh = img_.rows - oy - 1;
if (ox < 0)
ox = 0;
if (oy < 0)
oy = 0;
if (ox + ow >= img_.cols)
ow = img_.cols - ox - 1;
if (oy + oh >= img_.rows)
oh = img_.rows - oy - 1;
if (ow > 5 && oh > 5)
{
Target tgt;
@ -1046,10 +1156,8 @@ void CommonObjectDetectorBase::detectImpl(
std::vector<float> &boxes_h_,
std::vector<int> &boxes_label_,
std::vector<float> &boxes_score_,
std::vector<cv::Mat>& boxes_seg_
)
std::vector<cv::Mat> &boxes_seg_)
{
}
void CommonObjectDetectorBase::_load()
@ -1070,13 +1178,16 @@ void CommonObjectDetectorBase::_load()
this->_thrs_conf = 0.4;
this->_use_width_or_height = 0;
for (auto i : detector_params_value) {
for (auto i : detector_params_value)
{
if ("dataset" == std::string(i->key)) {
if ("dataset" == std::string(i->key))
{
this->_dataset = i->value.toString();
std::cout << "dataset: " << this->_dataset << std::endl;
}
else if ("inputSize" == std::string(i->key)) {
else if ("inputSize" == std::string(i->key))
{
// std::cout << "inputSize (old, new): " << this->_input_w << ", " << i->value.toNumber() << std::endl;
this->_input_w = i->value.toNumber();
if (this->_input_w != 640 && this->_input_w != 1280)
@ -1085,35 +1196,46 @@ void CommonObjectDetectorBase::_load()
}
this->_input_h = this->_input_w;
}
else if ("nmsThrs" == std::string(i->key)) {
else if ("nmsThrs" == std::string(i->key))
{
// std::cout << "nmsThrs (old, new): " << this->_thrs_nms << ", " << i->value.toNumber() << std::endl;
this->_thrs_nms = i->value.toNumber();
}
else if ("scoreThrs" == std::string(i->key)) {
else if ("scoreThrs" == std::string(i->key))
{
// std::cout << "scoreThrs (old, new): " << this->_thrs_conf << ", " << i->value.toNumber() << std::endl;
this->_thrs_conf = i->value.toNumber();
}
else if ("useWidthOrHeight" == std::string(i->key)) {
else if ("useWidthOrHeight" == std::string(i->key))
{
// std::cout << "useWidthOrHeight (old, new): " << this->_use_width_or_height << ", " << i->value.toNumber() << std::endl;
this->_use_width_or_height = i->value.toNumber();
}
else if ("withSegmentation" == std::string(i->key)) {
else if ("withSegmentation" == std::string(i->key))
{
bool json_tf = false;
if (i->value.getTag() == JSON_TRUE) json_tf = true;
if (i->value.getTag() == JSON_TRUE)
json_tf = true;
this->_with_segmentation = json_tf;
}
else if ("dataset" + this->_dataset == std::string(i->key)) {
if (i->value.getTag() == JSON_OBJECT) {
for (auto j : i->value) {
else if ("dataset" + this->_dataset == std::string(i->key))
{
if (i->value.getTag() == JSON_OBJECT)
{
for (auto j : i->value)
{
// std::cout << j->key << std::endl;
_class_names.push_back(std::string(j->key));
if (j->value.getTag() == JSON_ARRAY)
{
int k_cnt = 0;
for (auto k : j->value) {
for (auto k : j->value)
{
// std::cout << k->value.toNumber() << std::endl;
if (k_cnt == 0) _class_ws.push_back(k->value.toNumber());
else if (k_cnt == 1) _class_hs.push_back(k->value.toNumber());
if (k_cnt == 0)
_class_ws.push_back(k->value.toNumber());
else if (k_cnt == 1)
_class_hs.push_back(k->value.toNumber());
k_cnt++;
}
}
@ -1123,20 +1245,6 @@ void CommonObjectDetectorBase::_load()
}
setupImpl();
}
}

View File

@ -104,6 +104,10 @@ public:
std::string getAlgorithm();
int getBackend();
int getTarget();
double getObjectWs();
double getObjectHs();
int useWidthOrHeight();
protected:
virtual bool setupImpl();
virtual void initImpl(cv::Mat img_, const cv::Rect& bounding_box_);
@ -113,6 +117,9 @@ protected:
std::string _algorithm;
int _backend;
int _target;
int _use_width_or_height;
double _object_ws;
double _object_hs;
};

View File

@ -92,6 +92,8 @@ int main(int argc, char *argv[]) {
int(tgts.targets[0].h * tgts.height));
// 打印 跟踪目标 的视线角,跟相机视场相关
printf(" Tracking Line-of-sight (ax, ay) = (%.3f, %.3f)\n", tgts.targets[0].los_ax, tgts.targets[0].los_ay);
// 打印每个二维码的3D位置在相机坐标系下跟二维码实际边长、相机参数相关
printf(" Tracking Position = (x, y, z) = (%.3f, %.3f, %.3f)\n", tgts.targets[0].px, tgts.targets[0].py, tgts.targets[0].pz);
}
}

187
sv_algorithm_params.json Normal file
View File

@ -0,0 +1,187 @@
{
"CommonObjectDetector": {
"dataset": "COCO",
"inputSize": 1280,
"nmsThrs": 0.6,
"scoreThrs": 0.3,
"useWidthOrHeight": 1,
"withSegmentation": false,
"datasetPersonVehicle": {
"person": [0.5, 1.8],
"car": [4.1, 1.5],
"bus": [10, 3],
"truck": [-1, -1],
"bike": [-1, -1],
"train": [-1, -1],
"boat": [-1, -1],
"aeroplane": [-1, -1]
},
"datasetDrone": {
"drone": [0.4, 0.2]
},
"datasetCOCO": {
"person": [-1, -1],
"bicycle": [-1, -1],
"car": [-1, -1],
"motorcycle": [-1, -1],
"airplane": [-1, -1],
"bus": [-1, -1],
"train": [-1, -1],
"truck": [-1, -1],
"boat": [-1, -1],
"traffic light": [-1, -1],
"fire hydrant": [-1, -1],
"stop sign": [-1, -1],
"parking meter": [-1, -1],
"bench": [-1, -1],
"bird": [-1, -1],
"cat": [-1, -1],
"dog": [-1, -1],
"horse": [-1, -1],
"sheep": [-1, -1],
"cow": [-1, -1],
"elephant": [-1, -1],
"bear": [-1, -1],
"zebra": [-1, -1],
"giraffe": [-1, -1],
"backpack": [-1, -1],
"umbrella": [-1, -1],
"handbag": [-1, -1],
"tie": [-1, -1],
"suitcase": [-1, -1],
"frisbee": [-1, -1],
"skis": [-1, -1],
"snowboard": [-1, -1],
"sports ball": [-1, -1],
"kite": [-1, -1],
"baseball bat": [-1, -1],
"baseball glove": [-1, -1],
"skateboard": [-1, -1],
"surfboard": [-1, -1],
"tennis racket": [-1, -1],
"bottle": [-1, -1],
"wine glass": [-1, -1],
"cup": [-1, -1],
"fork": [-1, -1],
"knife": [-1, -1],
"spoon": [-1, -1],
"bowl": [-1, -1],
"banana": [-1, -1],
"apple": [-1, -1],
"sandwich": [-1, -1],
"orange": [-1, -1],
"broccoli": [-1, -1],
"carrot": [-1, -1],
"hot dog": [-1, -1],
"pizza": [-1, -1],
"donut": [-1, -1],
"cake": [-1, -1],
"chair": [-1, -1],
"couch": [-1, -1],
"potted plant": [-1, -1],
"bed": [-1, -1],
"dining table": [-1, -1],
"toilet": [-1, -1],
"tv": [-1, -1],
"laptop": [-1, -1],
"mouse": [-1, -1],
"remote": [-1, -1],
"keyboard": [-1, -1],
"cell phone": [-1, -1],
"microwave": [-1, -1],
"oven": [-1, -1],
"toaster": [-1, -1],
"sink": [-1, -1],
"refrigerator": [-1, -1],
"book": [-1, -1],
"clock": [-1, -1],
"vase": [-1, -1],
"scissors": [-1, -1],
"teddy bear": [-1, -1],
"hair drier": [-1, -1],
"toothbrush": [-1, -1]
}
},
"AutoFocusObjectDetector": {
"lock_thres": 5,
"unlock_thres": 5,
"lock_scale_init": 12.0,
"lock_scale": 8.0,
"categories_filter": [],
"keep_unlocked": false,
"use_square_region": false
},
"SingleObjectTracker": {
"algorithm": "nano",
"backend": 0,
"target": 0,
"useWidthOrHeight": 0,
"sigleobjectW":2,
"sigleobjectH":-1
},
"LandingMarkerDetector": {
"labels": ["h","x","1","2","8"],
"maxCandidates": 3
},
"EllipseDetector": {
"radiusInMeter": 0.1535,
"preProcessingGaussKernel": 5,
"preProcessingGaussSigma": 1.306,
"thPosition": 1.0,
"maxCenterDistance": 0.05,
"minEdgeLength": 9,
"minOrientedRectSide": 2.984,
"distanceToEllipseContour": 0.111,
"minScore": 0.7,
"minReliability": 0.470,
"ns": 22,
"percentNe": 0.99,
"T_CNC": 0.121,
"T_TCN_L": 0.468,
"T_TCN_P": 0.560,
"thRadius": 0.202
},
"ArucoDetector": {
"dictionaryId": 10,
"markerIds": [-1],
"markerLengths": [0.2],
"adaptiveThreshConstant": 35,
"adaptiveThreshWinSizeMax": 23,
"adaptiveThreshWinSizeMin": 3,
"adaptiveThreshWinSizeStep": 10,
"aprilTagCriticalRad": 0.17453292519,
"aprilTagDeglitch": 0,
"aprilTagMaxLineFitMse": 10.0,
"aprilTagMaxNmaxima": 10,
"aprilTagMinClusterPixels": 5,
"aprilTagMinWhiteBlackDiff": 5,
"aprilTagQuadDecimate": 0.0,
"aprilTagQuadSigma": 0.0,
"cornerRefinementMaxIterations": 30,
"cornerRefinementMethod": 0,
"cornerRefinementMinAccuracy": 0.1,
"cornerRefinementWinSize": 5,
"detectInvertedMarker": false,
"errorCorrectionRate": 0.6,
"markerBorderBits": 1,
"maxErroneousBitsInBorderRate": 0.35,
"maxMarkerPerimeterRate": 4.0,
"minCornerDistanceRate": 0.05,
"minDistanceToBorder": 3,
"minMarkerDistanceRate": 0.05,
"minMarkerLengthRatioOriginalImg": 0,
"minMarkerPerimeterRate": 0.03,
"minOtsuStdDev": 5.0,
"minSideLengthCanonicalImg": 32,
"perspectiveRemoveIgnoredMarginPerCell": 0.13,
"perspectiveRemovePixelPerCell": 4,
"polygonalApproxAccuracyRate": 0.03,
"useAruco3Detection": false
},
"ColorLineDetector": {
"line_color": "black",
"line_location": 0.5,
"line_location_a1": 0.3,
"line_location_a2": 0.7
}
}