diff --git a/marker.cpp b/marker.cpp index aeb1129..983bd84 100644 --- a/marker.cpp +++ b/marker.cpp @@ -73,30 +73,43 @@ void Marker::updatePoseMat(const cv::Mat &r, const cv::Mat &t, bool useSmoothing float *rVecPtr = rVec.ptr(0); float *tVecPtr = tVec.ptr(0); -// float curRotQuat[4]; -// Tools::rotVecToQuat(rVecPtr, curRotQuat); -// +// 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]; + 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]); + memcpy(prevRotQuat, intermRotQuat, sizeof(float) * 4); // printf("quat dot: %f\n", Tools::quatDot(curRotQuat, prevRotQuat)); +// } else { +// memcpy(prevRotQuat, curRotQuat, sizeof(float) * 4); // } - if (useSmoothing) { - pushVecsToHistory(rVecPtr, tVecPtr); - - if (pushedHistVecs >= OCV_AR_CONF_SMOOTHING_HIST_SIZE) { - calcSmoothPoseVecs(rVecPtr, tVecPtr); - } - } +// if (useSmoothing) { +// pushVecsToHistory(rVecPtr, tVecPtr); +// +// if (pushedHistVecs >= OCV_AR_CONF_SMOOTHING_HIST_SIZE) { +// calcSmoothPoseVecs(rVecPtr, tVecPtr); +// } +// } // Tools::composeModelViewMatrix(tVecPtr, rVecPtr, poseMat); //// cv::Mat cvPoseMat(4, 4, CV_32FC1, poseMat); //// cvPoseMat = cvPoseMat.inv(); //// memcpy(poseMat, cvPoseMat.data, 16 * sizeof(float)); - - printf("ocv_ar::Marker %d - rvec: %f, %f, %f\n", id, rVecPtr[0], rVecPtr[1], rVecPtr[2]); + // create rotation matrix cv::Mat rotMat(3, 3, CV_32FC1); @@ -130,8 +143,6 @@ void Marker::updatePoseMat(const cv::Mat &r, const cv::Mat &t, bool useSmoothing // Tools::printFloatMat(poseMat, 4, 4); /* END modified code from ArUco lib */ - -// memcpy(prevRotQuat, curRotQuat, sizeof(float) * 4); } void Marker::sortPoints() { @@ -174,7 +185,7 @@ void Marker::init() { rVec.zeros(3, 1, CV_32F); tVec.zeros(3, 1, CV_32F); -// memset(prevRotQuat, 0, sizeof(float) * 4); + memset(prevRotQuat, 0, sizeof(float) * 4); // create vectory history arrays tVecHist = new float[OCV_AR_CONF_SMOOTHING_HIST_SIZE * 3]; diff --git a/marker.h b/marker.h index eb9f296..ba975a8 100644 --- a/marker.h +++ b/marker.h @@ -181,7 +181,7 @@ class Marker { int pushedHistVecs; // number of vectors in and float *tVecHist; // marker position history with N * 3 elements for smoothing effect -// float prevRotQuat[4]; // previous rotation quaternion + float prevRotQuat[4]; // previous rotation quaternion // std::list rVecHist; // marker rotation history float *rVecHist; // marker rotation history with N * 4 elements (quaternions) for smoothing effect diff --git a/tools.cpp b/tools.cpp index be12fe2..17ced59 100644 --- a/tools.cpp +++ b/tools.cpp @@ -98,6 +98,120 @@ float Tools::quatDot(const float q1[4], const float q2[4]) { + q1[3] * q2[3]; } +void Tools::slerp(const float qa[4], const float qb[4], float t, float qc[4]) { + float cosHalfTheta = quatDot(qa, qb); + + // if qa=qb or qa=-qb then theta = 0 and we can return qa + if (abs(cosHalfTheta) >= 1.0){ + qc[0] = qa[0]; + qc[1] = qa[1]; + qc[2] = qa[2]; + qc[3] = qa[3]; + + return; + } + + // Calculate temporary values. + float halfTheta = acosf(cosHalfTheta); + float sinHalfTheta = sqrtf(1.0f - cosHalfTheta * cosHalfTheta); + + // if theta = 180 degrees then result is not fully defined + // we could rotate around any axis normal to qa or qb + if (fabs(sinHalfTheta) < 0.001f){ // fabs is floating point absolute + qc[0] = qa[0] * 0.5f + qb[0] * 0.5f; + qc[1] = qa[1] * 0.5f + qb[1] * 0.5f; + qc[2] = qa[2] * 0.5f + qb[2] * 0.5f; + qc[3] = qa[3] * 0.5f + qb[3] * 0.5f; + + return; + } + + float ratioA = sinf((1.0f - t) * halfTheta) / sinHalfTheta; + float ratioB = sinf(t * halfTheta) / sinHalfTheta; + + //calculate Quaternion + qc[0] = qa[0] * ratioA + qb[0] * ratioB; + qc[1] = qa[1] * ratioA + qb[1] * ratioB; + qc[2] = qa[2] * ratioA + qb[2] * ratioB; + qc[3] = qa[3] * ratioA + qb[3] * ratioB; +} + +void Tools::rotVecToEuler(const float r[3], float eu[3]) { + float ang = VEC3_NORM(r); + float s = sinf(ang); + float c = cosf(ang); + float t = 1.0f - c; + + float x = r[0] / ang; + float y = r[1] / ang; + float z = r[2] / ang; + + float singCheck = x * y * t + z * s; + if (fabsf(singCheck) > 0.998f) { // north or south pole singularity detected + float sign = singCheck > 0.0f ? 1.0f : -1.0f; + eu[0] = sign * 2.0f * atan2f(x * sinf(ang / 2.0f), cosf(ang / 2.0f)); + eu[1] = sign * M_PI / 2.0f; + eu[2] = 0.0f; + } else { + eu[0] = atan2f(y * s - x * z * t, 1.0f - (y*y + z*z) * t); + eu[1] = asinf(x * y * t + z * s); + eu[2] = atan2f(x * s - y * z * t, 1.0f - (x*x + z*z) * t); + } + +// if (eu[0] < 0.0f) { +// eu[0] += 2.0f * M_PI; +// eu[1] *= -1.0f; +// } else if (eu[0] > 2.0f * M_PI) { +// eu[0] -= 2.0f * M_PI; +// eu[1] *= -1.0f; +// eu[2] = 2.0f * M_PI - eu[2]; +// } + +// // extension: always keep the euler values positive +// if (eu[0] < 0.0f) eu[0] += 2.0f * M_PI; +// if (eu[1] < 0.0f) eu[1] += 2.0f * M_PI; +// if (eu[2] < 0.0f) eu[2] += 2.0f * M_PI; + +// float cosrx = cosf(eu[0]); +// float m10 = cosf(eu[0]) * cosf(eu[1]); +// float m5 = cosf(eu[0]) * cosf(eu[2]); +// if (m10 * cosrx < 0.0f) eu[1] += M_PI; +// if (m5 * cosrx < 0.0f) eu[2] += M_PI; +} + +void Tools::eulerToRotVec(const float eu[3], float r[3]) { + float euXhalf = eu[0] / 2.0f; + float euYhalf = eu[1] / 2.0f; + float euZhalf = eu[2] / 2.0f; + + float c1 = cosf(euXhalf); + float s1 = sinf(euXhalf); + float c2 = cosf(euYhalf); + float s2 = sinf(euYhalf); + float c3 = cosf(euZhalf); + float s3 = sinf(euZhalf); + + float c1c2 = c1 * c2; + float s1s2 = s1 * s2; + float w = c1c2 * c3 - s1s2 * s3; + r[0] =c1c2 * s3 + s1s2 * c3; + r[1] =s1 * c2 * c3 + c1 * s2 * s3; + r[2] =c1 * s2 * c3 - s1 * c2 * s3; + float ang = 2.0f * acosf(w); + + float norm = VEC3_NORM(r); + if (norm < 0.001f) { // when all euler angles are zero angle =0 so + // we can set axis to anything to avoid divide by zero + r[0] = 1.0f; + r[1] = r[2] = 0.0f; + } else { + float magn = ang / sqrtf(norm); + r[0] *= magn; + r[1] *= magn; + r[2] *= magn; + } +} + //void Tools::getAvgRotVec(const float *rotVecs, int numRotVecs, float *avgRotVec) { // float avgTheta = 0.0f; // float avgDirVec[] = {0.0f, 0.0f, 0.0f}; diff --git a/tools.h b/tools.h index c6cae60..d5ecc14 100644 --- a/tools.h +++ b/tools.h @@ -50,6 +50,11 @@ class Tools { static float quatDot(const float q1[4], const float q2[4]); + static void rotVecToEuler(const float r[3], float eu[3]); + static void eulerToRotVec(const float eu[3], float r[3]); + + static void slerp(const float qa[4], const float qb[4], float t, float qc[4]); + // /** // * // */