clfTrimmedICP.cpp

#include <Base/Common/BIASpragma.hh>
#include <algorithm>
#include <MathAlgo/SVD.hh>
#include <OpenCLFramework/Algorithm/clfTrimmedICP.hh>
#include <iostream>
#include <OpenCLFramework/Algorithm/clfRadixSort.hh>

#define SORT_GPU 1

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

struct PointVertex {
    GLfloat position[3];
    GLfloat color[4];
};

struct plainpair {
    float data[3];
    float model[3];
    float distance;
};

clfTrimmedICP::clfTrimmedICP(int w, int h) : context_(true), radixsort_(&context_) {
  sizeofvertex_ = 7;
  maxIterations_ = 40;
  iterationCounter_ = 0;
  lasterror_ = 10000.0f;
  w_ = w;
  h_ = h;
  vertexFormat_.AddAttribute(glfVertexFormat::ATTRIB_POSITION, GL_FLOAT, 3);
  vertexFormat_.AddAttribute(glfVertexFormat::ATTRIB_COLOR, GL_FLOAT, 4);
  modelPoints_.Create( w_*h_, vertexFormat_);
  dataPoints_.Create( w_*h_, vertexFormat_);
  
  maxComputeUnits_ = context_.GetDeviceInfo().maxComputeUnits;
  
  programCL_ = context_.CreateProgram();

  // add code to our program
  programCL_->AddSource("/OpenCLFramework/Algorithm/cl/trimmedicp.cl");

  programCL_->Build();
  
  programCL_->AddKernel( "findNearestPairs" );
  programCL_->AddKernel( "movePointCloud" );
  
  dataBuffer_ = NULL;
  modelBuffer_ = NULL;
  
  outBuffer_ = context_.CreateBuffer();
  cout << "Sizeof : " << sizeof( plainpair ) << endl;
  outBuffer_->Allocate( w_*h_ * sizeof( plainpair ), false, false);
  programCL_->KernelSetArgument( "findNearestPairs", 2, *outBuffer_ );
  programCL_->KernelSetArgument( "movePointCloud", 1, *outBuffer_ );
  pairs_.resize( w_*h_);
  sortedidx_ = NULL;
  distToSort_ = NULL;
}

clfTrimmedICP::~clfTrimmedICP() {
}

unsigned int clfTrimmedICP::SetModelFromDepthMap(const BIAS::Camera<float> depthMap, const BIAS::Camera<unsigned char> ampImage) {
  modelpointcnt_ = setFromDepthMap_(modelPoints_, depthMap, ampImage, false);
  if (modelBuffer_ == NULL) {
    modelBuffer_ = context_.CreateBuffer();
    modelBuffer_->AllocateFromVertexBuffer( modelPoints_, false, false);
    programCL_->KernelSetArgument( "findNearestPairs", 0, *modelBuffer_ );
  }
  programCL_->KernelSetArgument( "findNearestPairs", 3, (int)modelpointcnt_ );
  modelParams_ = depthMap.GetProj().GetParameterCloneWithAbsolutePose();
  return modelpointcnt_;
}
unsigned int clfTrimmedICP::SetDataFromDepthMap(const BIAS::Camera<float> depthMap, const BIAS::Camera<unsigned char> ampImage) {
  datapointcnt_ = setFromDepthMap_(dataPoints_, depthMap, ampImage, true, true);
  if (dataBuffer_ == NULL) {
    dataBuffer_ = context_.CreateBuffer();
    dataBuffer_->AllocateFromVertexBuffer( dataPoints_, false, false);
    programCL_->KernelSetArgument( "findNearestPairs", 1, *dataBuffer_ );
    programCL_->KernelSetArgument( "movePointCloud", 0, *dataBuffer_ );
  }
  dataParams_ = depthMap.GetProj().GetParameterCloneWithAbsolutePose();
  return datapointcnt_;
}


