git.maemo.org Git - opencv/blob - filters/Tracker3dFilter/src/Tracker3dFilter.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2002, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistributions of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistributions in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #pragma warning(disable:4786)
  43 #include <vector>
  44 #include <algorithm>
  45 #include <streams.h>
  46 #include "autorelease.h"
  47 #include <initguid.h>
  48 #include "Tracker3dFilter.h"
  49 #include "Tracker3dPropertyPage.h"
  50 #include "cvaux.h"
  51
  52 #define ARRAY_SIZEOF(a) (sizeof(a)/sizeof((a)[0]))
  53
  54 // locks a critical section, and unlocks it automatically
  55 // when the lock goes out of scope. Also allows explicit unlocking.
  56 class AutoLock {
  57     CCritSec *m_lock;
  58
  59 public:
  60     AutoLock(CCritSec *lock)
  61     {
  62         m_lock = lock;
  63         m_lock->Lock();
  64     };
  65
  66     ~AutoLock()
  67     {
  68         Unlock();
  69     };
  70
  71     void Unlock()
  72     {
  73         if (m_lock)
  74         {
  75             m_lock->Unlock();
  76             m_lock = NULL;
  77         }
  78     };
  79 };
  80
  81 //#define TRACE
  82 #ifdef TRACE
  83
  84 #include <stdarg.h>
  85 #include <stdio.h>
  86
  87 void Trace(const char *fmt, ...)
  88 {
  89     char buf[200];
  90     va_list args;
  91     va_start(args, fmt);
  92     vsprintf(buf, fmt, args);
  93     va_end(args);
  94     OutputDebugString(buf) ;
  95 }
  96 #endif
  97
  98 //
  99 // CreateInstance
 100 //
 101 // Provide the way for COM to create a Tracker3dFilter object
 102 //
 103 CUnknown *Tracker3dFilter::CreateInstance(IUnknown *unk, HRESULT *phr)
 104 {
 105     try
 106     {
 107         *phr = NOERROR;
 108         return new Tracker3dFilter(NAME("3d Tracker"), unk);
 109     }
 110     catch(HRESULT hr)
 111     {
 112         *phr = hr;
 113         return NULL;
 114     }
 115     catch (...)
 116     {
 117         *phr = E_OUTOFMEMORY;
 118         return NULL;
 119     }
 120 } // CreateInstance
 121
 122
 123 //
 124 // Constructor
 125 //
 126 Tracker3dFilter::Tracker3dFilter(TCHAR *name, IUnknown *unk)
 127     : CBaseFilter(name, unk, &m_cs, CLSID_Tracker3dFilter)
 128 {
 129     m_num_streams = 0;
 130     m_streams = NULL;
 131     m_camera_configuration_loaded = false;
 132     m_pCameraInfo = NULL;
 133     m_tracker_clsid = GUID_NULL;
 134     m_calibrate_cameras = 0;
 135     m_camera_intrinsics = NULL;
 136     m_viewing_stream = 0;
 137     m_preferred_size = SIZE_Any;
 138     m_end_of_stream_count = 0;
 139 }
 140
 141 Tracker3dFilter::~Tracker3dFilter()
 142 {
 143     for (int i = 0; i < m_callbacks.size(); i++)
 144         m_callbacks[i]->Release();
 145
 146     delete [] m_streams;
 147     delete [] m_pCameraInfo;
 148     delete [] m_camera_intrinsics;
 149 }
 150
 151
 152 //
 153 // NonDelegatingQueryInterface
 154 //
 155 STDMETHODIMP Tracker3dFilter::NonDelegatingQueryInterface(REFIID riid, void **ppv)
 156 {
 157     CheckPointer(ppv,E_POINTER);
 158
 159     if (riid == IID_ITracker3dFilter)
 160         return GetInterface(static_cast<ITracker3dFilter *>(this), ppv);
 161     else if (riid == IID_ITracker3dInternal)
 162         return GetInterface(static_cast<ITracker3dInternal *>(this), ppv);
 163     else if (riid == IID_ISpecifyPropertyPages)
 164         return GetInterface(static_cast<ISpecifyPropertyPages *>(this), ppv);
 165     else if (riid == IID_IPersist)
 166         return GetInterface(static_cast<IPersist *>(static_cast<IPersistStream *>(this)), ppv);
 167     else if (riid == IID_IPersistStream)
 168         return GetInterface(static_cast<IPersistStream *>(this), ppv);
 169     else
 170         return CBaseFilter::NonDelegatingQueryInterface(riid, ppv);
 171 }
 172
 173 int Tracker3dFilter::GetPinCount()
 174 {
 175     return 2 * m_num_streams;
 176 }
 177
 178 CBasePin *Tracker3dFilter::GetPin(int n)
 179 {
 180     if (n > GetPinCount())
 181         return NULL;
 182     if (n % 2)
 183         return m_streams[n/2].output_pin;
 184     else
 185         return m_streams[n/2].input_pin;
 186 }
 187
 188 static HRESULT InitImageHeader(IplImage *image_header, const CMediaType *mt)
 189 {
 190     const GUID *subtype = mt->Subtype();
 191     ASSERT(*mt->FormatType() == FORMAT_VideoInfo);
 192     VIDEOINFO *fmt = (VIDEOINFO *)mt->Format();
 193
 194     int channels;
 195     int origin;
 196
 197     // We assume that RGB images are inverted and YUV images are not...
 198     if (*subtype == MEDIASUBTYPE_RGB24 || *subtype == MEDIASUBTYPE_RGB32)
 199     {
 200         channels = fmt->bmiHeader.biBitCount / 8;
 201         origin = IPL_ORIGIN_BL;
 202     }
 203     else if (*subtype == MEDIASUBTYPE_YVU9
 204              || *subtype == MEDIASUBTYPE_YV12
 205              || *subtype == MEDIASUBTYPE_IYUV)
 206     {
 207         // Note: these are planar images, which for our purposes we treat as
 208         // a single plane of Y8 and ignore the rest.
 209         channels = 1;
 210         origin = IPL_ORIGIN_TL;
 211     }
 212     else // Other trackers could support other formats, which should be handled here
 213     {
 214         return E_FAIL;
 215     }
 216
 217     cvInitImageHeader(image_header, cvSize(fmt->bmiHeader.biWidth, fmt->bmiHeader.biHeight),
 218                       IPL_DEPTH_8U, channels, origin, 4);
 219     return NOERROR;
 220 }
 221
 222 HRESULT Tracker3dFilter::CheckMediaType(int pin, const CMediaType *mt)
 223 {
 224     Stream &stream = m_streams[pin];
 225
 226     if (stream.media_type.IsValid())
 227         return *mt == stream.media_type ? S_OK : E_FAIL;
 228
 229     if (*mt->Type() != MEDIATYPE_Video || *mt->FormatType() != FORMAT_VideoInfo)
 230         return E_FAIL;
 231
 232     IplImage image_header;
 233     HRESULT hr = InitImageHeader(&image_header, mt);
 234     if (FAILED(hr))
 235         return hr;
 236
 237     if (m_preferred_size == SIZE_320x240
 238         && (image_header.width != 320 || image_header.height != 240))
 239     {
 240         return E_FAIL;
 241     }
 242
 243     if (m_preferred_size == SIZE_640x480
 244         && (image_header.width != 640 || image_header.height != 480))
 245     {
 246         return E_FAIL;
 247     }
 248
 249     if (stream.tracker != NULL)
 250     {
 251         hr = stream.tracker->CheckFormat(&image_header);
 252         if (FAILED(hr))
 253             return hr;
 254     }
 255
 256     return S_OK;
 257 }
 258
 259
 260 HRESULT Tracker3dFilter::SetMediaType(int pin, const CMediaType *mt)
 261 {
 262     Stream &stream = m_streams[pin];
 263
 264     if (stream.media_type.IsValid())
 265         return *mt == stream.media_type ? S_OK : E_FAIL;
 266
 267     HRESULT hr = CheckMediaType(pin, mt);
 268     if (FAILED(hr))
 269         return hr;
 270
 271     hr = InitImageHeader(&stream.image_header1, mt);
 272     if (FAILED(hr))
 273         return hr;
 274     stream.image_header2 = stream.image_header1;
 275
 276     if (stream.tracker != NULL)
 277         stream.tracker->SetFormat(&stream.image_header1);
 278
 279     stream.media_type = *mt;
 280     return NOERROR;
 281 }
 282
 283 HRESULT Tracker3dFilter::GetMediaType(int pin, int pos, CMediaType *mt)
 284 {
 285     if (pos < 0)
 286         return E_INVALIDARG;
 287
 288 #define IYUV 0x56555949
 289     static const struct
 290     {
 291         const GUID *subtype;
 292         FOURCC fourcc;
 293         int width, height, depth;
 294     } types[] = {
 295         { &MEDIASUBTYPE_IYUV,  IYUV,   640, 480, 12 },
 296         { &MEDIASUBTYPE_RGB24, BI_RGB, 640, 480, 24 },
 297         { &MEDIASUBTYPE_RGB24, BI_RGB, 640, 480, 32 },
 298         { &MEDIASUBTYPE_RGB32, BI_RGB, 640, 480, 32 },
 299         { &MEDIASUBTYPE_IYUV,  IYUV,   320, 240, 12 },
 300         { &MEDIASUBTYPE_RGB24, BI_RGB, 320, 240, 24 },
 301         { &MEDIASUBTYPE_RGB24, BI_RGB, 320, 240, 32 },
 302         { &MEDIASUBTYPE_RGB32, BI_RGB, 320, 240, 32 },
 303     };
 304
 305     // If our media type has been set, we respond only with that type;
 306     // otherwise, we respond with each of the types we know about.
 307     if (m_streams[pin].media_type.IsValid())
 308     {
 309         if (pos == 0)
 310         {
 311             *mt = m_streams[pin].media_type;
 312             return S_OK;
 313         }
 314     }
 315     else if (pos < ARRAY_SIZEOF(types))
 316     {
 317         const GUID *subtype = types[pos].subtype;
 318         FOURCC fourcc = types[pos].fourcc;
 319         int width = types[pos].width;
 320         int height = types[pos].height;
 321         int depth = types[pos].depth;
 322
 323         VIDEOINFOHEADER fmt = { { 0, 0, 0, 0 }, { 0, 0, 0, 0 }, 30*width*height*depth, 0, 333333,
 324                                 { sizeof(BITMAPINFOHEADER), width, height, 1, depth, fourcc, width*height*depth/8, 0, 0, 0, 0 } };
 325         mt->SetType(&MEDIATYPE_Video);
 326         mt->SetTemporalCompression(false);
 327         mt->SetSubtype(subtype);
 328         mt->SetSampleSize(width*height*depth/8);
 329         mt->SetFormatType(&FORMAT_VideoInfo);
 330         mt->SetFormat((BYTE *)&fmt, sizeof(fmt));
 331         return S_OK;
 332     }
 333
 334     return VFW_S_NO_MORE_ITEMS;
 335 }
 336
 337 STDMETHODIMP Tracker3dFilter::Run(REFERENCE_TIME t)
 338 {
 339     if (!m_camera_configuration_loaded && m_calibrate_cameras == 0)
 340         LoadCameraConfiguration();
 341     return CBaseFilter::Run(t);
 342 }
 343
 344 STDMETHODIMP Tracker3dFilter::Pause()
 345 {
 346     return CBaseFilter::Pause();
 347 }
 348
 349 STDMETHODIMP Tracker3dFilter::Stop()
 350 {
 351     AutoLock lock1(&m_cs);
 352     AutoLock lock2(&m_recv_cs);
 353
 354     HRESULT hr = CBaseFilter::Stop();
 355     if (FAILED(hr))
 356         return hr;
 357
 358     for (int i = 0; i < m_num_streams; i++)
 359         Flush(i);
 360     m_end_of_stream_count = 0;
 361
 362     return NOERROR;
 363 }
 364
 365 HRESULT Tracker3dFilter::Flush(int pin)
 366 {
 367     Stream::SampleQueue &q = m_streams[pin].queue;
 368     while (!q.empty())
 369         q.pop_front();
 370     return NOERROR;
 371 }
 372
 373 Tracker3dFilter::Stream::Stream()
 374      : input_pin(NULL),
 375        output_pin(NULL),
 376        tracker(NULL),
 377        discarded_frames(0),
 378        total_frames(0)
 379 {
 380 }
 381
 382 Tracker3dFilter::Stream::~Stream()
 383 {
 384     SAFE_RELEASE(input_pin);
 385     SAFE_RELEASE(output_pin);
 386     SAFE_RELEASE(tracker);
 387 }
 388
 389 void Tracker3dFilter::Stream::operator =(Stream &s)
 390 {
 391     SAFE_RELEASE(input_pin);
 392     SAFE_RELEASE(output_pin);
 393     SAFE_RELEASE(tracker);
 394
 395     input_pin = s.input_pin;   s.input_pin = NULL;
 396     output_pin = s.output_pin; s.output_pin = NULL;
 397     tracker = s.tracker;       s.tracker = NULL;
 398     image_header1 = image_header2 = s.image_header1;
 399     media_type = s.media_type;
 400 }
 401
 402 inline Tracker3dFilter::Stream::Sample::Sample(IMediaSample *s, const ITracker::TrackingInfo &t)
 403     : sample(s),
 404       tracking_info(t)
 405 {
 406     sample->AddRef();
 407     sample->GetTime(&ts, &te);
 408     sample->GetPointer(&data);
 409 }
 410
 411 inline Tracker3dFilter::Stream::Sample::Sample(const Sample &s)
 412     : sample(s.sample),
 413       ts(s.ts),
 414       te(s.te),
 415       data(s.data),
 416       tracking_info(s.tracking_info)
 417 {
 418     sample->AddRef();
 419 }
 420
 421 inline void Tracker3dFilter::Stream::Sample::operator=(const Sample &s)
 422 {
 423     sample->Release();
 424
 425     sample = s.sample;
 426     ts = s.ts;
 427     te = s.te;
 428     data = s.data;
 429     tracking_info = s.tracking_info;
 430     sample->AddRef();
 431 }
 432
 433 inline Tracker3dFilter::Stream::Sample::~Sample()
 434 {
 435     sample->Release();
 436 }
 437
 438 //-----------------------
 439 // This is an attempt to match the frames up across the streams which are connected to the tracker.
 440 // The goal is to minimize the time differences between the frames in one set.  This is necessary since
 441 // there is no available syncronization mechanism for the independant USB cameras or other video sources.
 442 //-----------------------
 443 bool Tracker3dFilter::QueuesReady()
 444 {
 445     int i;
 446
 447     REFERENCE_TIME greatest = 0;
 448     REFERENCE_TIME least = 0;
 449     for (i = 0; i < m_num_streams; i++)
 450     {
 451         Stream::SampleQueue &q = m_streams[i].queue;
 452         if (q.empty())
 453             return false;
 454         REFERENCE_TIME ts = q.front().ts;
 455         if (ts > greatest)
 456             greatest = ts;
 457         if (ts < least || least == 0)
 458             least = ts;
 459     }
 460
 461     // If the difference between the greatest timestamp and the least is less than
 462     // the specified difference (1/60 sec or 1/2 nominal frame time), all queues
 463     // are ready.
 464 #define MAX_DIFF (10000000/60)
 465     if (greatest - least <= MAX_DIFF)
 466         return true;
 467
 468     // Otherwise, check in each queue whether the 'greatest' frame is a closer time
 469     // match to the second frame in the queue than to the first frame in the queue.
 470     // If so, discard the first frame in the queue.
 471     // This algorithm requires a second frame in each queue, so it introduces one
 472     // full frame delay.
 473     for (i = 0; i < m_num_streams; i++)
 474     {
 475         Stream::SampleQueue &q = m_streams[i].queue;
 476         while (q.size() > 1 && abs(q.begin()->ts - greatest) > abs((q.begin() + 1)->ts - greatest))
 477         {
 478             m_streams[i].discarded_frames++;
 479 #ifdef TRACE
 480             Trace("Discard:   %*s %5lu\n", 6*i, "", (unsigned long)q.front().ts/10000);
 481 #endif
 482             q.pop_front();
 483         }
 484
 485         if (q.size() > 1)
 486             continue;
 487         if (abs(greatest - q.front().ts) <= MAX_DIFF)
 488             continue;
 489         return false;
 490     }
 491
 492     return true;
 493 }
 494
 495 //-----------------------
 496 // Performs the frame to frame work...
 497 //-----------------------
 498 HRESULT Tracker3dFilter::Receive(int pin, IMediaSample *sample)
 499 {
 500     int i;
 501
 502     Stream &stream = m_streams[pin];
 503     ITracker::TrackingInfo tracking_info;
 504
 505     if (stream.tracker != NULL && m_calibrate_cameras == 0)
 506     {
 507         unsigned char *data;
 508         sample->GetPointer(&data);
 509         stream.image_header1.imageData = (char *)data;
 510         try
 511         {
 512             stream.tracker->Process(&stream.image_header1);
 513             stream.tracker->GetTrackedObjects(tracking_info);
 514         }
 515         catch (...)
 516         {
 517         }
 518         stream.image_header1.imageData = NULL;
 519     }
 520
 521     AutoLock lock(&m_recv_cs);
 522
 523     stream.queue.push_back(Stream::Sample(sample, tracking_info));
 524
 525 #ifdef TRACE
 526     Trace("Receive(%d):%*s %5lu\n", pin, 6*pin, "", (unsigned long)stream.queue.back().ts/10000);
 527 #endif
 528
 529     if (!QueuesReady())
 530         return NOERROR;
 531
 532 #ifdef TRACE
 533     char buf[100] = "Process:   ";
 534     char *p = buf + strlen(buf);
 535     REFERENCE_TIME greatest = 0;
 536     REFERENCE_TIME least = 0;
 537     for (i = 0; i < m_num_streams; i++)
 538     {
 539         REFERENCE_TIME ts = m_streams[i].queue.front().ts;
 540         if (ts > greatest)
 541             greatest = ts;
 542         if (ts < least || least == 0)
 543             least = ts;
 544         sprintf(p, " %5lu", (unsigned long)ts/10000);
 545         p += strlen(p);
 546     }
 547     sprintf(p, "  %4ld", (long)((greatest - least)/10000));
 548     p += strlen(p);
 549     static unsigned long processed_frames;
 550     sprintf(p, ";  %4lu", ++processed_frames);
 551     p += strlen(p);
 552     for (i = 0; i < m_num_streams; i++)
 553     {
 554         Stream &s = m_streams[i];
 555         sprintf(p, " %4lu (%2lu%%)", s.discarded_frames, (100*s.discarded_frames/(processed_frames+s.discarded_frames)));
 556         p += strlen(p);
 557     }
 558
 559     Trace("%s\n", buf);
 560 #endif
 561
 562     if (m_calibrate_cameras != 0)
 563     {
 564         std::vector<IplImage *> samples(m_num_streams);
 565         for (i = 0; i < m_num_streams; i++)
 566         {
 567             m_streams[i].image_header2.imageData = (char *)m_streams[i].queue.front().data;
 568             samples[i] = &m_streams[i].image_header2;
 569         }
 570                 //
 571                 //  This is the second most significant call in the filter, this is how the relative locations of
 572                 //  all of the cameras are determined.  This information is vital to the later process of determining
 573                 //  the location of the targets in 3D from the set of 2D results provided by the trackers.
 574                 //
 575         if (cv3dTrackerCalibrateCameras(m_num_streams, m_camera_intrinsics, m_etalon_size,
 576                                         m_square_size, &samples[0], m_pCameraInfo))
 577         {
 578             if (m_calibrate_cameras == 1)
 579             {
 580                 m_calibrate_cameras = 0;
 581                 SaveCameraConfiguration();
 582             }
 583         }
 584     }
 585     else
 586     {
 587         int num_objects = 0;
 588
 589         for (i = 0; i < m_num_streams; i++)
 590         {
 591             int n = m_streams[i].queue.front().tracking_info.size();
 592             if (n > num_objects)
 593                 num_objects = n;
 594         }
 595
 596         if (num_objects != 0)
 597         {
 598             std::vector<Cv3dTracker2dTrackedObject> tracking_info(m_num_streams * num_objects,
 599                                                                   cv3dTracker2dTrackedObject(-1, CvPoint()));
 600             for (i = 0; i < m_num_streams; i++)
 601             {
 602                 ITracker::TrackingInfo &t = m_streams[i].queue.front().tracking_info;
 603
 604                 if (m_streams[i].image_header2.origin == IPL_ORIGIN_BL)
 605                     for (int j = 0; j < t.size(); j++)
 606                         t[j].p.y = m_streams[i].image_header2.height - 1 - t[j].p.y;
 607
 608                 for (int j = 0; j < t.size(); j++)
 609                     tracking_info[i*num_objects+j] = t[j];
 610             }
 611
 612             m_tracked_objects.resize(num_objects);
 613                         //
 614                         //  This is it.  The whole point of this is this call.
 615                         //  For each matched set of results from the trackers this call will compute the 3D coordinates of the targets.
 616                         //
 617             int n = cv3dTrackerLocateObjects(m_num_streams, num_objects, m_pCameraInfo, &tracking_info[0], &m_tracked_objects[0]);
 618             m_tracked_objects.resize(n);
 619         }
 620         else
 621             m_tracked_objects.clear();
 622     }
 623
 624     // Copy viewing sample out of queue before releasing lock.
 625     Stream::Sample viewing_sample = m_streams[m_viewing_stream].queue.front();
 626
 627     for (i = 0; i < m_num_streams; i++)
 628     {
 629         m_streams[i].output_pin->Deliver(m_streams[i].queue.front().sample);
 630         m_streams[i].queue.pop_front();
 631     }
 632
 633     lock.Unlock();
 634
 635     for (i = 0; i < m_callbacks.size(); i++)
 636         m_callbacks[i]->Callback(m_tracked_objects, viewing_sample.data, (IUnknown *)viewing_sample.sample);
 637
 638     return NOERROR;
 639 }
 640
 641 HRESULT Tracker3dFilter::EndOfStream(int pin)
 642 {
 643     AutoLock lock(&m_recv_cs);
 644
 645     m_end_of_stream_count++;
 646
 647     if (m_end_of_stream_count == m_num_streams)
 648     {
 649         for (int i = 0; i < m_num_streams; i++)
 650         {
 651             Flush(i);
 652             m_streams[i].output_pin->DeliverEndOfStream();
 653         }
 654     }
 655
 656     return NOERROR;
 657 }
 658
 659 void Tracker3dFilter::SetNumberOfStreams(int num_streams)
 660 {
 661     if (num_streams == m_num_streams)
 662         return;
 663
 664     delete [] m_camera_intrinsics;
 665     m_camera_intrinsics = NULL;
 666
 667     Stream *streams = NULL;
 668     Cv3dTrackerCameraInfo *camera_info = NULL;
 669
 670     if (num_streams > 0)
 671     {
 672         int i;
 673
 674         camera_info = new Cv3dTrackerCameraInfo [ num_streams ];
 675         streams = new Stream [ num_streams ];
 676         for (i = 0; i < num_streams; i++)
 677         {
 678             if (i < m_num_streams)
 679             {
 680                 streams[i] = m_streams[i];
 681                 camera_info[i] = m_pCameraInfo[i];
 682             }
 683             else
 684             {
 685                 streams[i].input_pin = new Tracker3dInputPin(i, this, &m_cs);
 686                 streams[i].output_pin = new Tracker3dOutputPin(i, this, &m_cs);
 687                 if (m_tracker_clsid != GUID_NULL)
 688                 {
 689                     // If CoCreateInstance fails, m_streams[i].tracker is left as NULL.
 690                     CoCreateInstance(m_tracker_clsid, NULL, CLSCTX_INPROC_SERVER, IID_ITracker, (void **)&streams[i].tracker);
 691                 }
 692                 camera_info[i].valid = false;
 693             }
 694         }
 695     }
 696
 697     delete [] m_streams;
 698     m_streams = streams;
 699
 700     delete [] m_pCameraInfo;
 701     m_pCameraInfo = camera_info;
 702
 703     m_num_streams = num_streams;
 704     IncrementPinVersion();
 705 }
 706
 707
 708 //
 709 // GetPages
 710 //
 711 // Returns the clsid's of the property pages we support
 712 //
 713 STDMETHODIMP Tracker3dFilter::GetPages(CAUUID *pPages)
 714 {
 715     std::vector<GUID> pages;
 716     pages.push_back(CLSID_Tracker3dPropertyPage);
 717
 718     // In addition to our property page, return a separate property page for each type of tracker.
 719     // Normally all the trackers will be the same, so only one property page will be returned,
 720     // but we call each tracker to see if it wants a different property page.
 721     // It is up to each property page to call GetTrackers and connect to all the trackers
 722     // that are of the appropriate type.
 723     if (m_streams != NULL)
 724     {
 725         for (int i = 0; i < m_num_streams; i++)
 726         {
 727             GUID page;
 728             if (m_streams[i].tracker != NULL
 729                 && SUCCEEDED(m_streams[i].tracker->GetPropertyPage(&page)))
 730             {
 731                 if (std::find(pages.begin(), pages.end(), page) == pages.end())
 732                     pages.push_back(page);
 733             }
 734         }
 735     }
 736
 737     // Now that we've determined how many pages we need, allocate the memory
 738     // and fill it in.
 739     pPages->cElems = pages.size();
 740     pPages->pElems = (GUID *) CoTaskMemAlloc(pages.size() * sizeof(GUID));
 741     if (pPages->pElems == NULL)
 742         return E_OUTOFMEMORY;
 743
 744     for (int i = 0; i < pages.size(); i++)
 745         pPages->pElems[i] = pages[i];
 746
 747     return NOERROR;
 748 }
 749
 750
 751 // IPersistStream
 752 STDMETHODIMP Tracker3dFilter::GetClassID(CLSID *clsid)
 753 {
 754     *clsid = CLSID_Tracker3dFilter;
 755     return NOERROR;
 756 }
 757
 758 STDMETHODIMP Tracker3dFilter::IsDirty()
 759 {
 760     return S_OK;
 761 }
 762
 763 static const int PERSIST_STREAM_VERSION0 = 0;
 764 static const int PERSIST_STREAM_VERSION0_SIZE = 3;
 765 static const int PERSIST_STREAM_VERSION1 = 1;
 766 static const int PERSIST_STREAM_VERSION1_SIZE = 4;
 767 static const int PERSIST_STREAM_VERSION2 = 2;
 768 static const int PERSIST_STREAM_VERSION2_SIZE = PERSIST_STREAM_VERSION1_SIZE;
 769 static const int PERSIST_STREAM_VERSION3 = 3;
 770 static const int PERSIST_STREAM_VERSION3_SIZE = 3 + sizeof(GUID);
 771
 772 STDMETHODIMP Tracker3dFilter::Load(IStream *stream)
 773 {
 774     unsigned long nread;
 775     unsigned char buf[3];
 776
 777     HRESULT hr = stream->Read(buf, 3, &nread);
 778     if (FAILED(hr))
 779         return hr;
 780
 781     if (nread != 3)
 782         return E_FAIL;
 783
 784     int num_streams;
 785
 786     if (buf[0] == PERSIST_STREAM_VERSION0 && buf[1] == PERSIST_STREAM_VERSION0_SIZE)
 787     {
 788         num_streams = buf[2];
 789     }
 790     else if (buf[0] == PERSIST_STREAM_VERSION1 && buf[1] == PERSIST_STREAM_VERSION1_SIZE)
 791     {
 792         num_streams = buf[2];
 793     }
 794     else if (buf[0] == PERSIST_STREAM_VERSION2 && buf[1] == PERSIST_STREAM_VERSION2_SIZE)
 795     {
 796         num_streams = buf[2] & 0x3f;
 797         m_preferred_size = (InputSize)((buf[2] >> 6) & 0x03);
 798     }
 799     else if (buf[0] == PERSIST_STREAM_VERSION3)
 800     {
 801         num_streams = buf[2] & 0x3f;
 802         m_preferred_size = (InputSize)((buf[2] >> 6) & 0x03);
 803         if (buf[1] == PERSIST_STREAM_VERSION3_SIZE)
 804         {
 805             GUID guid;
 806             hr = stream->Read(&guid, sizeof(GUID), &nread);
 807             if (FAILED(hr))
 808                 return hr;
 809             if (nread != sizeof(GUID))
 810                 return E_FAIL;
 811             m_tracker_clsid = guid;
 812         }
 813     }
 814     else
 815     {
 816         return E_FAIL;
 817     }
 818
 819     if (buf[0] == PERSIST_STREAM_VERSION1 || buf[0] == PERSIST_STREAM_VERSION2)
 820     {
 821         unsigned char lib_name_len;
 822
 823         hr = stream->Read(&lib_name_len, 1, &nread);
 824         if (FAILED(hr))
 825             return hr;
 826         if (nread != 1)
 827             return E_FAIL;
 828
 829         if (lib_name_len > 0)
 830         {
 831             char lib_name[256];
 832             hr = stream->Read(lib_name, lib_name_len, &nread);
 833             // We don't support this format any more; ignore the
 834             // default tracker dll name, but allow the graph
 835             // to load.
 836             //if (SUCCEEDED(hr) && nread == lib_name_len)
 837             //{
 838             //    lib_name[lib_name_len] = '\0';
 839             //    SetDefaultTrackerDLL(lib_name);
 840             //}
 841         }
 842     }
 843
 844     SetTrackers(num_streams, NULL);
 845
 846     return NOERROR;
 847 }
 848
 849 STDMETHODIMP Tracker3dFilter::Save(IStream *stream, BOOL clear_dirty)
 850 {
 851     HRESULT hr;
 852     unsigned long dummy;
 853     unsigned char buf[PERSIST_STREAM_VERSION3_SIZE] = { PERSIST_STREAM_VERSION3,
 854                                                         PERSIST_STREAM_VERSION3_SIZE,
 855                                                         (unsigned char)((m_num_streams & 0x7f) | (m_preferred_size << 6))
 856                                                       };
 857
 858     if (m_tracker_clsid != GUID_NULL)
 859         memcpy(buf+3, &m_tracker_clsid, sizeof(GUID));
 860     else
 861         buf[1] = 3;
 862
 863     hr = stream->Write(&buf, buf[1], &dummy);
 864     if (FAILED(hr))
 865         return hr;
 866
 867     return NOERROR;
 868 }
 869
 870 STDMETHODIMP Tracker3dFilter::GetSizeMax(ULARGE_INTEGER *size)
 871 {
 872     size->QuadPart = PERSIST_STREAM_VERSION3_SIZE;
 873     return NOERROR;
 874 }
 875
 876
 877 // ITracker3dFilter
 878
 879 STDMETHODIMP Tracker3dFilter::GetNumberOfCameras(int &num_streams)
 880 {
 881     num_streams = m_num_streams;
 882     return NOERROR;
 883 }
 884
 885 class AutoClose
 886 {
 887     FILE ** m_ppfile;
 888
 889 public:
 890     AutoClose(FILE ** ppfile) : m_ppfile(ppfile) { };
 891     ~AutoClose()
 892     {
 893         if (*m_ppfile != NULL)
 894         {
 895             fclose(*m_ppfile);
 896             *m_ppfile = NULL;
 897         }
 898     };
 899 };
 900
 901 #define AUTO_CLOSE(f) AutoClose AutoClose##f(&f)
 902
 903 STDMETHODIMP Tracker3dFilter::LoadCameraConfiguration(const char *filename)
 904 {
 905     FILE *f = fopen(filename, "r"); AUTO_CLOSE(f);
 906     if (f == NULL)
 907         return E_FAIL;
 908
 909     int num_streams;
 910     int r;
 911     r = fscanf(f, "%ld\n", &num_streams);
 912     if (r != 1 || num_streams <= 0)
 913         return E_FAIL;
 914
 915     SetNumberOfStreams(num_streams);
 916
 917     for (int c = 0; c < m_num_streams; c++)
 918     {
 919         Cv3dTrackerCameraInfo &camera = m_pCameraInfo[c];
 920
 921         char camera_name[80];
 922
 923         if (fscanf(f, "%[^:]:", camera_name) != 1)
 924             return E_FAIL;
 925         if (fscanf(f, "%g, %g;", &camera.principal_point.x, &camera.principal_point.y) != 2)
 926             return E_FAIL;
 927         for (int i = 0; i < 4; i++)
 928             for (int j = 0; j < 4; j++)
 929                 if (fscanf(f, "%g", &camera.mat[i][j]) != 1)
 930                     return E_FAIL;
 931         //camera.camera_name = camera_name;
 932         camera.valid = true;
 933     }
 934
 935     m_camera_configuration_loaded = true;
 936
 937     return NOERROR;
 938 }
 939
 940 STDMETHODIMP Tracker3dFilter::SaveCameraConfiguration(const char *filename)
 941 {
 942     if (m_pCameraInfo == NULL)
 943         return E_FAIL;
 944
 945     FILE *f = fopen(filename, "w");
 946     if (f == NULL)
 947         return E_FAIL;
 948
 949     fprintf(f, "%lu\n", m_num_streams);
 950
 951     for (int c = 0; c < m_num_streams; c++)
 952     {
 953         Cv3dTrackerCameraInfo &camera = m_pCameraInfo[c];
 954
 955         fprintf(f, "Camera %d: ", c);
 956         //fprintf(f, "%s: ", camera.camera_name.c_str());
 957         fprintf(f, " %g, %g;", camera.principal_point.x, camera.principal_point.y);
 958         for (int i = 0; i < 4; i++)
 959             for (int j = 0; j < 4; j++)
 960                 fprintf(f, " %g", camera.mat[i][j]);
 961         fprintf(f, "\n");
 962     }
 963
 964     fclose(f);
 965
 966     return NOERROR;
 967 }
 968
 969 static std::string subst(std::string s, char i, char o)
 970 {
 971     std::string::size_type pos = 0;
 972     while ((pos = s.find(i, pos)) != std::string::npos)
 973         s.replace(pos, 1, 1, o);
 974     return s;
 975 }
 976
 977 std::string Tracker3dFilter::CalibrationName(int i)
 978 {
 979     std::string camera_name;
 980     GetCameraName(i, camera_name);
 981     return subst(camera_name, '\\', '#');
 982 }
 983
 984 class AutoCloseKey
 985 {
 986     HKEY *m_pkey;
 987
 988 public:
 989     AutoCloseKey(HKEY *pkey) : m_pkey(pkey) { };
 990     ~AutoCloseKey()
 991     {
 992         if (*m_pkey != 0)
 993         {
 994             RegCloseKey(*m_pkey);
 995             *m_pkey = 0;
 996         }
 997     };
 998 };
 999
