ICP算法实现C++

/*
Copyright 2011. All rights reserved.
Institute of Measurement and Control Systems
Karlsruhe Institute of Technology Germany

This file is part of libicp.
Authors: Andreas Geiger

libicp is free software; you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the Free Software
Foundation; either version 3 of the License or any later version.

libicp 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 General Public License for more details.

You should have received a copy of the GNU General Public License along with
libicp; if not write to the Free Software Foundation Inc. 51 Franklin
Street Fifth Floor Boston MA 02110-1301 USA 
*/

// Demo program showing how libicp can be used

#include 
#include “icpPointToPoint.h“
#include “icpPointToPlane.h“
#include “lasreader.h“
#include “laswriter.h“
#include “lasdefinitions.h“

using namespace std;

int main （int argc char** argv） {

  // define a 3 dim problem with 10000 model points
  // and 10000 template points:
  int32_t dim = 3;
  
  int32_t num=10000;
   double* M = new double[3*num];
  double* T =  new double[3*num];
  
  // set model and template points
  cout << endl << “Creating model with 10000 points ...“ << endl;
  cout << “Creating template by shifting model by （111） ...“ << endl;
  int32_t k=0;
  for （double x=-2; x<2; x+=0.04） {
    for （double y=-2; y<2; y+=0.04） {
      double z=5*x*exp（-x*x-y*y）;
      M[k*3+0] = x;
      M[k*3+1] = y;
      M[k*3+2] = z;
      T[k*3+0] = x-1;
      T[k*3+1] = y-1;
      T[k*3+2] = z-1;
      k++;
    }
  }
  
  // start with identity as initial transformation
  // in practice you might want to use some kind of prediction here
  Matrix R = Matrix::eye（3）;
  Matrix t（31）;
  //double m=0.01;

  // run point-to-plane ICP （-1 = no outlier threshold）
  cout << endl << “Running ICP （point-to-plane no outliers）“ << endl;
  IcpPointToPlane icp（Mnumdim）;
  icp.fit（TnumRt-1）;

  // results
  cout << endl << “Transformation results:“ << endl;
  cout << “R:“ << endl << R << endl << endl;
  cout << “t:“ << endl << t << endl << endl;

  // free memory
  free（M）;
 free（T）;
  //delete []M;
   //delete []T;

  // success
  return 0;
}

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件     325120  2014-03-30 18:29  ICPdemo_testDebugICPdemo_test.exe

     文件    1862524  2014-03-30 18:29  ICPdemo_testDebugICPdemo_test.ilk

     文件    2903040  2014-03-30 18:29  ICPdemo_testDebugICPdemo_test.pdb

     文件      11034  2014-03-31 19:30  ICPdemo_testICPdemo_testDebugcl.command.1.tlog

     文件     535634  2014-03-31 19:30  ICPdemo_testICPdemo_testDebugCL.read.1.tlog

     文件       8602  2014-03-31 19:30  ICPdemo_testICPdemo_testDebugCL.write.1.tlog

     文件        406  2014-03-17 11:07  ICPdemo_testICPdemo_testDebugICPdemo_test.exe.embed.manifest

     文件        472  2014-03-30 18:26  ICPdemo_testICPdemo_testDebugICPdemo_test.exe.embed.manifest.res

     文件        381  2014-03-30 18:29  ICPdemo_testICPdemo_testDebugICPdemo_test.exe.intermediate.manifest

     文件         84  2014-03-31 19:30  ICPdemo_testICPdemo_testDebugICPdemo_test.lastbuildstate

     文件       2003  2014-03-31 19:30  ICPdemo_testICPdemo_testDebugICPdemo_test.log

     文件          0  2014-03-31 19:30  ICPdemo_testICPdemo_testDebugICPdemo_test.unsuccessfulbuild

     文件        214  2014-03-17 11:07  ICPdemo_testICPdemo_testDebugICPdemo_test_manifest.rc

     文件      79369  2014-03-31 19:28  ICPdemo_testICPdemo_testDebuglasreader.obj

     文件      77775  2014-03-31 19:28  ICPdemo_testICPdemo_testDebuglaswriter.obj

     文件          2  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink-cvtres.read.1.tlog

     文件          2  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink-cvtres.write.1.tlog

     文件          2  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink.2472-cvtres.read.1.tlog

     文件          2  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink.2472-cvtres.write.1.tlog

     文件          2  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink.2472.read.1.tlog

     文件          2  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink.2472.write.1.tlog

     文件          2  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink.3236-cvtres.read.1.tlog

     文件          2  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink.3236-cvtres.write.1.tlog

     文件          2  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink.3236.read.1.tlog

     文件          2  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink.3236.write.1.tlog

     文件       5590  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink.command.1.tlog

     文件       9966  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink.read.1.tlog

     文件       3558  2014-03-30 18:29  ICPdemo_testICPdemo_testDebuglink.write.1.tlog

     文件     169566  2014-03-31 19:28  ICPdemo_testICPdemo_testDebugmatrix.obj

     文件        850  2014-03-30 18:29  ICPdemo_testICPdemo_testDebugmt.command.1.tlog

............此处省略56个文件信息