Skip to content

Commit

Permalink
euler, quaternions, angle-axis, slerp ... everything tested, nothing …
Browse files Browse the repository at this point in the history 
…works
  • Loading branch information
@internaut
internaut committed Jul 1, 2014
1 parent 517cfc6 commit 4088105
Show file tree
Hide file tree
Showing 4 changed files with 148 additions and 18 deletions.
45 changes: 28 additions & 17 deletions marker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,30 +73,43 @@ void Marker::updatePoseMat(const cv::Mat &r, const cv::Mat &t, bool useSmoothing
float *rVecPtr = rVec.ptr<float>(0);
float *tVecPtr = tVec.ptr<float>(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);
Expand Down Expand Up @@ -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() {
Expand Down Expand Up @@ -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];
Expand Down
2 changes: 1 addition & 1 deletion marker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,7 +181,7 @@ class Marker {
int pushedHistVecs; // number of vectors in <tVecHist> and <rVecHist>

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<cv::Mat> rVecHist; // marker rotation history
float *rVecHist; // marker rotation history with N * 4 elements (quaternions) for smoothing effect

Expand Down
114 changes: 114 additions & 0 deletions tools.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down
5 changes: 5 additions & 0 deletions tools.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]);

// /**
// *
// */
Expand Down

0 comments on commit 4088105

Please sign in to comment.