1000 #define AUTO_CLOSE_KEY(f) AutoCloseKey AutoCloseKey##f(&f)
1001
1002 static const char key_name[] = "Software\\Intel\\VAI\\3d Tracker\\";
1003
1004 //------------------
1005 // Once camera extrinsic parameters are determined they are stored in the registry in the key
1006 // Software\\Intel\\VAI\\3d Tracker\\
1007 // Read them back from here when the app starts.
1008 //------------------
1009 void Tracker3dFilter::LoadCameraConfiguration()
1010 {
1011     LONG r;
1012
1013     HKEY key = 0; AUTO_CLOSE_KEY(key);
1014     r = RegOpenKeyEx(HKEY_LOCAL_MACHINE, key_name, 0,KEY_READ, &key);
1015     if (r != ERROR_SUCCESS)
1016         return;
1017
1018     int valid_count = 0;
1019     for (int c = 0; c < m_num_streams; c++)
1020     {
1021         Cv3dTrackerCameraInfo &camera = m_pCameraInfo[c];
1022         char buf[18*15];
1023         DWORD size = sizeof(buf);
1024         r = RegQueryValueEx(key, CalibrationName(c).c_str(), 0, NULL, (BYTE *)buf, &size);
1025         if (r != ERROR_SUCCESS)
1026             continue;
1027
1028         if (sscanf(buf, "%g %g; %g %g %g %g, %g %g %g %g, %g %g %g %g, %g %g %g %g",
1029                         &camera.principal_point.x, &camera.principal_point.y,
1030                         &camera.mat[0][0], &camera.mat[0][1], &camera.mat[0][2], &camera.mat[0][3],
1031                         &camera.mat[1][0], &camera.mat[1][1], &camera.mat[1][2], &camera.mat[1][3],
1032                         &camera.mat[2][0], &camera.mat[2][1], &camera.mat[2][2], &camera.mat[2][3],
1033                         &camera.mat[3][0], &camera.mat[3][1], &camera.mat[3][2], &camera.mat[3][3]) != 18)
1034             continue;
1035
1036         camera.valid = true;
1037         valid_count++;
1038     }
1039
1040     m_camera_configuration_loaded = (valid_count == m_num_streams);
1041 }
1042
1043 void Tracker3dFilter::SaveCameraConfiguration()
1044 {
1045     LONG r;
1046
1047     HKEY key = 0; AUTO_CLOSE_KEY(key);
1048     r = RegCreateKeyEx(HKEY_LOCAL_MACHINE, key_name, 0, NULL, 0, KEY_WRITE, NULL, &key, NULL);
1049     if (r != ERROR_SUCCESS)
1050         return;
1051
1052     for (int c = 0; c < m_num_streams; c++)
1053     {
1054         Cv3dTrackerCameraInfo &camera = m_pCameraInfo[c];
1055         char buf[18*15];
1056         char *p = buf;
1057         int len = sprintf(p, "%g %g;", camera.principal_point.x, camera.principal_point.y);
1058         p += len;
1059         for (int i = 0; i < 4; i++)
1060         {
1061             for (int j = 0; j < 4; j++)
1062             {
1063                 len = sprintf(p, " %g", camera.mat[i][j]);
1064                 p += len;
1065             }
1066             *p++ = ',';
1067         }
1068         RegSetValueEx(key, CalibrationName(c).c_str(), 0, REG_SZ, (BYTE *)buf, p - buf + 1);
1069     }
1070 }
1071
1072
1073 STDMETHODIMP Tracker3dFilter::GetDefaultTracker(GUID &tracker_clsid)
1074 {
1075     tracker_clsid = m_tracker_clsid;
1076     return NOERROR;
1077 }
1078
1079 STDMETHODIMP Tracker3dFilter::SetDefaultTracker(const GUID &tracker_clsid)
1080 {
1081     m_tracker_clsid = tracker_clsid;
1082     return NOERROR;
1083 }
1084
1085 STDMETHODIMP Tracker3dFilter::SetTrackers(int num_streams, ITracker * const *trackers)
1086 {
1087     SetNumberOfStreams(num_streams);
1088
1089     if (trackers == NULL)
1090     {
1091         for (int i = 0; i < m_num_streams; i++)
1092         {
1093             SAFE_RELEASE(m_streams[i].tracker);
1094
1095             if (m_tracker_clsid != GUID_NULL)
1096             {
1097                 // If CoCreateInstance fails, m_streams[i].tracker is left as NULL.
1098                 CoCreateInstance(m_tracker_clsid, NULL, CLSCTX_INPROC_SERVER, IID_ITracker, (void **)&m_streams[i].tracker);
1099                 if (m_streams[i].media_type.IsValid())
1100                     m_streams[i].tracker->SetFormat(&m_streams[i].image_header1);
1101             }
1102         }
1103     }
1104     else
1105     {
1106         for (int i = 0; i < m_num_streams; i++)
1107         {
1108             SAFE_RELEASE(m_streams[i].tracker);
1109
1110             // Store and addref new tracker
1111             m_streams[i].tracker = trackers[i];
1112             if (m_streams[i].tracker != NULL)
1113             {
1114                 m_streams[i].tracker->AddRef();
1115                 if (m_streams[i].media_type.IsValid())
1116                     m_streams[i].tracker->SetFormat(&m_streams[i].image_header1);
1117             }
1118         }
1119     }
1120
1121     return NOERROR;
1122 }
1123
1124
1125 STDMETHODIMP Tracker3dFilter::GetTrackers(std::vector<ITracker *> &trackers)
1126 {
1127     trackers.resize(m_num_streams);
1128     for (int i = 0; i < m_num_streams; i++)
1129     {
1130         trackers[i] = m_streams[i].tracker;
1131         if (trackers[i] != NULL)
1132             trackers[i]->AddRef();
1133     }
1134     return NOERROR;
1135 }
1136
1137
1138 //----------------------
1139 // Read the intrinsics information for all of the from a list of files.
1140 // The following is an example of the format that the file is reading.
1141 // The top left 2 diagonal elements of the matrix are the horizontal and
1142 // vertical focal lengths and the top 2 elements in the last column
1143 // indicate the image center coordinates, x in the top row, y in the second.
1144 //
1145 // The Distortion parameters indicate the radial and barrel distortion of the lens.
1146 //
1147 // All of these results are further discussed in the documentation concerning
1148 // the CalibFilter.
1149 //----------------------
1150
1151 /*
1152 Camera Matrix:
1153 M[0.0]=         401.3238525M[0.1]=           0.0000000M[0.2]=         135.9540710
1154 M[1.0]=           0.0000000M[1.1]=         403.9630737M[1.2]=         116.6698456
1155 M[2.0]=           0.0000000M[2.1]=           0.0000000M[2.2]=           1.0000000
1156
1157
1158 Distortion:
1159 D[0]=-0.011825
1160 D[1]=0.241698
1161 D[2]=0.001189
1162 D[3]=0.003923
1163 */
1164
1165 HRESULT Tracker3dFilter::ReadCameraIntrinsics(const char *filenames[])
1166 {
1167     // read camera intrinsics for all cameras: focal length[2], principal_point.x&y, distortion[4]
1168     if (m_camera_intrinsics == NULL)
1169         m_camera_intrinsics = new Cv3dTrackerCameraIntrinsics [ m_num_streams ];
1170
1171     for (int c = 0; c < m_num_streams; c++)
1172     {
1173         Cv3dTrackerCameraIntrinsics &camera = m_camera_intrinsics[c];
1174
1175         FILE *file = fopen(filenames[c], "r"); AUTO_CLOSE(file);
1176         if (file == NULL)
1177             return E_FAIL;
1178
1179 #define BUF_SIZE 500
1180         char buffer[BUF_SIZE+1];
1181         int sz = fread( buffer, 1, BUF_SIZE, file );
1182         buffer[sz] = '\0';
1183
1184         int i, j, k;
1185         float camera_matrix[3][3];
1186         char* ptr = buffer;
1187         /* read matrix */
1188         for( k = 0; k < 9; k++ )
1189         {
1190             ptr = strstr( ptr, "M[" );
1191             if( ptr )
1192             {
1193                 int s = 0;
1194                 ptr += 2;
1195                 if( sscanf( ptr, "%d%*[.,]%d%n", &i, &j, &s ) == 2 && i == k/3 && j == k%3 )
1196                 {
1197                     ptr += s;
1198                     ptr = strstr( ptr, "=" );
1199                     if( ptr )
1200                     {
1201                         s = 0;
1202                         ptr++;
1203                         if( sscanf( ptr, "%f%n", &camera_matrix[i][j], &s ) == 1 )
1204                         {
1205                             ptr += s;
1206                             continue;
1207                         }
1208                     }
1209                 }
1210             }
1211
1212             return E_FAIL;
1213         }
1214
1215         camera.focal_length[0] = camera_matrix[0][0];
1216         camera.focal_length[1] = camera_matrix[1][1];
1217
1218         camera.principal_point.x = camera_matrix[0][2];
1219         camera.principal_point.y = camera_matrix[1][2];
1220
1221         /* read distortion */
1222         for( k = 0; k < 4; k++ )
1223         {
1224             ptr = strstr( ptr, "D[" );
1225             if( ptr )
1226             {
1227                 int s = 0;
1228                 ptr += 2;
1229                 if( sscanf( ptr, "%d%n", &i, &s ) == 1 && i == k )
1230                 {
1231                     ptr += s;
1232                     ptr = strstr( ptr, "=" );
1233                     if( ptr )
1234                     {
1235                         s = 0;
1236                         ptr++;
1237                         if( sscanf( ptr, "%f%n", &camera.distortion[k], &s ) == 1 )
1238                         {
1239                             ptr += s;
1240                             continue;
1241                         }
1242                     }
1243                 }
1244             }
1245
1246             return E_FAIL;
1247         }
1248     }
1249
1250     return NOERROR;
1251 }
1252
1253
1254 //---------------------
1255 // Initiate the camera calibration process.  Actual calibration call is made in Receive()
1256 //---------------------
1257 STDMETHODIMP Tracker3dFilter::CalibrateCameras(int checkerboard_width, int checkerboard_height,
1258                                                const char *camera_intrinsics_filenames[],
1259                                                float square_size,
1260                                                bool continuous)
1261 {
1262     AutoLock lock(&m_recv_cs);
1263     HRESULT hr = ReadCameraIntrinsics(camera_intrinsics_filenames);
1264     if (FAILED(hr))
1265         return hr;
1266     m_tracked_objects.clear();
1267     m_etalon_size = cvSize(checkerboard_width-1, checkerboard_height-1);
1268     m_calibrate_cameras = continuous ? 2 : 1;
1269     m_square_size = square_size;
1270     for (int c = 0; c < m_num_streams; c++)
1271         m_pCameraInfo[c].valid = false;
1272     return NOERROR;
1273 }
1274
1275
1276 STDMETHODIMP Tracker3dFilter::GetTrackedObjects(std::vector<Cv3dTrackerTrackedObject> &tracked_objects)
1277 {
1278     AutoLock lock(&m_recv_cs);
1279     tracked_objects = m_tracked_objects;
1280     return NOERROR;
1281 }
1282
1283 STDMETHODIMP Tracker3dFilter::AddCallback(ITracker3dCallback *new_callback)
1284 {
1285     if (new_callback == NULL)
1286         return E_INVALIDARG;
1287
1288     new_callback->AddRef();
1289     m_callbacks.push_back(new_callback);
1290
1291     return NOERROR;
1292 }
1293
1294
1295 STDMETHODIMP Tracker3dFilter::RemoveCallback(ITracker3dCallback *callback)
1296 {
1297     if (callback == NULL)
1298         return E_INVALIDARG;
1299
1300     std::vector<ITracker3dCallback *>::iterator i = std::find(m_callbacks.begin(), m_callbacks.end(), callback);
1301     if (i != m_callbacks.end())
1302     {
1303         m_callbacks.erase(i);
1304         callback->Release();
1305     }
1306
1307     return NOERROR;
1308 }
1309
1310
1311
1312 STDMETHODIMP Tracker3dFilter::SetViewingStream(int stream)
1313 {
1314     if (stream < 0 || stream >= m_num_streams)
1315         return E_INVALIDARG;
1316
1317     m_viewing_stream = stream;
1318     return NOERROR;
1319 }
1320
1321 STDMETHODIMP Tracker3dFilter::GetViewingStream(int &stream)
1322 {
1323     stream = m_viewing_stream;
1324     return NOERROR;
1325 }
1326
1327 //--------------------
1328 // Begin at input pin 'i' and work upstream in the graph to find the source filter.  Return
1329 // the name of the source in the string name.
1330 //--------------------
1331 STDMETHODIMP Tracker3dFilter::GetCameraName(int i, std::string &name)
1332 {
1333     // Set up a default, in case we can't generate something better
1334     // (This could be improved.)
1335     name = (char)('0'+i);
1336
1337     // Work upstream from input pin 'i' to find the capture filter
1338     HRESULT hr;
1339     IPin *input_pin = m_streams[i].input_pin; input_pin->AddRef(); AUTO_RELEASE(input_pin);
1340     IBaseFilter *capture_filter = NULL; AUTO_RELEASE(capture_filter);
1341
1342     while (1)
1343     {
1344         IPin *output_pin = NULL; AUTO_RELEASE(output_pin);
1345         hr = input_pin->ConnectedTo(&output_pin);
1346         if (FAILED(hr))
1347             return hr;
1348
1349         PIN_INFO pin_info;
1350         hr = output_pin->QueryPinInfo(&pin_info);
1351         if (FAILED(hr))
1352             return hr;
1353         IBaseFilter *filter = pin_info.pFilter; AUTO_RELEASE(filter);
1354
1355         FILTER_INFO filter_info;
1356         hr = filter->QueryFilterInfo(&filter_info);
1357         filter_info.pGraph->Release(); filter_info.pGraph = NULL;
1358
1359         IEnumPins *e = NULL; AUTO_RELEASE(e);
1360         hr = filter->EnumPins(&e);
1361         if (FAILED(hr))
1362             return hr;
1363
1364         input_pin->Release(); input_pin = NULL;
1365         while (e->Next(1, &input_pin, NULL) == S_OK)
1366         {
1367             hr = input_pin->QueryPinInfo(&pin_info);
1368             if (FAILED(hr))
1369                 return hr;
1370             pin_info.pFilter->Release();
1371             if (pin_info.dir == PINDIR_INPUT)
1372                 break;
1373             input_pin->Release(); input_pin = NULL;
1374                 }
1375
1376         if (input_pin == NULL) // No input pin found; this must be the capture filter
1377         {
1378             capture_filter = filter;
1379             capture_filter->AddRef();
1380             break;
1381         }
1382     }
1383
1384     // Use IPersistStream to save the filter data, which includes a text representation of the device id.
1385     IPersistStream *persist_stream = NULL; AUTO_RELEASE(persist_stream);
1386     hr = capture_filter->QueryInterface(IID_IPersistStream, (void **)&persist_stream);
1387     if (FAILED(hr))
1388         return hr;
1389
1390     HGLOBAL mem = GlobalAlloc(GMEM_MOVEABLE, 0);
1391     if (mem == 0)
1392         return E_OUTOFMEMORY;
1393
1394     IStream *stream = NULL; AUTO_RELEASE(stream);
1395     CreateStreamOnHGlobal(mem, true, &stream);
1396     hr = persist_stream->Save(stream, false);
1397     if (FAILED(hr))
1398         return hr;
1399
1400     STATSTG stat;
1401     hr = stream->Stat(&stat, STATFLAG_NONAME);
1402     if (FAILED(hr))
1403         return hr;
1404
1405     int size = stat.cbSize.LowPart;
1406
1407     wchar_t *p = (wchar_t *)GlobalLock(mem);
1408     wchar_t *end = p + size;
1409     wchar_t *id = NULL;
1410     while (p < end-8)
1411     {
1412         if (wcsncmp(p, L"@device:", 8) == 0)
1413         {
1414             id = p;
1415             break;
1416         }
1417         p++;
1418     }
1419
1420     if (id == NULL)// Didn't find something we recognize
1421         return E_FAIL;
1422
1423     p = id + 8;
1424     while (p < end && *p != L'\0')
1425         p++;
1426     int len = p - id;
1427
1428     name.resize(len);
1429
1430     for (i = 0; i < len; i++)
1431         name[i] = (char)id[i];
1432
1433     return NOERROR;
1434 }
1435
1436 STDMETHODIMP Tracker3dFilter::SetPreferredInputSize(InputSize size)
1437 {
1438     m_preferred_size = size;
1439     return NOERROR;
1440 }
1441
1442 STDMETHODIMP Tracker3dFilter::GetPreferredInputSize(InputSize &size)
1443 {
1444     size = m_preferred_size;
1445     return NOERROR;
1446 }
1447
1448 STDMETHODIMP Tracker3dFilter::IsConnected(bool &any_connected, bool &all_connected)
1449 {
1450     any_connected = false;
1451     all_connected = true;
1452     for (int i = 0; i < m_num_streams; i++)
1453     {
1454         if (m_streams[i].input_pin->IsConnected())
1455             any_connected = true;
1456         else
1457             all_connected = false;
1458
1459         if (m_streams[i].output_pin->IsConnected())
1460             any_connected = true;
1461         else
1462             all_connected = false;
1463     }
1464     return NOERROR;
1465 }
1466
1467 // ITracker3dInternal
1468 STDMETHODIMP Tracker3dFilter::GetCameraInfo(std::vector<Cv3dTrackerCameraInfo> &info)
1469 {
1470     info.resize(m_num_streams);
1471     for (int i = 0; i < m_num_streams; i++)
1472         info[i] = m_pCameraInfo[i];
1473     return NOERROR;
1474 }
1475
1476
1477 // Implementation of pins for Tracker 3d Filter
1478
1479 static const wchar_t *PinName(PIN_DIRECTION pin_dir, int pin_number)
1480 {
1481     static wchar_t buf[14];
1482     swprintf(buf, L"%s %d", pin_dir == PINDIR_INPUT ? L"Input" : L"Output", pin_number);
1483     return buf;
1484 }
1485
1486
1487 Tracker3dInputPin::Tracker3dInputPin(int pin_number, Tracker3dFilter *filter, CCritSec *cs)
1488     : CBaseInputPin("Tracker3dInputPin", filter, cs, NULL, PinName(PINDIR_INPUT, pin_number)),
1489       m_pin_number(pin_number),
1490       m_pFilter(filter),
1491       m_refcnt(1)
1492 {
1493 }
1494
1495 Tracker3dInputPin::~Tracker3dInputPin()
1496 {
1497 }
1498
1499 STDMETHODIMP_(ULONG) Tracker3dInputPin::NonDelegatingAddRef()
1500 {
1501     InterlockedIncrement(&m_refcnt);
1502     return CBaseInputPin::NonDelegatingAddRef();
1503 }
1504
1505 STDMETHODIMP_(ULONG) Tracker3dInputPin::NonDelegatingRelease()
1506 {
1507     if (InterlockedDecrement(&m_refcnt) == 0)
1508     {
1509         delete this;
1510         return 0;
1511     }
1512     return CBaseInputPin::NonDelegatingRelease();
1513 }
1514
1515 STDMETHODIMP Tracker3dInputPin::GetAllocator(IMemAllocator **allocator)
1516 {
1517     if (m_pAllocator == NULL)
1518         return VFW_E_NO_ALLOCATOR;
1519     *allocator = m_pAllocator;
1520     (*allocator)->AddRef();
1521     return NOERROR;
1522 }
1523
1524 STDMETHODIMP Tracker3dInputPin::NotifyAllocator(IMemAllocator *allocator, BOOL read_only)
1525 {
1526     ALLOCATOR_PROPERTIES props, actual;
1527     allocator->GetProperties(&props);
1528     props.cBuffers = 5;
1529     allocator->SetProperties(&props, &actual);
1530     return CBaseInputPin::NotifyAllocator(allocator, read_only);
1531 }
1532
1533 HRESULT Tracker3dInputPin::CheckMediaType(const CMediaType *mt)
1534 {
1535     return m_pFilter->CheckMediaType(m_pin_number, mt);
1536 }
1537
1538 HRESULT Tracker3dInputPin::SetMediaType(const CMediaType *mt)
1539 {
1540     CBaseInputPin::SetMediaType(mt);
1541     return m_pFilter->SetMediaType(m_pin_number, mt);
1542 }
1543
1544 HRESULT Tracker3dInputPin::GetMediaType(int pos, CMediaType *mt)
1545 {
1546     return m_pFilter->GetMediaType(m_pin_number, pos, mt);
1547 }
1548
1549
1550 HRESULT Tracker3dInputPin::Receive(IMediaSample *sample)
1551 {
1552     HRESULT hr = CBaseInputPin::Receive(sample);
1553     if (FAILED(hr))
1554         return hr;
1555
1556     return m_pFilter->Receive(m_pin_number, sample);
1557 }
1558
1559 HRESULT Tracker3dInputPin::EndOfStream()
1560 {
1561     m_pFilter->EndOfStream(m_pin_number);
1562     return NOERROR;
1563 }
1564
1565 HRESULT Tracker3dInputPin::BeginFlush()
1566 {
1567     HRESULT hr = CBaseInputPin::BeginFlush();
1568     if (FAILED(hr))
1569         return hr;
1570
1571     m_pFilter->Flush(m_pin_number);
1572
1573     return static_cast<Tracker3dOutputPin *>(m_pFilter->GetPin(m_pin_number*2+1))->DeliverBeginFlush();
1574 }
1575
1576 HRESULT Tracker3dInputPin::EndFlush()
1577 {
1578     HRESULT hr =  static_cast<Tracker3dOutputPin *>(m_pFilter->GetPin(m_pin_number*2+1))->DeliverEndFlush();
1579     if (FAILED(hr))
1580         return hr;
1581
1582     return CBaseInputPin::EndFlush();
1583 }
1584
1585 HRESULT Tracker3dInputPin::NewSegment(REFERENCE_TIME start, REFERENCE_TIME stop, double rate)
1586 {
1587     CBasePin::NewSegment(start, stop, rate);
1588     return static_cast<Tracker3dOutputPin *>(m_pFilter->GetPin(m_pin_number*2+1))->DeliverNewSegment(start, stop, rate);
1589 }
1590
1591
1592 Tracker3dOutputPin::Tracker3dOutputPin(int pin_number, Tracker3dFilter *filter, CCritSec *cs)
1593     : CBaseOutputPin("Tracker3dOutputPin", filter, cs, NULL, PinName(PINDIR_OUTPUT, pin_number)),
1594       m_pin_number(pin_number),
1595       m_pFilter(filter),
1596       m_refcnt(1)
1597 {
1598 }
1599
1600 Tracker3dOutputPin::~Tracker3dOutputPin()
1601 {
1602 }
1603
1604 STDMETHODIMP_(ULONG) Tracker3dOutputPin::NonDelegatingAddRef()
1605 {
1606     InterlockedIncrement(&m_refcnt);
1607     return CBaseOutputPin::NonDelegatingAddRef();
1608 }
1609
1610 STDMETHODIMP_(ULONG) Tracker3dOutputPin::NonDelegatingRelease()
1611 {
1612     if (InterlockedDecrement(&m_refcnt) == 0)
1613     {
1614         delete this;
1615         return 0;
1616     }
1617     return CBaseOutputPin::NonDelegatingRelease();
1618 }
1619
1620
1621 HRESULT Tracker3dOutputPin::CheckMediaType(const CMediaType *mt)
1622 {
1623     return m_pFilter->CheckMediaType(m_pin_number, mt);
1624 }
1625
1626 HRESULT Tracker3dOutputPin::SetMediaType(const CMediaType *mt)
1627 {
1628     CBaseOutputPin::SetMediaType(mt);
1629     return m_pFilter->SetMediaType(m_pin_number, mt);
1630 }
1631
1632 HRESULT Tracker3dOutputPin::GetMediaType(int pos, CMediaType *mt)
1633 {
1634     return m_pFilter->GetMediaType(m_pin_number, pos, mt);
1635 }
1636
1637 HRESULT Tracker3dOutputPin::DecideAllocator(IMemInputPin *pPin, IMemAllocator **ppAlloc)
1638 {
1639     *ppAlloc = NULL;
1640
1641     Tracker3dInputPin *my_input_pin = static_cast<Tracker3dInputPin *>(m_pFilter->GetPin(m_pin_number*2));
1642     IMemAllocator *alloc;
1643     HRESULT hr = my_input_pin->GetAllocator(&alloc);
1644     if (FAILED(hr))
1645         return hr;
1646
1647     // get downstream prop request
1648     ALLOCATOR_PROPERTIES props, request;
1649     alloc->GetProperties(&props);
1650     if (SUCCEEDED(pPin->GetAllocatorRequirements(&request)))
1651     {
1652         bool changed = false;
1653         if (request.cbAlign > props.cbAlign)
1654             props.cbAlign = request.cbAlign, changed = true;
1655         if (request.cBuffers > props.cBuffers)
1656             props.cBuffers = request.cBuffers, changed = true;
1657         if (request.cbBuffer > props.cbBuffer)
1658             props.cbBuffer = request.cbBuffer, changed = true;
1659         if (request.cbPrefix > props.cbPrefix)
1660             props.cbPrefix = request.cbPrefix, changed = true;
1661         if (changed)
1662         {
1663             ALLOCATOR_PROPERTIES actual;
1664             alloc->SetProperties(&props, &actual);
1665         }
1666     }
1667
1668     hr = pPin->NotifyAllocator(alloc, my_input_pin->IsReadOnly());
1669     if (FAILED(hr))
1670     {
1671         alloc->Release();
1672         return hr;
1673     }
1674
1675     *ppAlloc = alloc;
1676     return NOERROR;
1677 }
1678
1679
1680 // Setup information
1681
1682 const AMOVIESETUP_MEDIATYPE sudPinTypes =
1683 {
1684     &MEDIATYPE_Video,       // Major type
1685     &MEDIASUBTYPE_NULL      // Minor type
1686 };
1687
1688 const AMOVIESETUP_PIN sudpPins[] =
1689 {
1690     { L"Input",             // Pins string name
1691       FALSE,                // Is it rendered
1692       FALSE,                // Is it an output
1693       TRUE,                 // Are we allowed none
1694       TRUE,                 // And allowed many
1695       &CLSID_NULL,          // Connects to filter
1696       NULL,                 // Connects to pin
1697       1,                    // Number of types
1698       &sudPinTypes          // Pin information
1699     },
1700     { L"Output",            // Pins string name
1701       FALSE,                // Is it rendered
1702       TRUE,                 // Is it an output
1703       TRUE,                 // Are we allowed none
1704       TRUE,                 // And allowed many
1705       &CLSID_NULL,          // Connects to filter
1706       NULL,                 // Connects to pin
1707       1,                    // Number of types
1708       &sudPinTypes          // Pin information
1709     }
1710 };
1711
1712 const AMOVIESETUP_FILTER sudFilter =
1713 {
1714     &CLSID_Tracker3dFilter, // Filter CLSID
1715     L"3d Tracker",          // String name
1716     MERIT_DO_NOT_USE,       // Filter merit
1717     2,                      // Number of pins
1718     sudpPins                // Pin information
1719 };
1720
1721
1722 // List of class IDs and creator functions for the class factory. This
1723 // provides the link between the OLE entry point in the DLL and an object
1724 // being created. The class factory will call the static CreateInstance
1725
1726 CFactoryTemplate g_Templates[] = {
1727     { L"3d Tracker"
1728     , &CLSID_Tracker3dFilter
1729     , Tracker3dFilter::CreateInstance
1730     , NULL
1731     , &sudFilter }
1732   ,
1733     { L"3d Tracker Property Page"
1734     , &CLSID_Tracker3dPropertyPage
1735     , Tracker3dPropertyPage::CreateInstance }
1736 };
1737 int g_cTemplates = sizeof(g_Templates) / sizeof(g_Templates[0]);
1738
1739
1740 //
1741 // DllRegisterServer
1742 //
1743 // Handles sample registry and unregistry
1744 //
1745 STDAPI DllRegisterServer()
1746 {
1747     return AMovieDllRegisterServer2( TRUE );
1748
1749 } // DllRegisterServer
1750
1751
1752 //
1753 // DllUnregisterServer
1754 //
1755 STDAPI DllUnregisterServer()
1756 {
1757     return AMovieDllRegisterServer2( FALSE );
1758
1759 } // DllUnregisterServer