unsigned int clfTrimmedICP::
setFromDepthMap_(glfVertexBuffer &buffer, const Camera<float> depthMap, const Camera<unsigned char> ampImage, bool keep, bool padData) {

  lasterror_ = 10000.0f;
  
  unsigned int ret = 0;
  
  PointVertex *vertices = (PointVertex*)buffer.Map();
  ProjectionParametersPerspective *ppp = dynamic_cast<ProjectionParametersPerspective *>(depthMap.GetProj().GetParameterCloneWithAbsolutePose());
  ProjectionParametersPerspectiveDepth *ppd = dynamic_cast<ProjectionParametersPerspectiveDepth *>(ppp);
  const float **ida = depthMap.GetImageDataArray();
  HomgPoint2D p2d;
  unsigned int idx;
  for (int y=0;y<h_;y++) {
    for (int x=0;x<w_;x++) {
      idx = ret;
      float d = ida[y][x];
      if (fabs(d) > 0.1) {
        p2d.Set(x,y,1);
        if (ppd != NULL)
          ppd->UnDistortDepth(p2d, d, false);
        HomgPoint3D p3d = ppp->UnProjectToPoint(p2d, d, false);
        Vector3<double> p3dv = p3d.GetEuclidean();
        // position
        vertices[idx].position[0] = (float)p3dv[0];
        vertices[idx].position[1] = (float)p3dv[1];
        vertices[idx].position[2] = (float)p3dv[2];
        // color
        vertices[idx].color[0] = (float)x/(float)w_;
        vertices[idx].color[1] = (float)y/(float)h_;
        vertices[idx].color[2] = (float)d/(float)10000.0f;
        vertices[idx].color[3] = 1.0f;
        ret ++;
      }
    }
  }
  if (padData) {
    unsigned int padval = ret % maxComputeUnits_;
    if (padval != 0) {
      padval = maxComputeUnits_ - padval;
    }
    cout << "padding " << padval << " " << endl;
    unsigned int start = ret;
    for (unsigned int idx=start;idx<start+padval;idx++) {
      // position
      vertices[idx].position[0] = (float)0.0f;
      vertices[idx].position[1] = (float)0.0f;
      vertices[idx].position[2] = (float)16000.0f;
      // color
      vertices[idx].color[0] = 0.0f;
      vertices[idx].color[1] = 0.0f;
      vertices[idx].color[2] = 16000.0f/10000.0f;
      vertices[idx].color[3] = 1.0f;
      ret ++;
    }
    cout << "padding " << padval << " full: " << ret << endl;

  }
  buffer.Unmap();
  glFlush();
  glFinish();

  if (keep) {
    inDepth_ = depthMap;
    inTex_ = ampImage;
  }
  return ret;
}

int clfTrimmedICP::
Compute(float nearestDistance, unsigned int numberOfPoints) {
  float res=0;
  ComputeNearestPoints_(nearestDistance, numberOfPoints, res);
  if (res < 10e-3 && res>0) {
    std::cout << "CONVERGENCE!!" << std::endl;
    return 0;
  } else if (res > lasterror_ && false) {
    std::cout << "previous result was better than current, finishing" << std::endl;
    return 0;
  } else if (iterationCounter_ > maxIterations_) {
    std::cout << "max iterations reached" << std::endl;
    return 0;
  }
  lasterror_ = res;

  BIAS::Vector3<double> c = dataParams_->GetC(), diff;
  BIAS::RMatrix R = dataParams_->GetR();
  
  centerA_.SetZero();
  centerB_.SetZero();

  // compute optimal motion here!
  ComputeOptimalMotion_(c, R);
  diff = c - dataParams_->GetC();
  if (diff.NormL2() < 0.1) {
    std::cout << "optimum found" << std::endl;
    return 0;
  }
  // calculate new parameters here!
  dataParams_->SetC( c );
  dataParams_->SetR( R );

  // todo change dataPoints HERE

  iterationCounter_++;
  return 1;
}

