clfTrimmedICP.hh

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

Copyright (C) 2003, 2004    (see file CONTACTS 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
*/

#ifndef CLFTRIMMEDICP_HH_
#define CLFTRIMMEDICP_HH_

#include <Base/Common/BIASpragmaStart.hh>
#include <Base/Debug/Debug.hh>
#include <Base/Image/Image.hh>
#include <Image/Camera.hh>
#include <OpenGLFramework/Base/glfVertexBuffer.hh>
#include <OpenGLFramework/Base/glfBatch.hh>
#include <OpenGLFramework/Base/glfFramebufferObject.hh>
#include <OpenGLFramework/Base/glfTexture2D.hh>
#include <OpenCLFramework/clfContext.hh>
#include <OpenCLFramework/Algorithm/clfRadixSort.hh>

namespace BIAS {

  struct icppair {
    BIAS::Vector3<double> modelpoint;
    BIAS::Vector3<double> inputpoint;
    float distance;
  };
  
  struct icpvecandval {
    BIAS::Vector<double> vector;
    float value;
  };
  
  class BIASOpenCLFramework_EXPORT clfTrimmedICP : public BIAS::Debug {
    
    public:
      clfTrimmedICP(int w = 640, int h = 480);
      
      ~clfTrimmedICP();
      
      unsigned int SetModelFromDepthMap(const BIAS::Camera<float> depthMap, const BIAS::Camera<unsigned char> ampImage);
      unsigned int SetDataFromDepthMap(const BIAS::Camera<float> depthMap, const BIAS::Camera<unsigned char> ampImage);
      
      int Compute(float nearestDistance, unsigned int numberOfPoints);
      
      BIAS::glfVertexBuffer* GetData() {
        return &dataPoints_;
      }
      BIAS::glfVertexBuffer* GetModel() {
        return &modelPoints_;
      }
      
      BIAS::ProjectionParametersBase* GetParams() {
        return dataParams_;
      }
      
      inline void SetMaxIterations(unsigned int maxiterations) {
        maxIterations_ = maxiterations;
      }
    
      static bool comparePairByDistance(icppair a, icppair b) {
        return a.distance < b.distance;
      }
      
      static bool compareVectorByValue(icpvecandval a, icpvecandval b) {
        return a.value < b.value;
      }
      int Fuse(float nearestDistance);
    protected:
      int ComputeNearestPoints_(float nearestDistance, unsigned int numberOfPoints, float &res);
      int ComputeOptimalMotion_(BIAS::Vector3<double> &c, BIAS::RMatrix &R);
      
      unsigned int setFromDepthMap_(BIAS::glfVertexBuffer &buffer, const BIAS::Camera<float> depthMap, const BIAS::Camera<unsigned char> ampImage, bool keep = false, bool padData = false);
      
      BIAS::glfVertexBuffer dataPoints_;
      BIAS::glfVertexBuffer modelPoints_;
      BIAS::glfVertexFormat vertexFormat_;
      unsigned int modelpointcnt_, datapointcnt_;
      

      BIAS::ProjectionParametersBase *dataParams_, *modelParams_;
      std::vector<icppair> pairs_;
      BIAS::Camera<float> inDepth_;
      BIAS::Camera<unsigned char> inTex_;
      BIAS::Vector3<double> centerA_, centerB_;
      
      unsigned int iterationCounter_, maxIterations_, sizeofvertex_;
      
      float lasterror_;
      int w_, h_;
      
      clfContext context_;
      
      clfProgram *programCL_;
      
      clfBuffer *modelBuffer_;
      clfBuffer *dataBuffer_;
      clfBuffer *outBuffer_;
      
      unsigned int numberOfPoints_;
      
      unsigned int *sortedidx_;
      float *distToSort_;
      
      clfRadixSort radixsort_;
      
      int maxComputeUnits_;
  };
}
#include <Base/Common/BIASpragmaEnd.hh>
#endif /* CLFTRIMMEDICP_HH_ */