VideoSource_Controller_Kinect.cpp

#ifdef WIN32
// workaround for wx 2.6.x redefiniton of _CRT_SECURE_NO_DEPRECATE 
// fixed in wx 2.7.2 
#  pragma warning(disable: 4005)
#endif

#include "VideoSource_Controller_Kinect.hh"

#ifdef WIN32
#  include <Base/Common/getopt_W32.h>  
#endif

#include <iostream>
#include <Base/Image/ImageIO.hh>
#include <Image/Camera.hh>
#include <Gui/StringConv.hh>
#include <wx/string.h>

using namespace std;
using namespace BIAS;

VideoSource_Controller_Kinect::
VideoSource_Controller_Kinect(VideoSource *cam, wxWindow *parent, const std::string &title)
  : VideoSource_Controller_Base(parent, -1, AsciiToWx(title)
  ,wxDefaultPosition,wxSize(365,310),wxSTAY_ON_TOP |wxCAPTION)
{
  update_ms_ = 100;
  curTilt_ = 0;

  Cam_ = dynamic_cast<BIAS::VideoSource_Kinect_Base*>(cam);


  this->SetSizeHints( wxDefaultSize, wxDefaultSize );

  wxGridSizer* gSizer1;
  gSizer1 = new wxGridSizer( 2, 2, 0, 0 );

  wxBoxSizer* bSizer1;
  bSizer1 = new wxBoxSizer( wxVERTICAL );

  wxString m_radioBox1Choices[] = { wxT("RGB Color"), wxT("Infrared"), wxT("Switch every Frame") };
  int m_radioBox1NChoices = sizeof( m_radioBox1Choices ) / sizeof( wxString );
  m_radioBox1 = new wxRadioBox( this, wxID_ANY, wxT("Video Mode"), wxDefaultPosition, wxDefaultSize, m_radioBox1NChoices, m_radioBox1Choices, 1, wxRA_SPECIFY_COLS );
  m_radioBox1->SetSelection( 0 );
  bSizer1->Add( m_radioBox1, 0, wxALL, 5 );

  gSizer1->Add( bSizer1, 1, wxEXPAND, 5 );

  wxBoxSizer* bSizer4;
  bSizer4 = new wxBoxSizer( wxVERTICAL );

  m_grid1 = new wxGrid( this, wxID_ANY, wxDefaultPosition, wxDefaultSize, 0 );

  // Grid
  m_grid1->CreateGrid( 4, 1 );
  m_grid1->EnableEditing( false );
  m_grid1->EnableGridLines( true );
  m_grid1->EnableDragGridSize( false );
  m_grid1->SetMargins( 0, 0 );

  // Columns
  m_grid1->EnableDragColMove( false );
  m_grid1->EnableDragColSize( false );
  m_grid1->SetColLabelSize( 30 );
  m_grid1->SetColLabelValue( 0, wxT("Value") );
  m_grid1->SetColLabelAlignment( wxALIGN_CENTRE, wxALIGN_CENTRE );

  // Rows
  m_grid1->EnableDragRowSize( false );
  m_grid1->SetRowLabelSize( 80 );
  m_grid1->SetRowLabelValue( 0, wxT("tilt") );
  m_grid1->SetRowLabelValue( 1, wxT("x") );
  m_grid1->SetRowLabelValue( 2, wxT("y") );
  m_grid1->SetRowLabelValue( 3, wxT("z") );
  m_grid1->SetRowLabelAlignment( wxALIGN_CENTRE, wxALIGN_CENTRE );

  // Label Appearance
  m_grid1->SetLabelBackgroundColour( wxSystemSettings::GetColour( wxSYS_COLOUR_BTNHIGHLIGHT ) );

  // Cell Defaults
  m_grid1->SetDefaultCellAlignment( wxALIGN_LEFT, wxALIGN_TOP );
  bSizer4->Add( m_grid1, 0, wxALL, 5 );

  gSizer1->Add( bSizer4, 1, wxEXPAND, 5 );

  wxBoxSizer* bSizer41;
  bSizer41 = new wxBoxSizer( wxVERTICAL );

  m_button1 = new wxButton( this, wxID_ANY, wxT("Load Calibration xml"), wxDefaultPosition, wxDefaultSize, 0 );
  bSizer41->Add( m_button1, 0, wxALL, 5 );

  m_button5 = new wxButton( this, wxID_ANY, wxT("Toggle Depth Stream"), wxDefaultPosition, wxDefaultSize, 0 );
  bSizer41->Add( m_button5, 0, wxALL, 5 );

  m_button6 = new wxButton( this, wxID_ANY, wxT("Hide"), wxDefaultPosition, wxDefaultSize, 0 );
  bSizer41->Add( m_button6, 0, wxALL, 5 );

  gSizer1->Add( bSizer41, 1, wxEXPAND, 5 );

  wxBoxSizer* bSizer3;
  bSizer3 = new wxBoxSizer( wxVERTICAL );

  m_button2 = new wxButton( this, wxID_ANY, wxT("Up +5"), wxDefaultPosition, wxDefaultSize, 0 );
  bSizer3->Add( m_button2, 0, wxALL, 5 );

  m_button3 = new wxButton( this, wxID_ANY, wxT("Level"), wxDefaultPosition, wxDefaultSize, 0 );
  bSizer3->Add( m_button3, 0, wxALL, 5 );

  m_button4 = new wxButton( this, wxID_ANY, wxT("Down -5"), wxDefaultPosition, wxDefaultSize, 0 );
  bSizer3->Add( m_button4, 0, wxALL, 5 );

  gSizer1->Add( bSizer3, 1, wxEXPAND, 5 );

  this->SetSizer( gSizer1 );
  this->Layout();
  
  timer_ = new wxTimer(this, ID_Timer);

  // Connect Events
  m_radioBox1->Connect( wxEVT_COMMAND_RADIOBOX_SELECTED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnSelectVideoMode ), NULL, this );
  m_button1->Connect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnLoadCalib ), NULL, this );
  m_button5->Connect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnToggleDepth ), NULL, this );
  m_button6->Connect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnHide ), NULL, this );
  m_button2->Connect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnUp ), NULL, this );
  m_button3->Connect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnLevel ), NULL, this );
  m_button4->Connect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnDown ), NULL, this );
  this->Connect( ID_Timer, wxEVT_TIMER, wxTimerEventHandler( VideoSource_Controller_Kinect::OnTimer ) );

  timer_->Start(update_ms_, true);
}