int clfTrimmedICP::Fuse(float nearestDistance) {
  BIASWARN ("fuse function disabled");
  /* todo what is this for anyways...
  OcTree3D **branches = incomingTree_->GetNodes();
  for (unsigned int i=0;i<8;i++) {
    delete branches[i];
    branches[i] = NULL;
  }
//    setFromDepthMap_(incomingTree_, inDepth_, inTex_);
  incomingTree_->AddDepthMap(inDepth_, dataParams_, inTex_,0,5.0);
  
  // for each point in incoming, find nearest point in model.
  std::vector<OcTree3D* > nIn;
  std::vector<OcTree3D* > nModel;
  incomingTree_->GetAllActiveNodes(nIn);
  pairs_.clear();
  icppair p;

// #pragma omp parallel for private (nModel)
  for (unsigned int i=0;i<nIn.size();i++) {
    modelTree_->GetSupersetVolumeNodes(nModel, nearestDistance, nIn[i]->GetPosition());
    if (nModel.empty())
      continue;
    OcTree3D* minDistNode = NULL;
    float minDist = 10e5;
    float dist = 10e5;
    BIAS::Vector3<double> *pos =  nIn[i]->GetPosition();
    for (unsigned int j=0;j<nModel.size();j++) {
      dist = nModel[j]->GetPosition()->Dist( *pos );
      if (dist < minDist) {
        minDist = dist;
        minDistNode = nModel[j];
      }
    }
    p.inputpoint = nIn[i]->GetPosition();
    p.modelpoint = minDistNode->GetPosition();
    p.distance = minDist;
    pairs_.push_back(p);
//    std::cout << "point " << *(p.inputpoint) << " " << *(p.modelpoint) << " distance: " << p.distance << " " << nModel.size() << std::endl;
    nModel.clear();
  }  
  std::sort(pairs_.begin(), pairs_.end(), clfTrimmedICP::comparePairByDistance);
  OcTree3D fusedtree;
  fusedtree.SetSize(16000);
  fusedtree.SetContentActive(false);

// #pragma omp parallel for
  for (unsigned int i=0;i<pairs_.size();i++) {
    OcTree3D *oct = modelTree_->GetDeepestNodeContaining(pairs_[i].modelpoint);
    if (oct == NULL) continue;
    DataElementBase *modelelement = oct->GetContent();
    oct = incomingTree_->GetDeepestNodeContaining(pairs_[i].inputpoint);
    if (oct == NULL) continue;
    DataElementBase *inputelement = oct->GetContent();
    if (modelelement != NULL && inputelement != NULL) {
      DataElementBase *newElement = new DataElementBase();
      newElement->SetPosition(*(pairs_[i].modelpoint));
      fusedtree.AddTreeItem(newElement, 5.0, 0.5);
    }
    if (modelelement != NULL)
      modelTree_->DeleteTreeItem(modelelement);
    if (inputelement != NULL)
      incomingTree_->DeleteTreeItem(inputelement);
  }
  
  std::cout << "saving fuse result" << std::endl;
  fusedtree.Write("fused-tree.txt");
  nIn.clear();
  incomingTree_->GetAllActiveNodes(nIn);
  modelTree_->GetAllActiveNodes(nModel);
  branches = fusedtree.GetNodes();
  for (unsigned int i=0;i<8;i++) {
    delete branches[i];
    branches[i] = NULL;
  }
  for (unsigned int i=0;i<nIn.size();i++) {
    fusedtree.AddTreeItem(nIn[i]);
  }
  for (unsigned int i=0;i<nModel.size();i++) {
    fusedtree.AddTreeItem(nModel[i]);
  }
  std::cout << "saving remains result" << std::endl;
  fusedtree.Write("remains-tree.txt");
  */
  return 0;
}

