diff --git a/detect.cpp b/detect.cpp index 4848b3d..5460fed 100644 --- a/detect.cpp +++ b/detect.cpp @@ -230,7 +230,28 @@ void Detect::processFrame() { findContours(); findMarkerCandidates(); identifyMarkers(); - estimatePositions(); +} + +void Detect::estimateMarkersPoses() { + // estimate the 3D pose of each found marker + for (vector::iterator it = foundMarkers.begin(); + it != foundMarkers.end(); + ++it) + { + Marker *marker = *it; + + // find marker pose from 3D-2D point correspondences between + // and 2D points in getPoints()> + cv::Mat rVec; // pose rotation vector + cv::Mat tVec; // pose translation vector + cv::solvePnP(normMarkerCoord3D, marker->getPoints(), + camMat, distCoeff, + rVec, tVec, + false); + + // generate an OpenGL model-view matrix from the rotation and translation vectors + marker->updatePoseMat(rVec, tVec); + } } cv::Mat *Detect::getOutputFrame() const { @@ -440,28 +461,6 @@ void Detect::identifyMarkers() { } } -void Detect::estimatePositions() { - // estimate the 3D pose of each found marker - for (vector::iterator it = foundMarkers.begin(); - it != foundMarkers.end(); - ++it) - { - Marker *marker = *it; - - // find marker pose from 3D-2D point correspondences between - // and 2D points in getPoints()> - cv::Mat rVec; // pose rotation vector - cv::Mat tVec; // pose translation vector - cv::solvePnP(normMarkerCoord3D, marker->getPoints(), - camMat, distCoeff, - rVec, tVec, - false); - - // generate an OpenGL model-view matrix from the rotation and translation vectors - marker->updatePoseMat(rVec, tVec); - } -} - void Detect::discardDuplicateMarkers(vector &markerList) { const float maxDuplDistSquared = OCV_AR_CONF_MAX_DUPLICATE_DIST * OCV_AR_CONF_MAX_DUPLICATE_DIST; diff --git a/detect.h b/detect.h index 92079ee..d055715 100644 --- a/detect.h +++ b/detect.h @@ -114,6 +114,11 @@ class Detect { */ void processFrame(); + /** + * After detection and identification of the markers, estimate their 3D pose. + */ + void estimateMarkersPoses(); + /** * Get the output frame according to the "frame output level" set via * . If this level is set to PROC_LEVEL_DEFAULT, it will @@ -167,11 +172,6 @@ class Detect { */ void identifyMarkers(); - /** - * Detection step 5 - estimating the 3D pose of each found valid marker. - */ - void estimatePositions(); - /** * Discard duplicate markers after detection step 4. */ diff --git a/marker.cpp b/marker.cpp index a127cc5..6dd84d6 100644 --- a/marker.cpp +++ b/marker.cpp @@ -22,18 +22,18 @@ using namespace ocv_ar; -void printVec3TrigVals(float v[3]) { - float x1 = cosf(v[0]); - float y1 = sinf(v[0]); - float x2 = cosf(v[1]); - float y2 = sinf(v[1]); - float x3 = cosf(v[2]); - float y3 = sinf(v[2]); - - printf("> %f, %f\n", x1, y1); - printf("> %f, %f\n", x2, y2); - printf("> %f, %f\n", x3, y3); -} +//void printVec3TrigVals(float v[3]) { +// float x1 = cosf(v[0]); +// float y1 = sinf(v[0]); +// float x2 = cosf(v[1]); +// float y2 = sinf(v[1]); +// float x3 = cosf(v[2]); +// float y3 = sinf(v[2]); +// +// printf("> %f, %f\n", x1, y1); +// printf("> %f, %f\n", x2, y2); +// printf("> %f, %f\n", x3, y3); +//} #pragma mark public methods @@ -68,137 +68,107 @@ Marker::~Marker() { if (tVecHist) delete [] tVecHist; } +void Marker::mapPoints(const ocv_ar::Marker &otherMrk) { + Point2fVec otherPts = otherMrk.getPoints(); + int rotBy = 0; + + // find the rotation the rotation that yields minimum average distance + // between the vertex points of this marker and + float minAvgDist = numeric_limits::max(); + for (int rot = 0; rot < 4; ++rot) { // for each possible rotation + float avgDist = 0.0f; + for (int p = 0; p < 4; ++p) { // for each vertex point + // calculate squared distance to the (rotated) other vertex + avgDist += Tools::distSquared(points[p], otherPts[(p + rot) % 4]); + } + + avgDist /= 4; + + if (avgDist < minAvgDist) { + minAvgDist = avgDist; + rotBy = rot; + } + } + + // rotate our points to match the vertex order of + printf("ocv_ar::Marker %d - rotating vertices by %d with min. avg. dist. %f\n", id, rotBy, minAvgDist); + rotatePoints(rotBy); +} + void Marker::updateDetectionTime() { detectMs = Tools::nowMs(); } -void Marker::updatePoseMat(const cv::Mat &r, const cv::Mat &t, bool useSmoothing) { +void Marker::updateForTracking(const Marker &other) { + setPoints(other.getPoints()); + + const cv::Mat r = other.getRVec(); + const cv::Mat t = other.getTVec(); + if (!r.data || !t.data) return; // r and t are double vectors from solvePnP - // convert them to floats! + // convert them to floats and save them as member + // variables and r.convertTo(rVec, CV_32F); t.convertTo(tVec, CV_32F); - float *rVecPtr = rVec.ptr(0); float *tVecPtr = tVec.ptr(0); float rVecEu[3]; Tools::rotVecToEuler(rVecPtr, rVecEu); - float tempRVec[] = { rVecEu[0], rVecEu[1], rVecEu[2] }; -// printf("ocv_ar::Marker %d - rvec1: %f, %f, %f\n", id, rVecPtr[0], rVecPtr[1], rVecPtr[2]); - -// float rEuler[3]; -// Tools::rotVecToEuler(rVecPtr, rEuler); -// printf("ocv_ar::Marker %d - euler: %f, %f, %f\n", id, rEuler[0], rEuler[1], rEuler[2]); -// Tools::eulerToRotVec(rEuler, rVecPtr); -// printf("ocv_ar::Marker %d - rvec2: %f, %f, %f\n", id, rVecPtr[0], rVecPtr[1], rVecPtr[2]); - -// float curRotQuat[4]; -// Tools::rotVecToQuat(rVecPtr, curRotQuat); - -// if (prevRotQuat[3] != 0.0f) { -// float intermRotQuat[4]; -// -// float quatDot = Tools::quatDot(prevRotQuat, curRotQuat); -// if (quatDot < 0.0f) { -// curRotQuat[0] *= -1.0f; -// curRotQuat[1] *= -1.0f; -// curRotQuat[2] *= -1.0f; -// curRotQuat[3] *= -1.0f; -// } -// quatDot = Tools::quatDot(prevRotQuat, curRotQuat); -// -// Tools::slerp(prevRotQuat, curRotQuat, 0.5f, intermRotQuat); -// Tools::quatToRotVec(intermRotQuat, rVecPtr); -// printf("ocv_ar::Marker %d - quat1: %f, %f, %f, %f\n", id, curRotQuat[0], curRotQuat[1], curRotQuat[2], curRotQuat[3]); -//// printf("ocv_ar::Marker %d - quat2: %f, %f, %f, %f\n", id, intermRotQuat[0], intermRotQuat[1], intermRotQuat[2], intermRotQuat[3]); -// printf("ocv_ar::Marker %d - dot: %f\n", id, quatDot); -// -// memcpy(prevRotQuat, intermRotQuat, sizeof(float) * 4); -// printf("quat dot: %f\n", Tools::quatDot(curRotQuat, prevRotQuat)); -// } else { -// 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]; -// } - - if (useSmoothing) { - pushVecsToHistory(rVecEu, tVecPtr); - - if (pushedHistVecs >= OCV_AR_CONF_SMOOTHING_HIST_SIZE) { - calcSmoothPoseVecs(rVecEu, tVecPtr); - } + pushVecsToHistory(rVecEu, tVecPtr); - Tools::eulerToRotVec(rVecEu, rVecPtr); + if (pushedHistVecs >= OCV_AR_CONF_SMOOTHING_HIST_SIZE) { + calcSmoothPoseVecs(rVecEu, tVecPtr); } + + Tools::eulerToRotVec(rVecEu, rVecPtr); - prevRVec[0] = tempRVec[0]; - prevRVec[1] = tempRVec[1]; - prevRVec[2] = tempRVec[2]; + // re-calculate the pose matrix from and + calcPoseMat(); +} + +void Marker::updatePoseMat(const cv::Mat &r, const cv::Mat &t) { + if (!r.data || !t.data) return; -// printf("ocv_ar::Marker %d - rvec2: %f, %f, %f\n", id, rVecPtr[0], rVecPtr[1], rVecPtr[2]); + // r and t are double vectors from solvePnP + // convert them to floats and save them as member + // variables and + r.convertTo(rVec, CV_32F); + t.convertTo(tVec, CV_32F); -// Tools::composeModelViewMatrix(tVecPtr, rVecPtr, poseMat); + // re-calculate the pose matrix from and + calcPoseMat(); +} -//// cv::Mat cvPoseMat(4, 4, CV_32FC1, poseMat); -//// cvPoseMat = cvPoseMat.inv(); -//// memcpy(poseMat, cvPoseMat.data, 16 * sizeof(float)); +#pragma mark private methods +void Marker::init() { + // set defaults + id = -1; + pushedHistVecs = 0; - // create rotation matrix - cv::Mat rotMat(3, 3, CV_32FC1); - cv::Rodrigues(rVec, rotMat); - - /* BEGIN modified code from ArUco lib */ - float para[3][4]; - for (int i=0; i < 3; i++) { - float *rotMatRow = rotMat.ptr(i); - for (int j = 0; j < 3; j++) { - para[i][j] = rotMatRow[j]; - } - } - //now, add the translation - float *tVecData = tVec.ptr(0); - para[0][3] = tVecData[0]; - para[1][3] = tVecData[1]; - para[2][3] = tVecData[2]; + rVec.zeros(3, 1, CV_32F); + tVec.zeros(3, 1, CV_32F); - // create and init modelview_matrix - memset(poseMat, 0, 16 * sizeof(float)); // init with zeros + // memset(prevRotQuat, 0, sizeof(float) * 4); - for (int i = 0; i < 3; i++) { - float sign = (i != 2) ? 1.0f : -1.0f; - for (int j = 0; j < 4; j++) { - poseMat[i + j * 4] = sign * para[i][j]; - } - } + // create vectory history arrays + tVecHist = new float[OCV_AR_CONF_SMOOTHING_HIST_SIZE * 3]; + rVecHist = new float[OCV_AR_CONF_SMOOTHING_HIST_SIZE * 3]; - poseMat[15] = 1.0f; + // initialize them with zeros + memset(tVecHist, 0, sizeof(float) * OCV_AR_CONF_SMOOTHING_HIST_SIZE * 3); + memset(rVecHist, 0, sizeof(float) * OCV_AR_CONF_SMOOTHING_HIST_SIZE * 3); -// Tools::printFloatMat(poseMat, 4, 4); - /* END modified code from ArUco lib */ + sortPoints(); + calcShapeProperties(); + updateDetectionTime(); // set to now } -#pragma mark private methods - void Marker::rotatePoints(int rot) { rotate(points.begin(), points.begin() + 4 - rot, points.end()); } @@ -213,28 +183,6 @@ 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() { @@ -255,29 +203,6 @@ void Marker::calcShapeProperties() { perimeterRad = maxDist; } -void Marker::init() { - // set defaults - id = -1; - pushedHistVecs = 0; - - rVec.zeros(3, 1, CV_32F); - tVec.zeros(3, 1, CV_32F); - -// memset(prevRotQuat, 0, sizeof(float) * 4); - - // create vectory history arrays - tVecHist = new float[OCV_AR_CONF_SMOOTHING_HIST_SIZE * 3]; - rVecHist = new float[OCV_AR_CONF_SMOOTHING_HIST_SIZE * 3]; - - // initialize them with zeros - memset(tVecHist, 0, sizeof(float) * OCV_AR_CONF_SMOOTHING_HIST_SIZE * 3); - memset(rVecHist, 0, sizeof(float) * OCV_AR_CONF_SMOOTHING_HIST_SIZE * 3); - - sortPoints(); - calcShapeProperties(); - updateDetectionTime(); // set to now -} - void Marker::pushVecsToHistory(const float *r, const float *t) { const int numHistElems = OCV_AR_CONF_SMOOTHING_HIST_SIZE * 3; @@ -382,4 +307,39 @@ void Marker::calcSmoothPoseVecs(float *r, float *t) { t[0] /= OCV_AR_CONF_SMOOTHING_HIST_SIZE; t[1] /= OCV_AR_CONF_SMOOTHING_HIST_SIZE; t[2] /= OCV_AR_CONF_SMOOTHING_HIST_SIZE; +} + +void Marker::calcPoseMat() { + // create rotation matrix + cv::Mat rotMat(3, 3, CV_32FC1); + cv::Rodrigues(rVec, rotMat); + + /* BEGIN modified code from ArUco lib */ + float para[3][4]; + for (int i=0; i < 3; i++) { + float *rotMatRow = rotMat.ptr(i); + for (int j = 0; j < 3; j++) { + para[i][j] = rotMatRow[j]; + } + } + //now, add the translation + float *tVecData = tVec.ptr(0); + para[0][3] = tVecData[0]; + para[1][3] = tVecData[1]; + para[2][3] = tVecData[2]; + + // create and init modelview_matrix + memset(poseMat, 0, 16 * sizeof(float)); // init with zeros + + for (int i = 0; i < 3; i++) { + float sign = (i != 2) ? 1.0f : -1.0f; + for (int j = 0; j < 4; j++) { + poseMat[i + j * 4] = sign * para[i][j]; + } + } + + poseMat[15] = 1.0f; + + // Tools::printFloatMat(poseMat, 4, 4); + /* END modified code from ArUco lib */ } \ No newline at end of file diff --git a/marker.h b/marker.h index f65b2bc..54cb844 100644 --- a/marker.h +++ b/marker.h @@ -82,6 +82,12 @@ class Marker { */ Point2fVec getPoints() const { return points; } + /** + * Rotate the vertices (the points vector) so that this marker's points + * match up with 's vertex points. + */ + void mapPoints(const Marker &otherMrk); + /** * Set the points in . */ @@ -120,9 +126,12 @@ class Marker { /** * Update the 3D pose by rotation vector and translation vector . */ - void updatePoseMat(const cv::Mat &r, const cv::Mat &t, bool useSmoothing = false); + void updatePoseMat(const cv::Mat &r, const cv::Mat &t); -// void updateFromOtherMarker(const Marker *otherMrk); + /** + * + */ + void updateForTracking(const Marker &other); /** * Return the 4x4 OpenGL 3D pose model-view matrix as pointer to @@ -165,6 +174,12 @@ class Marker { */ void calcSmoothPoseVecs(float *r, float *t); + /** + * Calculate the 3D pose matrix from translation and rotation vectors and + * . + */ + void calcPoseMat(); + int id; // marker ID @@ -184,7 +199,7 @@ class Marker { // float prevRotQuat[4]; // previous rotation quaternion // std::list rVecHist; // marker rotation history float *rVecHist; // marker rotation history with N * 3 elements (euler vectors) for smoothing effect - float prevRVec[3]; +// float prevRVec[3]; float poseMat[16]; // OpenGL 4x4 matrix with model-view-transformation }; diff --git a/track.cpp b/track.cpp index cdcb3b0..7090cd3 100644 --- a/track.cpp +++ b/track.cpp @@ -15,11 +15,22 @@ using namespace ocv_ar; +#pragma mark public methods + void Track::detect(const cv::Mat *frame) { assert(detector && frame); + + // set the input frame detector->setInputFrame(frame, frame->channels() == 1); + // detect and identify the markers detector->processFrame(); + + // correct the vertices of the found markers + correctMarkerVertexOrder(detector->getMarkers()); + + // estimate the markers' 3D poses + detector->estimateMarkersPoses(); } void Track::lockMarkers() { @@ -40,7 +51,7 @@ void Track::update() { double now = Tools::nowMs(); for (MarkerMap::iterator it = markers.begin(); it != markers.end(); - ) + ) // no op -- map::erase might be called below { int existingMrkId = it->first; Marker *existingMrk = &it->second; @@ -50,14 +61,14 @@ void Track::update() { // try to find a matching marker in the "new markers" vector for (vector::const_iterator newMrkIt = newMarkers.begin(); newMrkIt != newMarkers.end(); - ) + ) // no op -- vector::erase might be called below { const Marker *newMrk = *newMrkIt; if (existingMrkId == newMrk->getId()) { // we found a matching marker // printf("ocv_ar::Track - updating marker %d\n", existingMrkId); // update the existing marker with the information of the "new" marker - existingMrk->updatePoseMat(newMrk->getRVec(), newMrk->getTVec(), true); + existingMrk->updateForTracking(*newMrk); // update detection time to "now" existingMrk->updateDetectionTime(); @@ -96,4 +107,35 @@ void Track::update() { } unlockMarkers(); +} + +#pragma mark private methods + +void Track::correctMarkerVertexOrder(std::vector newMarkers) { + // map the vertex order of currently found markers to previously found + // markers. this prevents the vertex order from jumping around and + // therefore changing the rotation vector of the markers + lockMarkers(); // lock markers vector + for (MarkerMap::const_iterator it = markers.begin(); + it != markers.end(); + ++it) + { + int existingMrkId = it->first; + const Marker *existingMrk = &it->second; + + // try to find a matching marker in the "new markers" vector + for (vector::iterator newMrkIt = newMarkers.begin(); + newMrkIt != newMarkers.end(); + ++newMrkIt) + { + Marker *newMrk = *newMrkIt; + if (existingMrkId == newMrk->getId()) { // we found a matching marker + // update the new marker so that the order of vertices matches to + // the existing marker + newMrk->mapPoints(*existingMrk); + } + } + } + + unlockMarkers(); // unlock markers vector again } \ No newline at end of file diff --git a/track.h b/track.h index 4b2d669..6c1dc5b 100644 --- a/track.h +++ b/track.h @@ -12,6 +12,9 @@ #ifndef OCV_AR_TRACK_H #define OCV_AR_TRACK_H +#include +#include + #include #include "types.h" @@ -49,6 +52,9 @@ class Track { const MarkerMap *getMarkers() const { return &markers; } private: + void correctMarkerVertexOrder(std::vector newMarkers); + + MarkerMap markers; bool markersLocked;