VideoSource_Controller_Kinect::
~VideoSource_Controller_Kinect() {
  timer_->Stop();
  // Disconnect Events
  m_radioBox1->Disconnect( wxEVT_COMMAND_RADIOBOX_SELECTED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnSelectVideoMode ), NULL, this );
  m_button1->Disconnect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnLoadCalib ), NULL, this );
  m_button5->Disconnect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnToggleDepth ), NULL, this );
  m_button6->Disconnect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnHide ), NULL, this );
  m_button2->Disconnect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnUp ), NULL, this );
  m_button3->Disconnect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnLevel ), NULL, this );
  m_button4->Disconnect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VideoSource_Controller_Kinect::OnDown ), NULL, this );
  this->Disconnect( wxEVT_TIMER, wxTimerEventHandler( VideoSource_Controller_Kinect::OnTimer ) );
}

void VideoSource_Controller_Kinect::
SetVideoSource(BIAS::VideoSource *cam){
  Cam_ = dynamic_cast<BIAS::VideoSource_Kinect_Base*>(cam);
}


void VideoSource_Controller_Kinect::
OnHide(wxCommandEvent& event){
  Show(false);
  timer_->Stop();
}

void VideoSource_Controller_Kinect::
OnClose(wxCloseEvent& event) {
  timer_->Stop();
  Show(false);
}

void VideoSource_Controller_Kinect::
OnLoadCalib(wxCommandEvent& event) {
  
  wxFileDialog fileDialog(
    this,                          // parent
    wxT("Load internal calibration file for IR camera (BIAS Projection xml)"), // message
    wxEmptyString,                   // default directory
    wxEmptyString,                  // default file
    wxT("Projection files (proj;xml)|*.proj;*.xml|All files (*.*)|*.*"), // wildcard
    wxFD_OPEN);                       // style

  if (fileDialog.ShowModal() == wxID_OK) {
    wxString wxStr = fileDialog.GetPath();
    Projection p;
    p.Load(WxToAscii(wxStr));
    ProjectionParametersPerspective *ppp = dynamic_cast<ProjectionParametersPerspective*>(p.GetParameters());
    if (ppp == NULL) {
      wxMessageDialog messageDialog(this, wxT("Sorry, no Projection Parameters found!"), wxT("ShowCamWx - Kinect Calibration file"), wxICON_ERROR | wxOK);
      messageDialog.ShowModal();
    }
    double fx, fy, px, py;
    ppp->GetFocalLength(fx);
    fy = ppp->GetAspectratio() * fx;
    ppp->GetPrincipal(px,py);
    Cam_->CreateDepthLookupTable(px, py, fx, fy);
  } 
}

void VideoSource_Controller_Kinect::
OnSelectVideoMode( wxCommandEvent& event ) {
  int id = event.GetInt();
  switch (id) {
  case 0: // rgb
    Cam_->SetVideoModeColor();
    break;
  case 1: // rgb
    Cam_->SetVideoModeIR();
    break;
  case 2: // rgb
    Cam_->SetSwitchVideoModeAfterGrab();
    break;
  }
}

void VideoSource_Controller_Kinect::
OnUp( wxCommandEvent& event ) {
  if (curTilt_ != -64)
    Cam_->SetTilt(curTilt_+5.0);
}
void VideoSource_Controller_Kinect::
OnLevel( wxCommandEvent& event ) {
  Cam_->SetTilt(0.0);
}
void VideoSource_Controller_Kinect::
OnDown( wxCommandEvent& event ) {
  if (curTilt_ != -64)
    Cam_->SetTilt(curTilt_-5.0);
}
void VideoSource_Controller_Kinect::
OnToggleDepth( wxCommandEvent& event ) {
  Cam_->ToggleDepthStream();
}
void VideoSource_Controller_Kinect::
OnTimer( wxTimerEvent& event ) {
  timer_->Stop();
  double x,y,z;
  if (Cam_->GetAccelerometer(curTilt_, x,y,z) != 0) {
    // if cam does not support, do not reactivate timer
    return;
  }

  wxString tilts;
  wxString xs;
  wxString ys;
  wxString zs;
  if (curTilt_ == -64) {
    tilts = AsciiToWx("moving");
  } else {
    tilts << curTilt_;
  }
  xs << x;
  ys << y;
  zs << z;
  
  m_grid1->SetCellValue(0,0,tilts);
  m_grid1->SetCellValue(1,0,xs);
  m_grid1->SetCellValue(2,0,ys);
  m_grid1->SetCellValue(3,0,zs);
  
  timer_->Start(update_ms_, true);
}