VideoSource_DCAM_BumbleBee.cpp

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "VideoSource_DCAM_BumbleBee.hh"

#ifdef WIN32

#include <wtypes.h>
#include <1394Camera.h>
#include <Base/Debug/TimeMeasure.hh>
#include <Base/Image/ImageConvert.hh>

#else

#include <Base/Image/ImageIO.hh>
#include <dc1394/control.h>

#include "sys/time.h"
#include "unistd.h"
#include <sys/stat.h>

#endif // WIN32

#define WIDTH 1280
#define HEIGHT 960
#define WIDTH_S 640
#define HEIGHT_S 480

using namespace BIAS;
using namespace std;


VideoSource_DCAM_BumbleBee::VideoSource_DCAM_BumbleBee(int Card): VideoSource_DCAM(Card)
{
  stereo_ = false;
}

int VideoSource_DCAM_BumbleBee::SetStereoDefaultMode(CameraInfo *camInfo) {
  if (camInfo != NULL) {
#ifdef WIN32
      // fkellner: todo parameter ignored in DCAM CMU, so lookup makes no sense
      LARGE_INTEGER li;
      CamModel_ = GetModelForEuid(li);
#else
    CamModel_ = GetModelForEuid(camInfo->guid);
#endif
  } else {
    CamModel_ = CAMERA_PTGREY_BUMBLEBEE_XB3;
  }
  switch (CamModel_) {
    case CAMERA_PTGREY_BUMBLEBEE_XB3:
      SetFormat7(3,7680,DC1394_COLOR_CODING_RGB8);
      SetLeftTop(0,0);
      SetSize(1280,960,3);
      SetColorModel(ImageBase::CM_PGR_XB3_F7M3_GBRG);
      break;
    case CAMERA_PTGREY_BUMBLEBEE_XB2:
      SetModeAndFramerate(DC1394_VIDEO_MODE_1024x768_MONO16, 15.0f);
      // TODO this doesnt work so well (camera reports that no format 7 is supported)
//      SetFormat7(3,3264,DC1394_COLOR_CODING_RAW16);
//      SetLeftTop(0,0);
//      SetSize(1024,768,2);
//      SetColorModel(ImageBase::CM_Grey);
      break;
    default:
      cout << "camera is not a bumblebee" << endl;
      return 1;
  }
  stereo_ = true;
  return 0;
}

int VideoSource_DCAM_BumbleBee::InitImage(ImageBase &Image){
  if (!Image.IsEmpty())
    BIASERR("VideoSource::InitImage() should be called with uninitialized image");

  if (stereo_) {
    switch (CamModel_) {
      case CAMERA_PTGREY_BUMBLEBEE_XB3:
        Image.Init(Width_, Height_, 3, ImageBase::ST_unsignedchar);
        Image.SetColorModel(ImageBase::CM_PGR_XB3_F7M3_GBRG);
        break;
      case CAMERA_PTGREY_BUMBLEBEE_XB2:
        Image.Init(Width_, Height_, 2, ImageBase::ST_unsignedchar);
        // todo there is no mode for converting this for proper display
        Image.SetColorModel(ImageBase::CM_Grey);
        break;
      default:
        BIASERR("camera is not a bumblebee");
    }
  } else {
    InitSingleImage(Image);
  }
  return 0;
}

int VideoSource_DCAM_BumbleBee::InitSingleImage(ImageBase &Image){
  if (!Image.IsEmpty()) {
    BIASERR("VideoSource_DCAM_BumbleBee::InitSingleImage() should be called with uninitialized image");
    Image.Release();
  }
  Image.Init(Width_, Height_, 1, ImageBase::ST_unsignedchar);
  switch (CamModel_) {
    case CAMERA_PTGREY_BUMBLEBEE_XB3:
      Image.SetColorModel(ImageBase::CM_Bayer_GBRG);
      break;
    case CAMERA_PTGREY_BUMBLEBEE_XB2:
      Image.SetColorModel(ImageBase::CM_Bayer_BGGR);
      break;
    default:
      BIASERR("camera is not a bumblebee");
  }
  return 0;
}

int VideoSource_DCAM_BumbleBee::GetNumImages() {
  switch (CamModel_) {
    case CAMERA_PTGREY_BUMBLEBEE_XB3:
      return 3;
    case CAMERA_PTGREY_BUMBLEBEE_XB2:
      return 2;
    default:
      BIASERR("camera is not a bumblebee");
      return -1;
  }
}