int clfTrimmedICP::
ComputeNearestPoints_(float nearestDistance, unsigned int numberOfPoints, float &res) {
  cout << "1" << endl;
  numberOfPoints_ = numberOfPoints;
  cout << "2" << endl;

  try {
    context_.AcquireGLObject( *modelBuffer_ );
    context_.AcquireGLObject( *dataBuffer_ );
    cout << "run on " << datapointcnt_ << " points" << endl;
    context_.RunOn1DRange( *programCL_, "findNearestPairs", datapointcnt_, maxComputeUnits_);
    context_.Finish();
    cout << "release gl" << endl;
    context_.ReleaseGLObject( *dataBuffer_ );
    context_.ReleaseGLObject( *modelBuffer_ );
    context_.Finish();
  } catch (clfException &e) {
    cout << "find nearest exception: " << e.GetDetailedString() << endl;
  }

  plainpair *pairs = (plainpair*)outBuffer_->MapBuffer(false);
  context_.Finish();

//  for (int i=0;i<w_*h_;i++) {
//    for (int j=0;j<3;j++) cout << pairs[i].data[j] << " ";
//    for (int j=0;j<3;j++) cout << pairs[i].model[j] << " ";
//    cout << pairs[i].distance << endl;
//  }

//  plainpair *pairs = new plainpair[w_*h_];
//  outBuffer_->ReadFromBuffer(pairs);

  std::cout << "sort!" << std::endl;
  // copy stuff to vector
  if (distToSort_ == NULL) {
    distToSort_ = new float[w_*h_];
  }
  for (unsigned int i=0;i<datapointcnt_;i++) {
    pairs_[i].inputpoint.Set(pairs[i].data[0], pairs[i].data[1],pairs[i].data[2]);
    pairs_[i].modelpoint.Set(pairs[i].model[0], pairs[i].model[1],pairs[i].model[2]);
    pairs_[i].distance = pairs[i].distance;
    distToSort_[i] = pairs[i].distance;
  }
  outBuffer_->UnMap( pairs );
  context_.Finish();
//  delete pairs;
  std::cout << "copy done." << endl;  
  
  
  if (pairs_.size() < numberOfPoints_) {
    numberOfPoints_ = pairs_.size();
  }
  if (numberOfPoints_ > datapointcnt_) {
    numberOfPoints_ = datapointcnt_;
  }
  
  if (SORT_GPU) {
    try {
      radixsort_.SetData(datapointcnt_, distToSort_);
      radixsort_.Sort();
      radixsort_.RecupGPU();
      sortedidx_ = radixsort_.GetPermutation();
    } catch (clfException &e) {
      cout << "Radixsort Exception: " << e.GetDetailedString() << endl;
    }
  } else {
    if (sortedidx_ == NULL) {
      sortedidx_ = new unsigned int[w_*h_];
      for (int i=0;i<w_*h_;i++) {
        sortedidx_[i] = i;
      }
    }
    std::sort(pairs_.begin(), pairs_.begin()+datapointcnt_, clfTrimmedICP::comparePairByDistance);
  }
  
  std::cout << "sort finished!" << std::endl;
  std::cout << numberOfPoints_ << " -> " << pairs_[numberOfPoints_-1].distance << std::endl;

//  for (unsigned int i=0;i<datapointcnt_;i++) {
//    cout << pairs_[i].inputpoint << " " << pairs_[i].modelpoint << " " << pairs_[i].distance << endl;
//    if (pairs_[i].distance > 1.0) {
//      cout << "first non null: " << i << " " << pairs_[i].distance << endl;
//      //break;
//    }
////    cout << pairs_[i].inputpoint << " " << pairs_[i].modelpoint << " " << pairs_[i].distance << endl;
//  }
  double sum = 0.0;
  for (unsigned int i=0;i<numberOfPoints_;i++) {
    if (pairs_[ sortedidx_[i] ].distance > 1000.0) {
      numberOfPoints_ = i;
      break;
    }
    //std::cout << pairs_[ sortedidx_[i] ].distance << std::endl;
    sum += pairs_[sortedidx_[i] ].distance;
  }
  if (numberOfPoints_ == 0) {
    res = 0;
    std::cout << "no pairs?!" << std::endl;
    return 0;
  }
  sum /= double(numberOfPoints_);
  std::cout << "current error: " << sqrt(sum) << std::endl;
  res = (float)sum;
  
  return 0;
}

