trying out rotation fixing with least distance to origin

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);
-}
+#include "ident.h"

ident.h

@@ -58,11 +58,6 @@ class IdentificatorBase {
     IdentificatorType getType() { return type; }
 
 protected:
-    /**
-     * Helper function to set an <id> and a valid rotation <rot> to a <marker>.
-     */
-    void setFoundPropertiesForMarker(Marker &marker, int id, int rot);
-
 
     IdentificatorType type; // identificator type
     int reqMarkerSize;      // required marker size

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;
     	}
 

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;
         }

marker.cpp

@@ -13,6 +13,8 @@
  * See LICENSE for license.
  */
 
+#include <limits>
+
 #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<float>::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;

marker.h

@@ -97,11 +97,6 @@ class Marker {
      */
     void clearPoints() { points.clear(); }
 
-    /**
-     * Rotate the corner points in the vector <rot> 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 <rot> 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 (<tVecHist> and <rVecHist>) with

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);

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 <m> by 90°
      */

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);