fixed 'fly in' problem

htw-inka · Jun 30, 2014 · 73991bd · 73991bd
1 parent fb51774
commit 73991bd
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 1 deletion.
diff --git a/marker.cpp b/marker.cpp
@@ -73,7 +73,10 @@ 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);
         pushVecsToHistory(rVecPtr, tVecPtr);
-        calcSmoothPoseVecs(rVecPtr, tVecPtr);
+
+        if (pushedHistVecs >= OCV_AR_CONF_SMOOTHING_HIST_SIZE) {
+            calcSmoothPoseVecs(rVecPtr, tVecPtr);
+        }
     }
 
     // create rotation matrix
@@ -143,6 +146,7 @@ void Marker::calcShapeProperties() {
 void Marker::init() {
     // set defaults
     id = -1;
+    pushedHistVecs = 0;
 
 	rVec.zeros(3, 1, CV_32F);
 	tVec.zeros(3, 1, CV_32F);
@@ -179,6 +183,10 @@ void Marker::pushVecsToHistory(const float *r, const float *t) {
     rVecHist[numHistElems - 3] = r[0];
     rVecHist[numHistElems - 2] = r[1];
     rVecHist[numHistElems - 1] = r[2];
+
+    if (pushedHistVecs < OCV_AR_CONF_SMOOTHING_HIST_SIZE) {
+        pushedHistVecs++;
+    }
 }
 
 void Marker::calcSmoothPoseVecs(float *r, float *t) {

diff --git a/marker.h b/marker.h
@@ -177,6 +177,8 @@ class Marker {
     cv::Mat rVec;           // 3D pose rotation vector
     cv::Mat tVec;           // 3D pose translation vector
 
+    int pushedHistVecs;     // number of vectors in <tVecHist> and <rVecHist>
+
     float *tVecHist;        // marker position history with N * 3 elements for smoothing effect
     float *rVecHist;        // marker rotation history with N * 3 elements for smoothing effect