decsCLTrimmedICP.cpp

#include <mip_config.h>
#ifndef DECS_HAVE_WXWIDGETS
#  error Your DECS needs WX. Please recompile DECS with USE_WXWIDGETS.
#endif

#include <string>
#include <limits>
#include <iostream>

#include <Base/Common/BIASpragmaStart.hh>
#include <Base/Common/BIASpragmaEnd.hh>
#include <Base/Image/ImageConvert.hh>
#include <Base/Common/FileHandling.hh>
#include <Base/CommonUtils/Param.hh>

#include <GLviewer/ContextWX.hh>
#include <GLviewer/GLProjectionParametersPerspective.hh>
#include <GLviewer/Controller/TrackballControl.hh>
#include <GLviewer/Scenes/SceneOpenSceneGraph.hh>
#include <GLviewer/Scenes/SceneOcTree3D.hh>

#include <wx/wx.h>
#include <wx/listctrl.h>
#include <wx/filename.h>

#include <Gui/StringConv.hh>
#include <Gui/ConsoleRedirectIO.hh>
#include <Gui/ImageCanvas.hh>
#include <Gui/biasgl.h> 
#include <Utils/IOUtils.hh>

#include <OpenGLFramework/RenderingContext/glfPBuffer.hh>
#include <OpenCLFramework/Algorithm/clfTrimmedICP.hh>
#include <OpenGLFramework/RenderingContext/glfRenderingContext.hh>


using namespace std;
using namespace DECS;
using namespace BIAS;

enum
{
  ID_Exit,
  ID_Timer
};

class MainFrame : public wxFrame
{
public:
  MainFrame(std::string depthModel, std::string texModel, std::string depthIn, std::string texIn, int ylast, double zthres, bool closeafterfinish, bool notrefinewithall);
  int FindLargestOverlappingRegion_(const ProjectionParametersBase *camAproj, const ProjectionParametersBase *camBproj, ROI &roi);
  bool inited;
protected:
  void OnExit(wxCommandEvent& event);
  void OnTimer(wxTimerEvent& event);
  void OnKey(wxKeyEvent& event);

  ImageCanvas *ic_;
  
  clfTrimmedICP          *tricp_;
  wxTimer             _timer;
  int                computationStep_;
  
  Camera<float> depth1, depth2;
  Camera<unsigned char> amp1c, amp2c;
  Camera<float> d1copy, d2copy, d2orig;
  
  std::string depthModelName_, texModelName_, depthInName_, texInName_;
  
  unsigned int overlapPixels_;
  
  bool closeAfterFinish_, notrefinewithall_;
  
  BIAS::glfRenderingContext* glcontext_;
  BIAS::glfTexture2D outputTextureDepth_;
  BIAS::glfTexture2D outputTextureColor_;
  
  BIAS::glfFramebufferObject fbo_;
  BIAS::glfBatch batch_;
  glfDepthBufferMode depthBufferMode_;
  glfViewport viewport_;
  glfMatrix modelviewmatrix;
  glfMatrix projectionmatrix;
  
  unsigned int mpc, dpc;

  DECLARE_EVENT_TABLE()
};

BEGIN_EVENT_TABLE(MainFrame, wxFrame)
  EVT_BUTTON(ID_Exit, MainFrame::OnExit)
  EVT_TIMER(ID_Timer, MainFrame::OnTimer)
  EVT_KEY_DOWN(MainFrame::OnKey)
END_EVENT_TABLE()

void MainFrame::OnKey(wxKeyEvent& event) {
  //int code = event.GetKeyCode();
}


void MainFrame::OnExit(wxCommandEvent& /*event*/) {
  glcontext_->Destroy();
  Close();
}