int VideoSource_DCAM_BumbleBee::GrabSingle(Camera <unsigned char> &allImages,
                                     Camera <unsigned char> &imageLeft,
                                     Camera <unsigned char> &imageMiddle,
                                     Camera <unsigned char> &imageRight)
{

  if (!stereo_) {
    return VideoSource_DCAM::GrabSingle(allImages);
  }
  switch (CamModel_) {
    case CAMERA_PTGREY_BUMBLEBEE_XB3: {
      
      // author: djung

      // fetch complete image from cam
      VideoSource_DCAM::GrabSingle(allImages);
      
      // create 3 images out of source image
      unsigned char* grabbedPixels = allImages.GetImageData();
      unsigned char* sinkPixels = NULL;
      bool skip;
    
      for(unsigned int j=0; j<3; j++)
      {
        switch(j)
        {
          case 0:
            if(!imageRight.IsEmpty())
            {
              sinkPixels = imageRight.GetImageData();
              skip = false;
            }
            else
            {
    //          cout<<"\tEmpty image given: skip right image"<<endl;
              skip = true;
            }
            break;
          case 1:
            if(!imageMiddle.IsEmpty())
            {
              sinkPixels = imageMiddle.GetImageData();
              skip = false;
            }
            else
            {
    //          cout<<"\tEmpty image given: skip middle image"<<endl;
              skip = true;
            }
            break;
          case 2:
            if(!imageLeft.IsEmpty())
            {
              sinkPixels = imageLeft.GetImageData();
              skip = false;
            }
            else
            {
    //          cout<<"\tEmpty image given: skip left image"<<endl;
              skip = true;
            }
            break;
          default:
            skip = true;
            break;
        }
        if(!skip)
        {
          int
            size = WIDTH*HEIGHT;
          for(int index = 0; index < size; index++)
          {
            sinkPixels[index] = grabbedPixels[index*3 + j];
          }
        }
      }
      unsigned long long sec, usec;
      allImages.GetTime(sec, usec);
      imageLeft.SetTime(sec, usec);
      imageLeft.UpdateMetaData();
      imageMiddle.SetTime(sec, usec);
      imageMiddle.UpdateMetaData();
      imageRight.SetTime(sec, usec);
      imageRight.UpdateMetaData();
    }
    break;
    case CAMERA_PTGREY_BUMBLEBEE_XB2: {
      GrabSingle(allImages, imageLeft, imageRight);
    }
    break;
    default:
      BIASERR("camera is not a bumblebee");
      break;
  }
  return 0;
}


int VideoSource_DCAM_BumbleBee::GrabSingle(Camera <unsigned char> &allImages,
                                     Camera <unsigned char> &imageLeft,
                                     Camera <unsigned char> &imageRight)
{
  BIASASSERT(CamModel_ == CAMERA_PTGREY_BUMBLEBEE_XB2);

  VideoSource_DCAM::GrabSingle(allImages);
  
  BIASASSERT(!imageLeft.IsEmpty());
  BIASASSERT(!imageRight.IsEmpty());

  unsigned char *left = imageLeft.GetImageData();
  unsigned char *right = imageRight.GetImageData();
  unsigned char *all = allImages.GetImageData();
  
  for (unsigned int x=0;x<allImages.GetPixelCount();x++) {
    *right++ = *all++;
    *left++ = *all++;
  }
  unsigned long long sec, usec;
  allImages.GetTime(sec, usec);
  imageLeft.SetTime(sec, usec);
  imageLeft.UpdateMetaData();
  imageRight.SetTime(sec, usec);
  imageRight.UpdateMetaData();

  return 0;
}