int clfTrimmedICP::
ComputeOptimalMotion_(BIAS::Vector3<double> &c, BIAS::RMatrix &R) {

  for (unsigned int i=0;i<numberOfPoints_;i++) {
    centerA_ += (pairs_[sortedidx_[i] ].modelpoint);
    centerB_ += (pairs_[sortedidx_[i] ].inputpoint);
  }
  centerA_.DivideIP(double(numberOfPoints_));
  centerB_.DivideIP(double(numberOfPoints_));

  BIAS::Matrix3x3<double> M;
  M.SetZero();
  for (unsigned int i=0;i<numberOfPoints_;i++) {
    for (unsigned int x=0;x<3;x++) {
      for (unsigned int y=0;y<3;y++) {
        M[x][y] += ((pairs_[sortedidx_[i] ].modelpoint)[x] - centerA_[x]) * ( (pairs_[sortedidx_[i] ].inputpoint) [y] - centerB_[y]);
      }
    }
  }
  BIAS::Matrix4x4<double> S;
  unsigned int x=0, y=1, z=2;
  S[0][0] = M[x][x] + M[y][y] + M[z][z];
  S[0][1] = M[y][z] - M[z][y];
  S[0][2] = M[z][x] - M[x][z];
  S[0][3] = M[x][y] - M[y][x];
  S[1][0] = S[0][1];
  S[1][1] = M[x][x] - M[y][y] - M[z][z];
  S[1][2] = M[x][y] + M[y][x];
  S[1][3] = M[z][x] + M[x][z];
  S[2][0] = S[0][2];
  S[2][1] = S[1][2];
  S[2][2] = -M[x][x] + M[y][y] - M[z][z];
  S[2][3] = M[y][z] + M[z][y];
  S[3][0] = S[0][3];
  S[3][1] = S[1][3];
  S[3][2] = S[2][3];
  S[3][3] = -M[x][x] - M[y][y] + M[z][z];

  BIAS::SVD svd(S);

  BIAS::Matrix<double> vt = svd.GetVT();
  std::vector<icpvecandval> eigValues(vt.GetRows());
  // now we need the eigenvalues (with sign !)
  // compute this by projecting the (normalized) eigenvectors onto their images
  BIAS::Vector<double> eigenvector, eigenimage;
  for (unsigned int i=0; i<vt.GetRows(); i++) {
    // get ith eigenvector and its image
    eigenvector=vt.GetRow(i);
    eigenimage = S * eigenvector;
    // compute scalar product
    icpvecandval t;
    t.value = (float)(eigenimage * eigenvector);
    t.vector = eigenvector;
    eigValues[i] = t;
  }
  std::sort(eigValues.begin(),eigValues.end(), compareVectorByValue);

  BIAS::Vector<double> qnorm = eigValues[eigValues.size()-1].vector;
  BIAS::Quaternion<double> q;
  q[3] = qnorm[0];
  q[0] = qnorm[1];
  q[1] = qnorm[2];
  q[2] = qnorm[3];
  BIAS::RMatrix turn;
  turn.SetFromQuaternion(q);
  turn.InvertIP();

  BIAS::Vector3<double> centertocamera = c - centerB_;

  BIAS::Vector3<double> centertocamera_rotated = turn * centertocamera;

  BIAS::Vector3<double> cmod = centertocamera_rotated - centertocamera;

  cmod += (centerA_-centerB_);
  c = c + cmod;
  std::cout << " rot   : " << q << std::endl;
  std::cout << " trans : " << cmod << std::endl;

  R = turn * R;
  
  float *tmat = (float*)outBuffer_->MapBuffer(true, 0, 15*sizeof(float));
  
  for (int i=0;i<9;i++) {
    tmat[i] = (float)turn.GetData()[i];
  }
  for (int i=0;i<3;i++) {
    tmat[9+i] = (float)(centerA_-centerB_)[i];
  }
  for (int i=0;i<3;i++) {
    tmat[12+i] = (float)(centerB_)[i];
  }
  outBuffer_->UnMap( tmat );
  
  context_.AcquireGLObject( *dataBuffer_ );
  cout << "move" << endl;
  context_.RunOn1DRange( *programCL_, "movePointCloud", datapointcnt_, maxComputeUnits_);
  cout << "release gl" << endl;
  context_.ReleaseGLObject( *dataBuffer_ );
  context_.Finish();

  return 0;
}