void MainFrame::OnTimer(wxTimerEvent& /*event*/) {
  _timer.Stop();
//  cout << "on timer" << endl;
  if (computationStep_ == 0) {
  
    int res = tricp_->Compute(100, 18000);
    cout << "Compute!" << endl;
    if (res == 0) 
      computationStep_ = 1;
  } else if (computationStep_ == 1) {
    if (notrefinewithall_) {
      Projection proj(*(tricp_->GetParams()));
      std::string outname = BIAS::FileHandling::Basename(depthInName_);
      outname+="-icpaligned.mip";
      cout << outname << endl;
      IOUtils::SaveCamera(outname, d2orig, proj);
      computationStep_ = 3;
    } else {
      computationStep_ ++;
      Projection proj(*(tricp_->GetParams()));
      d2copy.SetProj(proj);
      
      delete tricp_;
      
      tricp_ = new clfTrimmedICP;
      tricp_->SetMaxIterations(100);
      
      Projection p1(d1copy.GetProj());
          
      // find overlap
      ROI overlap;
      FindLargestOverlappingRegion_(proj.GetParameters(), p1.GetParameters(), overlap);
      cout << overlap << endl;
      unsigned int corners[4];
      overlap.GetCorners(corners[0], corners[1], corners[2], corners[3]);
      overlapPixels_ = (corners[2] - corners[0]) * (corners[3] - corners[1]) * 2 / 3;
      std::cout << overlapPixels_ << std::endl;
      mpc = tricp_->SetModelFromDepthMap(d1copy, amp1c);
      dpc = tricp_->SetDataFromDepthMap(d2copy, amp2c);
      // todo draw model
      // todo draw data
      computationStep_ = 2;
    }
  } else if (computationStep_ == 2) {
    int res = 0;
    
    if (overlapPixels_ > 500) {
      res = tricp_->Compute(20, overlapPixels_);
    } else {
      res = tricp_->Compute(20, 8000);
    }
    cout << "Compute!" << endl;
    if (res == 0) {
      Projection proj(*(tricp_->GetParams()));
      cout << proj << endl;
      std::string outname = BIAS::FileHandling::Basename(depthInName_);
      outname = "aligned-"+outname+".mip";
      IOUtils::SaveCamera(outname, d2orig, proj);
      computationStep_ = 3;
    }
    
    
  } else if (computationStep_ == 3) {
    tricp_->Fuse(30);
    if (closeAfterFinish_) {
      Close();
    }
    computationStep_++;
  }

  fbo_.ClearColorBuffer(.0f,.0f,.0f,.0f);
  fbo_.ClearDepthBuffer(1.0f);
  batch_.SetVertexBuffer( tricp_->GetModel() );
  batch_.SetRange(0, mpc);
  batch_.Draw();
  batch_.SetVertexBuffer( tricp_->GetData() );
  batch_.SetRange(0, dpc);
  batch_.Draw();
  Image<unsigned char> img;
  
  outputTextureColor_.CopyToImage( img );
  ImageIO::Save("test.mip", img);
  
  ic_->Show( img, "img" );

  glFinish();
  _timer.Start(5, true);
}

