From feb11ce449b0233b7fbebeaa475c43d8b75da649 Mon Sep 17 00:00:00 2001 From: Markus Konrad Date: Fri, 4 Jul 2014 13:37:26 +0200 Subject: [PATCH] trying out rotation fixing with least distance to origin --- ident.cpp | 9 +------ ident.h | 5 ---- ident_7x7.cpp | 4 +-- ident_templ.cpp | 1 - marker.cpp | 70 +++++++++++++++++++++++++++++++++---------------- marker.h | 22 ++++++++-------- tools.cpp | 4 +++ tools.h | 2 ++ track.cpp | 3 ++- 9 files changed, 69 insertions(+), 51 deletions(-) diff --git a/ident.cpp b/ident.cpp index f5ed930..d58ab52 100644 --- a/ident.cpp +++ b/ident.cpp @@ -9,11 +9,4 @@ * See LICENSE for license. */ -#include "ident.h" - -using namespace ocv_ar; - -void IdentificatorBase::setFoundPropertiesForMarker(Marker &marker, int id, int rot) { - marker.setId(id); - marker.rotatePoints(rot); -} \ No newline at end of file +#include "ident.h" \ No newline at end of file diff --git a/ident.h b/ident.h index c23e8ed..36c1578 100644 --- a/ident.h +++ b/ident.h @@ -58,11 +58,6 @@ class IdentificatorBase { IdentificatorType getType() { return type; } protected: - /** - * Helper function to set an and a valid rotation to a . - */ - void setFoundPropertiesForMarker(Marker &marker, int id, int rot); - IdentificatorType type; // identificator type int reqMarkerSize; // required marker size diff --git a/ident_7x7.cpp b/ident_7x7.cpp index a83bd57..d2039c9 100644 --- a/ident_7x7.cpp +++ b/ident_7x7.cpp @@ -59,8 +59,8 @@ bool Identificator7x7::readMarkerCode(const cv::Mat &area, Marker &marker) { if (checkMarkerCode(bitMatrix)) { // found a valid marker code! // set the id and rotate the corner points int id = markerCodeToId(bitMatrix); - setFoundPropertiesForMarker(marker, id, rot); - + marker.setId(id); + return true; } diff --git a/ident_templ.cpp b/ident_templ.cpp index 7109be2..835d802 100644 --- a/ident_templ.cpp +++ b/ident_templ.cpp @@ -70,7 +70,6 @@ bool IdentificatorTemplMatch::readMarkerCode(const cv::Mat &area, Marker &marker { int validRot; if (checkTemplateRotations(areaContent, it->second, &validRot)) { // found a matching template! - setFoundPropertiesForMarker(marker, it->first, validRot); marker.setId(it->first); return true; } diff --git a/marker.cpp b/marker.cpp index 2a5b8ac..a127cc5 100644 --- a/marker.cpp +++ b/marker.cpp @@ -13,6 +13,8 @@ * See LICENSE for license. */ +#include + #include "marker.h" #include "conf.h" @@ -70,10 +72,6 @@ void Marker::updateDetectionTime() { detectMs = Tools::nowMs(); } -void Marker::rotatePoints(int rot) { - rotate(points.begin(), points.begin() + 4 - rot, points.end()); -} - void Marker::updatePoseMat(const cv::Mat &r, const cv::Mat &t, bool useSmoothing) { if (!r.data || !t.data) return; @@ -124,23 +122,23 @@ void Marker::updatePoseMat(const cv::Mat &r, const cv::Mat &t, bool useSmoothing // memcpy(prevRotQuat, curRotQuat, sizeof(float) * 4); // } - float rotVecsAng = Tools::vec3Angle(prevRVec, rVecEu); - - if (rotVecsAng > 0.1f) { - printf("ocv_ar::Marker %d - rvec1: %f, %f, %f\n", id, prevRVec[0], prevRVec[1], prevRVec[2]); - printVec3TrigVals(prevRVec); - printf("ocv_ar::Marker %d - angle: %f\n", id, rotVecsAng); - printf("ocv_ar::Marker %d - rvec2: %f, %f, %f\n", id, rVecEu[0], rVecEu[1], rVecEu[2]); - printVec3TrigVals(rVecEu); - printf("---\n"); - -// rVecEu[0] += M_PI; -//// rVecEu[1] *= -1.0f; -// rVecEu[2] = M_PI - rVecEu[2]; - rVecEu[0] = prevRVec[0]; - rVecEu[1] = prevRVec[1]; - rVecEu[2] = prevRVec[2]; - } +// float rotVecsAng = Tools::vec3Angle(prevRVec, rVecEu); +// +// if (rotVecsAng > 0.1f) { +// printf("ocv_ar::Marker %d - rvec1: %f, %f, %f\n", id, prevRVec[0], prevRVec[1], prevRVec[2]); +// printVec3TrigVals(prevRVec); +// printf("ocv_ar::Marker %d - angle: %f\n", id, rotVecsAng); +// printf("ocv_ar::Marker %d - rvec2: %f, %f, %f\n", id, rVecEu[0], rVecEu[1], rVecEu[2]); +// printVec3TrigVals(rVecEu); +// printf("---\n"); +// +//// rVecEu[0] += M_PI; +////// rVecEu[1] *= -1.0f; +//// rVecEu[2] = M_PI - rVecEu[2]; +// rVecEu[0] = prevRVec[0]; +// rVecEu[1] = prevRVec[1]; +// rVecEu[2] = prevRVec[2]; +// } if (useSmoothing) { pushVecsToHistory(rVecEu, tVecPtr); @@ -199,6 +197,12 @@ void Marker::updatePoseMat(const cv::Mat &r, const cv::Mat &t, bool useSmoothing /* END modified code from ArUco lib */ } +#pragma mark private methods + +void Marker::rotatePoints(int rot) { + rotate(points.begin(), points.begin() + 4 - rot, points.end()); +} + void Marker::sortPoints() { // Sort the points in anti-clockwise order cv::Point v1 = points[1] - points[0]; @@ -209,6 +213,28 @@ void Marker::sortPoints() { if ((v1.x * v2.y) - (v1.y * v2.x) < 0.0) { swap(points[1], points[3]); } + + float minDist = std::numeric_limits::max(); + int rotBy = 0; + int ptIdx = 0; + for (Point2fVec::const_iterator it = points.begin(); + it != points.end(); + ++it) + { + float dist = Tools::lengthSquared(*it); + if (dist < minDist) { + minDist = dist; + rotBy = ptIdx; + } + + ptIdx++; + } + + printf("ocv_ar::Marker %d - rotating points by %d\n", id, rotBy); + + if (rotBy > 0) { + rotatePoints(rotBy); + } } void Marker::calcShapeProperties() { @@ -229,8 +255,6 @@ void Marker::calcShapeProperties() { perimeterRad = maxDist; } -#pragma mark private methods - void Marker::init() { // set defaults id = -1; diff --git a/marker.h b/marker.h index c145431..f65b2bc 100644 --- a/marker.h +++ b/marker.h @@ -97,11 +97,6 @@ class Marker { */ void clearPoints() { points.clear(); } - /** - * Rotate the corner points in the vector times. - */ - void rotatePoints(int rot); - /** * Return the controid calculated from the corner points. */ @@ -135,22 +130,27 @@ class Marker { */ const float *getPoseMatPtr() const { return poseMat; }; +private: + /** + * Common initialize method for the marker class. + */ + void init(); + /** * Sort the points in anti-clockwise order. */ void sortPoints(); /** - * Calculate the shape properties from the corner points like centroid - * and perimeter radius. + * Rotate the corner points in the vector times. */ - void calcShapeProperties(); + void rotatePoints(int rot); -private: /** - * Common initialize method for the marker class. + * Calculate the shape properties from the corner points like centroid + * and perimeter radius. */ - void init(); + void calcShapeProperties(); /** * Update vector history arrays for smoothing ( and ) with diff --git a/tools.cpp b/tools.cpp index da54b6c..9945743 100644 --- a/tools.cpp +++ b/tools.cpp @@ -30,6 +30,10 @@ float Tools::distSquared(cv::Point2f p1, cv::Point2f p2) { return dX * dX + dY * dY; } +float Tools::lengthSquared(cv::Point2f p) { + return p.x * p.x + p.y * p.y; +} + void Tools::matRot90CW(cv::Mat &m) { cv::transpose(m, m); cv::flip(m, m, 1); diff --git a/tools.h b/tools.h index 2a77b49..52824a8 100644 --- a/tools.h +++ b/tools.h @@ -34,6 +34,8 @@ class Tools { */ static float distSquared(cv::Point2f p1, cv::Point2f p2); + static float lengthSquared(cv::Point2f p); + /** * Fast clock-wise rotation of by 90° */ diff --git a/track.cpp b/track.cpp index 58e53ca..cdcb3b0 100644 --- a/track.cpp +++ b/track.cpp @@ -73,7 +73,8 @@ void Track::update() { } } - // check if this marker was detected this time and if not, if it already time out + // check if this marker was detected this time + // and if not, if it already time out if (!markerUpdated && now - existingMrk->getDetectionTimeMs() > OCV_AR_CONF_TRACKER_MARKER_TIMEOUT_MS) { // if so, remove it! printf("ocv_ar::Track - lost marker %d\n", existingMrkId);