int VideoSource_DCAM_BumbleBee::GrabSingleLRGrey(Camera <unsigned char> &allImages,
                                                 Camera <unsigned char> &imageLeft,
                                                 Camera <unsigned char> &imageRight)
{
  BIASASSERT(CamModel_ == CAMERA_PTGREY_BUMBLEBEE_XB3);

  BIASASSERT((imageLeft.GetHeight() == 480)&&(imageLeft.GetWidth() == 640));
  BIASASSERT((imageRight.GetHeight() == 480)&&(imageRight.GetWidth() == 640));
  BIASASSERT(imageLeft.GetColorModel() == ImageBase::CM_Grey);
  BIASASSERT(imageRight.GetColorModel() == ImageBase::CM_Grey);

  // fetch complete image from cam
  VideoSource_DCAM::GrabSingle(allImages);
  
  // create 2 images (left and right) out of source image
  unsigned char* grabedPixels = allImages.GetImageData();
  unsigned char* sinkPixelsL = imageLeft.GetImageData();
  unsigned char* sinkPixelsR = imageRight.GetImageData();

  register unsigned indexG0, indexG_0;
  register unsigned indexG2, indexG_2;
  register unsigned indexR0, indexR2;
  register unsigned indexB0, indexB2;

  for (size_t row=0; row<480; row++) {
    for (size_t col=0; col<640; col++) {
      indexG0 = 7680*row + 6*col;
      indexG2 = indexG0 + 2;
      indexB0 = indexG0 + 3;
      indexB2 = indexG0 + 5;
      indexR0 = indexG0 + 3840;
      indexR2 = indexG0 + 3840 + 2;
      indexG_0 = indexG0 + 3840 + 3;
      indexG_2 = indexG0 + 3840 + 5;

      //first bayerpattern is right cam (seen in cam direction)
      sinkPixelsR[row*640 + col] =
        (grabedPixels[indexG0] + grabedPixels[indexR0] + grabedPixels[indexB0] + grabedPixels[indexG_0])/4;

      //third bayerpattern is left cam (seen in cam direction)
      sinkPixelsL[row*640 + col] =
        (grabedPixels[indexG2] + grabedPixels[indexR2] + grabedPixels[indexB2] + grabedPixels[indexG_2])/4;

    }
  }
  
  unsigned long long sec, usec;
  allImages.GetTime(sec, usec);
  imageLeft.SetTime(sec, usec);
  imageLeft.UpdateMetaData();
  imageRight.SetTime(sec, usec);
  imageRight.UpdateMetaData();

  return 0;
}


int VideoSource_DCAM_BumbleBee::GrabSingleAsSmallRGB(Camera <unsigned char> &allImages,
                                     Camera <unsigned char> &imageLeft,
                                     Camera <unsigned char> &imageMiddle,
                                     Camera <unsigned char> &imageRight)
{
  BIASASSERT(CamModel_ == CAMERA_PTGREY_BUMBLEBEE_XB3);
  
  // fetch complete image from cam
  VideoSource_DCAM::GrabSingle(allImages);

  // create 3 images out of source image
  unsigned char* grabedPixels = allImages.GetImageData();
  unsigned char* sinkPixels = NULL;
  bool skip;

  for(unsigned int j=0; j<3; j++)
  {
    switch(j)
    {
      case 0:
        if(!imageRight.IsEmpty())
        {
          sinkPixels = imageRight.GetImageData();
          skip = false;
        }
        else
        {
//          cout<<"\tEmpty image given: skip right image"<<endl;
          skip = true;
        }
        break;
      case 1:
        if(!imageMiddle.IsEmpty())
        {
          sinkPixels = imageMiddle.GetImageData();
          skip = false;
        }
        else
        {
//          cout<<"\tEmpty image given: skip middle image"<<endl;
          skip = true;
        }
        break;
      case 2:
        if(!imageLeft.IsEmpty())
        {
          sinkPixels = imageLeft.GetImageData();
          skip = false;
        }
        else
        {
//          cout<<"\tEmpty image given: skip left image"<<endl;
          skip = true;
        }
        break;
      default:
        skip = true;
        break;
    }
    if(!skip)
    {
      const int
        offsetR = WIDTH*3,
        offsetB = 3;
      int
        index = 0,
        indexG = j;
      for(int j = 0; j < HEIGHT_S; j++)
      {
        for(int i = 0; i < WIDTH_S; i++)
        {
          // R
          sinkPixels[index++] = grabedPixels[indexG+offsetR];
          // G
          sinkPixels[index++] = grabedPixels[indexG];
          // B
          sinkPixels[index++] = grabedPixels[indexG+offsetB];
          indexG += 6;
        }
        indexG += offsetR;
      }
    }
  }
  unsigned long long sec, usec;
  allImages.GetTime(sec, usec);
  imageLeft.SetTime(sec, usec);
  imageLeft.UpdateMetaData();
  imageMiddle.SetTime(sec, usec);
  imageMiddle.UpdateMetaData();
  imageRight.SetTime(sec, usec);
  imageRight.UpdateMetaData();

  return 0;
}