MainFrame::MainFrame(std::string depthModel, std::string texModel, std::string depthIn, std::string texIn, int ylast, double zthres, bool closeafterfinish, bool notrefinewithall) 
      : wxFrame((wxFrame *)NULL, -1,
             wxT("Trimmed ICP"), 
             wxPoint(0, 0), 
             wxSize(800, 600))
{
  mpc=0;
  dpc=0;
  
  depthModelName_ = depthModel;
  texModelName_ = texModel;
  depthInName_ = depthIn;
  texInName_ = texIn;
  closeAfterFinish_ = closeafterfinish;
  notrefinewithall_ = notrefinewithall;
  Show(true);
  inited = true;
  
  ic_ = new ImageCanvas(this);
  ic_->SetSize(800,600);

  BIAS::Vector3<double> pos(0,0,0);
  BIAS::Vector4<unsigned char> color(255,0,0,255);

  Camera<float> amp1, amp2;
//  Camera<unsigned char> amp1, amp2;
  Projection p1;
  IOUtils::LoadCamera(&depthModelName_, depth1, p1);
  depth1.ParseMetaData();
  IOUtils::LoadCamera(&depthInName_, depth2);
  depth2.ParseMetaData();
  IOUtils::LoadCamera(&texModelName_, amp1);
  IOUtils::LoadCamera(&texInName_, amp2);
  amp1.ScaleShiftBetween(0,255);
  amp2.ScaleShiftBetween(0,255);
  amp1c.Init(amp1.GetWidth(), amp1.GetHeight(), 3);
  amp2c.Init(amp2.GetWidth(), amp2.GetHeight(), 3);
  unsigned char **a1c = amp1c.GetImageDataArray();
  unsigned char **a2c = amp2c.GetImageDataArray();
  float **a1 = amp1.GetImageDataArray();
  float **a2 = amp2.GetImageDataArray();
//  unsigned char **a1 = amp1.GetImageDataArray();
//  unsigned char **a2 = amp2.GetImageDataArray();
  
  d1copy = depth1;
  d2copy = depth2;
  d2orig = depth2;
  
  float **d1 = depth1.GetImageDataArray();
  float **d2 = depth2.GetImageDataArray();
  for (unsigned int y=0;y<amp1.GetHeight();y++) {
    for (unsigned int x=0;x<amp1.GetWidth();x++) {
//      if (/*d1[y][x] > 1500 || */ y > 123) {
//        d1[y][x] = 0;
//      }
//      if (/*d2[y][x] > 1500 || */ y > 123) {
//        d2[y][x] = 0;
//      }
      if (d1[y][x] > zthres || y > (unsigned int)ylast) {
        d1[y][x] = 0;
      }
      if (d2[y][x] > zthres || y > (unsigned int)ylast) {
        d2[y][x] = 0;
      }
      a1c[y][x*3+0] = (unsigned char)a1[y][x];
      a1c[y][x*3+1] = 0;
      a1c[y][x*3+2] = 0;
      a2c[y][x*3+0] = 0;
      a2c[y][x*3+1] = (unsigned char)a2[y][x];;
      a2c[y][x*3+2] = 0;
    }
  }
  
  glcontext_ = new glfPBuffer;

  unsigned int w=0,h=0;
  p1.GetParameters()->GetImageSize(w,h);

  // fill configuration
  glfRenderingContextConfig cfg;
  cfg.width         = w;
  cfg.height        = h;
  cfg.doubleBuffer  = 0;
  cfg.redSize       = 8;
  cfg.greenSize     = 8;
  cfg.blueSize      = 8;
  cfg.alphaSize     = 8;
  cfg.depthSize     = 24;
  cfg.stencilSize   = 0;

  // initialize context
  glcontext_->Init(cfg);

  computationStep_ = 0;
  
  tricp_ = new clfTrimmedICP(amp1.GetWidth(), amp1.GetHeight());
  
  tricp_->SetMaxIterations(1000);
  
  mpc = tricp_->SetModelFromDepthMap(depth1, amp1c);
  dpc = tricp_->SetDataFromDepthMap(depth2, amp2c);
  
  _timer.SetOwner(this, ID_Timer);
  _timer.Start(5,true);
  cout << "camera on" << endl;
  //Refresh();
  cout << "refresh" << endl;
  
  outputTextureDepth_.Create();
  outputTextureDepth_.Set();
  outputTextureDepth_.Allocate(w,h, GL_DEPTH_COMPONENT24);

  outputTextureColor_.Create();
  outputTextureColor_.Set();
  outputTextureColor_.Allocate(w,h, GL_RGBA);

  fbo_.Create();

  fbo_.AttachTexture(outputTextureColor_, GL_COLOR_ATTACHMENT0_EXT);
  fbo_.AttachTexture(outputTextureDepth_, GL_DEPTH_ATTACHMENT_EXT);
  fbo_.CheckComplete();
  
  // create depth buffer mode
  depthBufferMode_.SetDepthTest(true);
  viewport_.SetOrigin(0, 0);
  viewport_.SetSize(w,h);

  ProjectionParametersPerspective *pp = dynamic_cast<ProjectionParametersPerspective *>(p1.GetParameters());
  float zNear = 10.0f;
  float zFar = 10000.0f;
  projectionmatrix.MakeProjectionMatrixNew(*pp, zNear, zFar, true);
  modelviewmatrix.MakeViewMatrixNew(*pp);

  // create and draw batch
  batch_.SetDepthBufferMode(&depthBufferMode_);
  batch_.SetPrimitiveType(GL_POINTS);
  batch_.SetViewport(&viewport_);
  batch_.SetRenderTarget(&fbo_);
  batch_.SetModelViewMatrix(&modelviewmatrix);
  batch_.SetProjectionMatrix(&projectionmatrix);
  glPointSize(3);

}

