artvertiser/MatrixTracker/MatrixTracker.cpp

   1 #include "MatrixTracker.h"
   2
   3 #include "ofxMatrix4x4.h"
   4
   5 #include <stdio.h>
   6
   7
   8 /*static const int NUM_FRAMES_BACK_RAW = 5;
   9 static const int NUM_FRAMES_BACK_RETURNED = 20;*/
  10
  11
  12 MatrixTracker::MatrixTracker()
  13 : kalman(0)
  14 {
  15     rotation_smoothing   = 0.5f;
  16     position_smoothing   = 0.147f;
  17     position_smoothing_z = 0.7f;
  18
  19     // num_frames_back_* must be < PRUNE_MAX_SIZE
  20     num_frames_back_raw      = 2;
  21     num_frames_back_returned = 10;
  22
  23     kalman_dt = 0.05f; // 20 fps kalman default
  24
  25 }
  26
  27 MatrixTracker::~MatrixTracker()
  28 {
  29     lock();
  30     if ( kalman )
  31     {
  32         cvReleaseKalman( &kalman );
  33         cvReleaseMat( &x_k );
  34         cvReleaseMat( &w_k );
  35         cvReleaseMat( &z_k );
  36     }
  37     unlock();
  38 }
  39
  40
  41
  42
  43 void MatrixTracker::addPose( CvMat* matrix, const FTime& timestamp )
  44 {
  45     assert( matrix->width == 4 && matrix->height == 3 );
  46
  47     // construct 4x4 rot matrix from 3x3 in top left of matrix,
  48     // filling in last row/col with 0,0,0,1
  49     ofxMatrix4x4 rot;
  50     for ( int i=0; i<4; i++ ) // y
  51         for ( int j=0; j<4; j++ ) // x
  52             rot._mat[i][j] = (i==3||j==3)?0:cvGet2D(matrix, i, j).val[0];
  53     rot._mat[3][3] = 1;
  54
  55     ofxVec3f trans( cvGet2D(matrix, 0/*y*/, 3/*x*/ ).val[0], cvGet2D(matrix, 1, 3 ).val[0], cvGet2D( matrix, 2, 3 ).val[0] );
  56
  57     addPose( rot, trans, timestamp );
  58
  59 }
  60
  61
  62 void MatrixTracker::addPose( const ofxMatrix4x4& rot, const ofxVec3f& new_translation, const FTime& timestamp )
  63 {
  64   //  printf("adding pose for timestamp %f\n", timestamp.ToSeconds() );
  65
  66     // convert 4x4 rot matrix to quaternion
  67     ofxQuaternion new_rotation;
  68     new_rotation.set( rot );
  69
  70     lock();
  71
  72     found_poses[timestamp] = Pose( new_translation, new_rotation );
  73
  74     // limit size by trimming oldies
  75     if ( found_poses.size() > PRUNE_MAX_SIZE )
  76     {
  77         //delete (*found_poses.begin()).second;
  78         found_poses.erase( found_poses.begin() );
  79         assert( found_poses.size() == PRUNE_MAX_SIZE );
  80     }
  81
  82     unlock();
  83
  84 }
  85
  86 void MatrixTracker::addPoseKalman( CvMat* matrix, unsigned int at_frame_index )
  87 {
  88     assert( matrix->width == 4 && matrix->height == 3 );
  89
  90     // construct 4x4 rot matrix from 3x3 in top left of matrix,
  91     ofxMatrix4x4 rot;
  92     for ( int i=0; i<4; i++ ) // y
  93         for ( int j=0; j<4; j++ ) // x
  94             // fill; for last row/col use 0,0,0,0
  95             rot._mat[i][j] = (i==3||j==3)?0:cvGet2D(matrix, i, j).val[0];
  96     // now set bottom-right corner to 1
  97     rot._mat[3][3] = 1;
  98
  99     ofxVec3f trans( cvGet2D(matrix, 0/*y*/, 3/*x*/ ).val[0], cvGet2D(matrix, 1, 3 ).val[0], cvGet2D( matrix, 2, 3 ).val[0] );
 100
 101     // store in kalman map
 102     ofxQuaternion rot_quat;
 103     rot_quat.set( rot );
 104
 105     lock();
 106     kalman_found_poses[ at_frame_index ] = Pose( trans, rot_quat );
 107     // limit size by trimming oldies
 108     if ( kalman_found_poses.size() > PRUNE_MAX_SIZE )
 109     {
 110         //delete (*found_poses.begin()).second;
 111         kalman_found_poses.erase( kalman_found_poses.begin() );
 112         assert( kalman_found_poses.size() == PRUNE_MAX_SIZE );
 113     }
 114
 115     printf("added kalman pose at frame %i\n", at_frame_index );
 116
 117     unlock();
 118 }
 119
 120 bool MatrixTracker::getInterpolatedPoseKalman( CvMat* matrix, unsigned int for_frame_index )
 121 {
 122     ofxMatrix4x4 interpolated_pose;
 123     bool res = getInterpolatedPoseKalman( interpolated_pose, for_frame_index );
 124     for ( int i=0; i<3; i++ )
 125         for ( int j=0; j<4; j++ )
 126             cvmSet( matrix, i, j, interpolated_pose( i, j ) );
 127     return res;
 128 }
 129
 130 bool MatrixTracker::getInterpolatedPoseKalman( ofxMatrix4x4& interpolated_pose, unsigned int for_frame_index )
 131 {
 132     lock();
 133
 134     printf("kalman pose for frame %i requested\n", for_frame_index );
 135
 136     if ( kalman == 0 )
 137         createKalman();
 138
 139     // step through frames until we reach for_frame_index, updating kalman
 140     while ( kalman_curr_frame_index != for_frame_index /* use != not < in case it wraps */  )
 141     {
 142         // predict the new state
 143         const CvMat* y_k = cvKalmanPredict( kalman ); // predicted new state
 144         printf("kalman predicts:\n ");
 145         for ( int i=0; i<y_k->rows; i++ )
 146             printf("%7.3f ", cvmGet( y_k, i, 0 ) );
 147         printf("\n");
 148
 149         // read state back
 150         predicted_pose_kalman.translation.set(cvmGet( y_k, 0, 0 ),
 151                                               cvmGet( y_k, 1, 0 ),
 152                                               cvmGet( y_k, 2, 0 )
 153                                               );
 154         ofxVec4f kalman_predicted_rot(        cvmGet( y_k, 3, 0 ),
 155                                               cvmGet( y_k, 4, 0 ),
 156                                               cvmGet( y_k, 5, 0 ),
 157                                               cvmGet( y_k, 6, 0 )
 158                                               );
 159
 160         kalman_predicted_rot.normalize();
 161         predicted_pose_kalman.rotation = kalman_predicted_rot;
 162         printf("got rotation quaternion %7.3f %7.3f %7.3f %7.3f\n",
 163                 kalman_predicted_rot.x,
 164                 kalman_predicted_rot.y,
 165                 kalman_predicted_rot.z,
 166                 kalman_predicted_rot.w
 167                );
 168
 169         // new measurement?
 170         if ( hasMeasurementForTime( kalman_curr_frame_index ) )
 171         {
 172             // copy measurement to kalman_measurement matrix (z_k)
 173             const Pose& measurement = kalman_found_poses[kalman_curr_frame_index];
 174             // store translation
 175             cvmSet( z_k, 0, 0, measurement.translation.x );
 176             cvmSet( z_k, 1, 0, measurement.translation.y );
 177             cvmSet( z_k, 2, 0, measurement.translation.z );
 178             // and rotation
 179             cvmSet( z_k, 3, 0, measurement.rotation.asVec4().x );
 180             cvmSet( z_k, 4, 0, measurement.rotation.asVec4().y );
 181             cvmSet( z_k, 5, 0, measurement.rotation.asVec4().z );
 182             cvmSet( z_k, 6, 0, measurement.rotation.asVec4().w );
 183             /*predicted_pose_kalman.rotation = measurement.rotation * (1.0f-rotation_smoothing)
 184                 + predicted_pose_kalman.rotation * rotation_smoothing;*/
 185             // multiply measurements by measurement matrix
 186             // z_k = x_k*kalman->measurement_matrix + z_k
 187             //cvMatMulAdd( kalman->measurement_matrix, x_k, z_k, z_k );
 188             //cvMatMul( kalman->measurement_matrix, z_k, z_k );
 189
 190             // correct the prediction against our measurements
 191             cvKalmanCorrect( kalman, z_k );
 192         }
 193         else
 194         {
 195             // update process noise (w_k)
 196             cvRand( &kalman_rng, w_k );
 197             // multiply by process noise matrix
 198             cvMatMul( kalman->process_noise_cov, w_k, w_k );
 199             // update by pushing last predicted state through transition matrix, adding process noise
 200             cvMatMulAdd( kalman->transition_matrix, y_k, w_k, x_k );
 201             // read off values for z
 202             printf("pushing prediction through transition matrix gives us:\n ");
 203             for ( int i=0; i<3+4; i++ )
 204             {
 205                 printf("%7.3f ", cvmGet( x_k, i, 0 ) );
 206                 cvmSet( z_k, i, 0, cvmGet( x_k, i, 0 ) );
 207             }
 208             printf("\n");
 209             // normalise the rotation quaternion, to be sure
 210             ofxVec4f rot_quat(cvmGet(x_k, 3, 0),
 211                               cvmGet(x_k, 4, 0),
 212                               cvmGet(x_k, 5, 0),
 213                               cvmGet(x_k, 6, 0));
 214             rot_quat.normalize();
 215             // put back
 216             cvmSet( z_k, 3, 0, rot_quat.x );
 217             cvmSet( z_k, 4, 0, rot_quat.y );
 218             cvmSet( z_k, 5, 0, rot_quat.z );
 219             cvmSet( z_k, 6, 0, rot_quat.w );
 220             // update kalman
 221             cvKalmanCorrect( kalman, z_k );
 222         }
 223
 224
 225         /*// copy pre to post
 226         memcpy( x_k->data.fl, kalman->state_post->data.fl,
 227                sizeof(float)*kalman->state_pre->cols*kalman->state_pre->rows );*/
 228
 229
 230         // step time forward
 231         kalman_curr_frame_index++;
 232     }
 233
 234     // return
 235     make4x4MatrixFromQuatTrans(predicted_pose_kalman.rotation,
 236                                predicted_pose_kalman.translation,
 237                                interpolated_pose );
 238     unlock();
 239 }
 240
 241 bool MatrixTracker::hasMeasurementForTime( unsigned int frame_index )
 242 {
 243     return (kalman_found_poses.find(frame_index)!= kalman_found_poses.end());
 244 }
 245
 246 void MatrixTracker::createKalman()
 247 {
 248     assert( kalman == 0 );
 249
 250     // construct kalman struct
 251     int num_dynamic_params = 3+3+4+4; // pos+vel+rot+vr
 252     int num_measured_params = 3+4;  // pos, rot
 253     kalman = cvCreateKalman( num_dynamic_params, num_measured_params );
 254
 255     // setup transition matrix
 256     const float dt = kalman_dt;
 257     const float transition_matrix[] = { /*px*/ 1,  0,  0,  0,  0,  0,  0, dt,  0,  0,  0,  0,  0,  0, // px = px + dt*vx;
 258                                         /*py*/ 0,  1,  0,  0,  0,  0,  0,  0, dt,  0,  0,  0,  0,  0, // py = py + dt*vy;
 259                                         /*pz*/ 0,  0,  1,  0,  0,  0,  0,  0,  0, dt,  0,  0,  0,  0, // pz = pz + dt*vz;
 260                                         /*rot*/0,  0,  0,  1,  0,  0,  0,  0,  0,  0, dt,  0,  0,  0, // rx = rx + dt*vrx
 261                                         /*rot*/0,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0, dt,  0,  0, // ...
 262                                         /*rot*/0,  0,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0, dt,  0,
 263                                         /*rot*/0,  0,  0,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0, dt,
 264                                         /*vx*/ 0,  0,  0,  0,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0,
 265                                         /*vy*/ 0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  0,  0,  0,  0,
 266                                         /*vz*/ 0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  0,  0,  0,
 267                                         /*vr */0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  0,  0,
 268                                         /*vr */0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  0,
 269                                         /*vr */0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,
 270                                         /*vr */0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1
 271                                          };
 272     memcpy( kalman->transition_matrix->data.fl, transition_matrix, sizeof(transition_matrix) );
 273
 274     // setup other matrices
 275     w_k = cvCreateMat( num_dynamic_params,  1, CV_32FC1 );
 276     z_k = cvCreateMat( num_measured_params, 1, CV_32FC1 );
 277     x_k = cvCreateMat( num_dynamic_params,  1, CV_32FC1 );
 278
 279     // initialise remaining kalman struct
 280
 281     // measurement matrix is num_measured_params rows x num_dynamic_params cols
 282     cvSetIdentity( kalman->measurement_matrix,      cvRealScalar(1) );
 283
 284     // dunno what this is
 285     cvSetIdentity( kalman->error_cov_pre,           cvRealScalar(1) );
 286
 287     // how noisy are our measurements?
 288     cvSetIdentity( kalman->process_noise_cov );
 289     cvSetIdentity( kalman->measurement_noise_cov );
 290     // position is quite noisy
 291     for ( int i=0; i<3; i++ )
 292     {
 293         cvmSet( kalman->process_noise_cov,     i, i, 1e-2 );
 294         cvmSet( kalman->measurement_noise_cov, i, i, 1 );
 295     }
 296     // rotation is not so noisy
 297     for ( int i=3; i<7; i++ )
 298     {
 299         cvmSet( kalman->process_noise_cov,     i, i, 1e-3 );
 300         cvmSet( kalman->measurement_noise_cov, i, i, 1e-1 );
 301     }
 302
 303     // choose random initial state
 304     cvRandInit( &kalman_rng, 0, 1, -1, CV_RAND_UNI );
 305
 306     /*cvRandSetRange( &kalman_rng, 0, 0.1, 0  );
 307     kalman_rng.disttype = CV_RAND_NORMAL;*/
 308     cvRand( &kalman_rng, x_k );
 309     //cvRand( &kalman_rng, kalman->state_post );
 310     cvZero( x_k );
 311     cvZero( kalman->state_post );
 312     cvmSet( x_k, 7, 0, 1 );
 313     cvmSet( kalman->state_post, 7, 0, 1 );
 314
 315 }
 316
 317 /*
 318     // smooth
 319     // motivation: translation should be allowed to change faster than orientation
 320     translation = 0.3f*translation + 0.7f*new_translation;
 321     // so we allow the rotation to lag on a bit
 322     rotation = 0.125f*rotation.asVec4() + 0.875f*new_rotation.asVec4();
 323     //so = new_so;
 324
 325
 326   //  printf("so: %10f %10f %10f %10f\n", so.asVec4().x, so.asVec4().y, so.asVec4().z, so.asVec4().w );
 327   //  printf("scale: %10f %10f %10f\n", scale.x, scale.y, scale.z );
 328
 329     printf("translation now %10f %10f %10f\n", translation.x, translation.y, translation.z );
 330     ofxVec4f r = rotation.asVec4();
 331     printf("rotation now  %10f %10f %10f %10f\n", r.x, r.y, r.z, r.w );
 332
 333     //printf("translation: decomposed %10f %10f %10f, source %10f %10f %10f\n",
 334     //       new_translation.x, new_translation.y, new_translation.z,
 335     //       matrix(3,0), matrix(3,1), matrix(3,2) );
 336
 337     */
 338
 339
 340
 341
 342 bool MatrixTracker::getInterpolatedPose( CvMat* matrix, const FTime& for_time )
 343 {
 344     ofxMatrix4x4 interpolated_pose;
 345     bool res = getInterpolatedPose( interpolated_pose, for_time );
 346     for ( int i=0; i<3; i++ )
 347         for ( int j=0; j<4; j++ )
 348             cvmSet( matrix, i, j, interpolated_pose( i, j ) );
 349     return res;
 350 }
 351
 352
 353 bool MatrixTracker::getInterpolatedPose( ofxMatrix4x4& interpolated_pose, const FTime& for_time )
 354 {
 355
 356     // so then
 357     // where are we?
 358
 359     lock();
 360
 361     PoseMap::iterator prev_pose = found_poses.lower_bound( for_time );
 362     PoseMap::iterator next_pose = found_poses.upper_bound( for_time );
 363     // we must step forward
 364     /*if ( next_pose != found_poses.end() )
 365         ++next_pose;*/
 366     //if ( prev_pose !=   )
 367
 368     bool result = false;
 369
 370     if ( found_poses.size() == 0 )
 371     {
 372         // nothing there
 373         //printf("empty\n");
 374         interpolated_pose.makeIdentityMatrix();
 375         result = false;
 376     }
 377     else if ( for_time == (*prev_pose).first )
 378     {
 379         // exactly on first
 380         //printf("exactly on prev\n");
 381         smoothAndMakeMatrix( (*prev_pose).second.rotation, (*prev_pose).second.translation,
 382                             for_time, interpolated_pose );
 383         result = true;
 384     }
 385     else if ( next_pose == found_poses.end() )
 386     {
 387         // we only have prev : must estimate forwrads from known data
 388         //printf("future %f: after last (%f), forwards estimate: \n", for_time.ToSeconds(), (--prev_pose)!=found_poses.begin()?(*prev_pose).first.ToSeconds():0.0f );
 389         //interpolated_pose.makeIdentityMatrix();
 390
 391
 392         /*// just use the last one
 393         make4x4MatrixFromQuatTrans( (*found_poses.rbegin()).second.rotation, (*found_poses.rbegin()).second.translation, interpolated_pose );
 394         */
 395         --prev_pose;
 396         // walk back 3 frame
 397         const FTime* times[num_frames_back_raw];// = (*prev_pose).first;
 398         const Pose* poses[num_frames_back_raw];//  = (*prev_pose).second;
 399         int last_frame_back = num_frames_back_raw-1;
 400         bool failed = false;
 401         // collect previous poses: bail and set failed false if we don't have enough
 402         for ( int i=last_frame_back; i>=0 ;i-- )
 403         {
 404             if ( prev_pose==found_poses.begin() )
 405             {
 406                 smoothAndMakeMatrix( (*found_poses.rbegin()).second.rotation,
 407                                     (*found_poses.rbegin()).second.translation,
 408                                     for_time, interpolated_pose );
 409                 failed = true;
 410                 break;
 411             }
 412
 413             times[i] = &(*prev_pose).first;
 414             poses[i] = &(*prev_pose).second;
 415             --prev_pose;
 416         }
 417         /*
 418         i 2 == now
 419         i 1 == then
 420         i 0 == ages ago
 421         */
 422
 423         if ( !failed )
 424         {
 425             // estimate from priors
 426             ofxVec4f final_rot;
 427             ofxVec3f final_pos;
 428             estimateNewPose( poses, times, num_frames_back_raw, for_time, final_pos, final_rot );
 429
 430             //make4x4MatrixFromQuatTrans( final_rot_quat, final_pos, interpolated_pose );
 431             smoothAndMakeMatrix( final_rot, final_pos, for_time, interpolated_pose );
 432         }
 433
 434         result = !failed;
 435
 436     }
 437     else if ( prev_pose == found_poses.begin() )
 438     {
 439         // we don't have prev : must estimate backwards from known data
 440         //printf("past: before first, backwards estimate not implemented yet\n");
 441         interpolated_pose.makeIdentityMatrix();
 442         result = false;
 443     }
 444     else
 445     {
 446         // we have both: interpolate
 447         // pull prev back
 448         --prev_pose;
 449
 450         // fetch endpoints
 451         const FTime& prev_time =    (*prev_pose).first;
 452         const Pose& prev_p =        (*prev_pose).second;
 453         const FTime& next_time =    (*next_pose).first;
 454         const Pose& next_p =        (*next_pose).second;
 455
 456         // lerp
 457         ofxQuaternion lerp_rot;
 458         ofxVec3f lerp_trans;
 459         // calculate t percentage
 460         float t = (for_time.ToSeconds()-prev_time.ToSeconds())/(next_time.ToSeconds()-prev_time.ToSeconds());
 461         //printf("t is %f: before %f requested %f after %f\n", t, prev_time.ToSeconds(), for_time.ToSeconds(), next_time.ToSeconds() );
 462         lerp_rot.slerp( t, prev_p.rotation, next_p.rotation );
 463         lerp_trans = prev_p.translation + t*(next_p.translation-prev_p.translation);
 464
 465         // store in output
 466         smoothAndMakeMatrix( lerp_rot, lerp_trans, for_time, interpolated_pose );
 467
 468         result = true;
 469     }
 470
 471     unlock();
 472
 473     return result;
 474 }
 475
 476
 477
 478 //const ofxMatrix4x4&
 479 void MatrixTracker::make4x4MatrixFromQuatTrans( const ofxQuaternion& rot, const ofxVec3f& trans, ofxMatrix4x4& output )
 480 {
 481     // fetch quaternion as matrix
 482     rot.get( output );
 483     // now write translation into last column
 484     output( 0, 3 ) = trans.x;
 485     output( 1, 3 ) = trans.y;
 486     output( 2, 3 ) = trans.z;
 487 }
 488
 489 void MatrixTracker::smoothAndMakeMatrix( const ofxQuaternion& final_rot, const ofxVec3f& final_pos,
 490                                         const FTime& for_time, ofxMatrix4x4& output )
 491 {
 492
 493     // look up previous n poses in returned_poses
 494     const FTime* times_returned[num_frames_back_returned];
 495     const Pose*  poses_returned[num_frames_back_returned];
 496     int last_frame_back = num_frames_back_returned-1;
 497     bool failed = false;
 498     // collect previous poses: bail and set failed false if we don't have enough
 499     PoseMap::reverse_iterator it = returned_poses.rbegin();
 500     for ( int i=last_frame_back; i>=0; i-- )
 501     {
 502         if ( it==returned_poses.rend() )
 503         {
 504             // smoothAndMakeMatrix( (*found_poses.rbegin()).second.rotation, (*found_poses.rbegin()).second.translation, interpolated_pose );
 505             failed = true;
 506             break;
 507         }
 508
 509         times_returned[i] = &(*it).first;
 510         poses_returned[i] = &(*it).second;
 511
 512         ++it;
 513     }
 514
 515     ofxVec3f output_pos;
 516     ofxVec4f output_rot;
 517     if ( failed )
 518     {
 519         output_pos = final_pos;
 520         output_rot = final_rot.asVec4();
 521     }
 522     else
 523     {
 524         ofxVec3f final_pos_returned;
 525         ofxVec4f final_rot_returned;
 526         estimateNewPose( poses_returned, times_returned, num_frames_back_returned,
 527                         for_time, final_pos_returned, final_rot_returned );
 528
 529         output_rot = final_rot.asVec4()*(1.0f-rotation_smoothing) + final_rot_returned*rotation_smoothing;
 530         output_pos = final_pos*(1.0f-position_smoothing)          + final_pos_returned*position_smoothing;
 531         // different smoothing for z
 532         output_pos.z = final_pos.z*(1.0f-position_smoothing_z)    + final_pos_returned.z*position_smoothing_z;
 533     }
 534
 535     // make matrix
 536     make4x4MatrixFromQuatTrans( output_rot, output_pos, output );
 537
 538     // store
 539     returned_poses[for_time] = Pose( output_pos, output_rot );
 540     // limit size by trimming oldies
 541     if ( returned_poses.size() > PRUNE_MAX_SIZE )
 542     {
 543         //delete (*found_poses.begin()).second;
 544         returned_poses.erase( returned_poses.begin() );
 545         assert( returned_poses.size() == PRUNE_MAX_SIZE );
 546     }
 547
 548
 549 /*
 550     // deal with z: it should change very slowly
 551     float prev_returned_translation_z;
 552     prev_returned_translation_z = final_pos.z*0.1f + prev_returned_translation.z*0.9f;
 553
 554     // average with prev returned
 555     prev_returned_rotation    = final_rot*0.1f + prev_returned_rotation*0.9f;
 556     prev_returned_translation = final_pos*0.5f + prev_returned_translation*0.5f;
 557
 558     // use z
 559     prev_returned_translation.z = prev_returned_translation_z;
 560
 561     make4x4MatrixFromQuatTrans( prev_returned_rotation, prev_returned_translation, output );
 562     //make4x4MatrixFromQuatTrans( final_rot, final_pos, output );*/
 563 }
 564
 565
 566 /*void MatrixTracker::smoothAndMakeMatrix( const ofxQuaternion& final_rot, const ofxVec3f& final_pos, ofxMatrix4x4& output )
 567 {
 568     // deal with z: it should change very slowly
 569     float prev_returned_translation_z;
 570     prev_returned_translation_z = final_pos.z*0.1f + prev_returned_translation.z*0.9f;
 571
 572     // average with prev returned
 573     prev_returned_rotation    = final_rot*0.1f + prev_returned_rotation*0.9f;
 574     prev_returned_translation = final_pos*0.5f + prev_returned_translation*0.5f;
 575
 576     // use z
 577     prev_returned_translation.z = prev_returned_translation_z;
 578
 579     make4x4MatrixFromQuatTrans( prev_returned_rotation, prev_returned_translation, output );
 580     //make4x4MatrixFromQuatTrans( final_rot, final_pos, output );
 581 }
 582 */
 583
 584 void MatrixTracker::estimateNewPose( const Pose** poses, const FTime** times, int num_frames_back, const FTime& for_time,
 585                                     ofxVec3f& final_pos, ofxVec4f& final_rot )
 586 {
 587     // calculate velocity: use (/*9*values[2]+*/ 4*values[1] + values[0])/14.0:
 588     ofxVec3f pos_velocity;
 589     ofxVec4f rot_velocity;
 590     float total_rot_mul = 0;
 591     float total_pos_mul = 0;
 592     int last_frame_back = num_frames_back-1;
 593     for ( int i=0; i<last_frame_back - 1; i++ )
 594     {
 595         // square falloff : fresher = more important
 596         double abs_delta_time = for_time.ToSeconds() - times[i+1]->ToSeconds();
 597         //float pos_mul = (i+1)/**(i+1)*/;
 598         //float rot_mul = sqrtf(i+1);
 599         float pos_mul = 1.0f/(abs_delta_time*abs_delta_time);
 600         float rot_mul = 1.0f/abs_delta_time;
 601         double this_delta_time = times[i+1]->ToSeconds() - times[i]->ToSeconds();
 602         ofxVec3f this_pos_velocity = (poses[i+1]->translation       - poses[i]->translation)       / this_delta_time;
 603         ofxVec4f this_rot_velocity = (poses[i+1]->rotation.asVec4() - poses[i]->rotation.asVec4()) / this_delta_time;
 604
 605         // accumulate
 606         pos_velocity += pos_mul*this_pos_velocity;
 607         rot_velocity += rot_mul*this_rot_velocity;
 608         total_rot_mul += rot_mul;
 609         total_pos_mul += pos_mul;
 610     }
 611     pos_velocity /= total_pos_mul;
 612     rot_velocity /= total_rot_mul;
 613
 614     // so now: we have a position velocity + a rotation velocity as at times[2]
 615     double time_from_prev = for_time.ToSeconds() - times[last_frame_back]->ToSeconds();
 616     /*printf("  dt %f: pos vel %6.3f %6.3f %6.3f, rot vel %6.3f %6.3f %6.3f %6.3f\n",
 617            time_from_prev, pos_velocity.x, pos_velocity.y, pos_velocity.z,
 618            rot_velocity.x, rot_velocity.y, rot_velocity.z, rot_velocity.w );*/
 619
 620     // so final pos/rot are
 621     final_pos = poses[last_frame_back]->translation + pos_velocity*time_from_prev;
 622     final_rot = poses[last_frame_back]->rotation.asVec4()    + rot_velocity*time_from_prev;
 623 }
 624