int MainFrame::FindLargestOverlappingRegion_(const ProjectionParametersBase *camAproj,
    const ProjectionParametersBase *camBproj, ROI &roi) {

  vector<ProjectionParametersBase *> source;
  vector<float **> depths;
  depths.resize(2);
  source.resize(2);
  source[0] = (ProjectionParametersBase*) camAproj;
  source[1] = (ProjectionParametersBase*) camBproj;
  depths[0] = d1copy.GetImageDataArray();
  depths[1] = d2copy.GetImageDataArray();
  cout << source[0] << endl << source[1] << endl;
  PixelIterator it;
  unsigned int w, h;
  camAproj->GetImageSize(w, h);
  unsigned int xmin = 0, xmax = w, ymin = 0, ymax = h;
  for (unsigned int i = 0; i < source.size(); i++) {
    // mid point
    unsigned int mx, my;
    source[i]->GetImageSize(mx, my);
    HomgPoint2D midPoint(mx / 2, my / 2, 1);
    HomgPoint3D point3D = source[i]->UnProjectToPoint(midPoint, depths[i][my/2][mx/2]);
    point3D.Homogenize();
    midPoint = source[(i+1)%2]->Project(point3D);
    source[i]->GetFirstBorderPixel(it);
    HomgPoint2D edgepoint(0, 0, 0);
    HomgPoint2D oldPoint = edgepoint;
    do {
      HomgPoint2D sourcepoint(it.x, it.y, 1);
      point3D = source[i]->UnProjectToPoint(sourcepoint, depths[i] [ (unsigned int)it.y][ (unsigned int)it.x]);
      point3D.Homogenize();
      edgepoint = source[(i+1)%2]->Project(point3D);
      edgepoint.Homogenize();
      if (edgepoint[0] > 0 && edgepoint[0] < w && edgepoint[1]>0 && edgepoint[1] < h) {
        // get distance vector to midpoint
        HomgPoint2D dist = sourcepoint - midPoint;
        HomgPoint2D grad = sourcepoint - oldPoint;
  
        // going right (or left)
        if (abs(grad[0]) > abs(grad[1])) {
          if (dist[1] < 0) {
            if (edgepoint[1] > ymin) {
              ymin = edgepoint[1];
            }
          } else {
            if (edgepoint[1] < ymax) {
              ymax = edgepoint[1];
            }
          }
        } // going up (or down)
        else {
          if (dist[0] < 0) {
            if (edgepoint[0] > xmin) {
              xmin = edgepoint[0];
            }
          } else {
            if (edgepoint[0] < xmax) {
              xmax = edgepoint[0];
            }
          }
        }
      }
      oldPoint = sourcepoint;
    } while (source[i]->GetNextBorderPixel(it));
  }
  roi.SetCorners(xmin, ymin, xmax, ymax);
  return 0;
}




class icpapp : public wxApp
{
public:
  virtual bool OnInit();
};

bool icpapp::OnInit()

{
  
  Param *p = new Param();

  std::string *mdn = p->AddParamString("depthModel", "name of depthmap model", "bp-dud-CAM02Depth-00700.mip", 'm');
  std::string *mtn = p->AddParamString("texModel", "name of texture model", "bp-CAM02Amp-00700.mip", 't');
  std::string *idn = p->AddParamString("depthInput", "name of depthmap to be aligned", "bp-dud-CAM02Depth-00800.mip", 'i');
  std::string *itn = p->AddParamString("texInput", "name of texture to be aligned", "bp-CAM02Amp-00800.mip", 'n');
  int *ylast = p->AddParamInt("ythres", "cut image in y dir here", -1, 0, INT_MAX, 'y');
  double *zthres = p->AddParamDouble("zthres", "depth threshold" , -1, 0, 10000, 'z');
  bool *closeme = p->AddParamBool("closeAfterFinish", "close wx window after finish", false, 'x');
  bool *notrefinewithall = p->AddParamBool("notrefineonall", "z threshold will not be unset after first optimum found", false);

  p->ParseCommandLine(argc,WxToAsciiArray(argv, argc));

  p->ShowData();

  // create main frame
  MainFrame* mainFrame = new MainFrame(*mdn, *mtn, *idn, *itn, *ylast, *zthres, *closeme, *notrefinewithall);
  mainFrame->Show(true);
  SetTopWindow(mainFrame);


  //ConsoleRedirectIO();

  return true;
}

IMPLEMENT_APP(icpapp)