├── CMakeLists.txt
├── src
    ├── icpPointToPoint.h
    ├── icpPointToPlane.h
    ├── icp.cpp
    ├── icp.h
    ├── demo.cpp
    ├── matrix.h
    ├── kdtree.h
    ├── icpPointToPoint.cpp
    ├── icpPointToPlane.cpp
    ├── kdtree.cpp
    └── matrix.cpp
└── README.TXT


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | # project
 2 | cmake_minimum_required (VERSION 2.6)
 3 | project (libicp)
 4 | 
 5 | # directories
 6 | set (LIBICP_SRC_DIR src)
 7 | FIND_PATH(BOOST_DIR "boost")
 8 | 
 9 | # include directory
10 | include_directories("${LIBICP_SRC_DIR}")
11 | include_directories("${BOOST_DIR}")
12 | 
13 | #include directories for lasreaders
14 | 
15 | include_directories("/LAStools/LASlib/inc")
16 | include_directories("/LAStools/LASzip/src")
17 | include_directories("/LAStools/LASlib/lib")
18 | 
19 | 
20 | # determine if OpenMP can/should be used
21 | option(USE_OPENMP "Enable OpenMP?" ON) # set to OFF to disable
22 | if(USE_OPENMP)
23 |  FIND_PACKAGE(OpenMP)
24 |  if(OPENMP_FOUND OR OpenMP_FOUND)
25 |   message(STATUS "OpenMP flags = ${OpenMP_CXX_FLAGS}")
26 |   set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
27 |   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
28 |  else(OPENMP_FOUND OR OpenMP_FOUND)
29 |   message(STATUS "OpenMP not supported")
30 |  endif(OPENMP_FOUND OR OpenMP_FOUND)
31 | endif(USE_OPENMP)
32 | 
33 | # sources
34 | FILE(GLOB LIBICP_SRC_FILES "src/*.cpp")
35 | 
36 | # make release version
37 | set(CMAKE_BUILD_TYPE Release)
38 | 
39 | # build demo program
40 | add_executable(icp ${LIBICP_SRC_FILES})
41 | 
42 | target_link_libraries(icp /LAStools/LASlib/lib/liblas.a)
43 | 


--------------------------------------------------------------------------------
/src/icpPointToPoint.h:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright 2011. All rights reserved.
 3 | Institute of Measurement and Control Systems
 4 | Karlsruhe Institute of Technology, Germany
 5 | 
 6 | Authors: Andreas Geiger
 7 | 
 8 | libicp is free software; you can redistribute it and/or modify it under the
 9 | terms of the GNU General Public License as published by the Free Software
10 | Foundation; either version 2 of the License, or any later version.
11 | 
12 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY
13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
14 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
15 | 
16 | You should have received a copy of the GNU General Public License along with
17 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin
18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 
19 | */
20 | 
21 | #ifndef ICP_POINT_TO_POINT_H
22 | #define ICP_POINT_TO_POINT_H
23 | 
24 | #include "icp.h"
25 | 
26 | class IcpPointToPoint : public Icp {
27 | 
28 | public:
29 | 
30 |   IcpPointToPoint (double *M,const int32_t M_num,const int32_t dim) : Icp(M,M_num,dim) {}
31 |   virtual ~IcpPointToPoint () {}
32 | 
33 | private:
34 | 
35 |   double fitStep (double *T,const int32_t T_num,Matrix &R,Matrix &t,const std::vector<int32_t> &active);
36 |   std::vector<int32_t> getInliers (double *T,const int32_t T_num,const Matrix &R,const Matrix &t,const double indist);
37 |   double getResidual(double *T,const int32_t T_num,const Matrix &R,const Matrix &t,const std::vector<int> &active);
38 | };
39 | 
40 | #endif // ICP_POINT_TO_POINT_H
41 | 


--------------------------------------------------------------------------------
/src/icpPointToPlane.h:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright 2011. All rights reserved.
 3 | Institute of Measurement and Control Systems
 4 | Karlsruhe Institute of Technology, Germany
 5 | 
 6 | Authors: Andreas Geiger
 7 | 
 8 | libicp is free software; you can redistribute it and/or modify it under the
 9 | terms of the GNU General Public License as published by the Free Software
10 | Foundation; either version 2 of the License, or any later version.
11 | 
12 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY
13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
14 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
15 | 
16 | You should have received a copy of the GNU General Public License along with
17 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin
18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 
19 | */
20 | 
21 | #ifndef ICP_POINT_TO_PLANE_H
22 | #define ICP_POINT_TO_PLANE_H
23 | 
24 | #include "icp.h"
25 | 
26 | class IcpPointToPlane : public Icp {
27 | 
28 | public:
29 |   
30 |   IcpPointToPlane (double *M,const int32_t M_num,const int32_t dim,const int32_t num_neighbors=10,const double flatness=5.0) : Icp(M,M_num,dim) {
31 |     M_normal = computeNormals(num_neighbors,flatness);
32 |   }
33 | 
34 |   virtual ~IcpPointToPlane () {
35 |     free(M_normal);
36 |   }
37 | 
38 | private:
39 | 
40 | 	double fitStep (double *T,const int32_t T_num,Matrix &R,Matrix &t,const std::vector<int32_t> &active);
41 | 	std::vector<int32_t> getInliers (double *T,const int32_t T_num,const Matrix &R,const Matrix &t,const double indist);
42 | 	double getResidual(double *T,const int32_t T_num,const Matrix &R,const Matrix &t,const std::vector<int> &active);
43 | 	// utility functions to compute normals from the model tree
44 | 	void computeNormal (const kdtree::KDTreeResultVector &neighbors,double *M_normal,const double flatness);
45 | 	double* computeNormals (const int32_t num_neighbors,const double flatness);
46 | 
47 | 	// normals of model points
48 | 	double *M_normal;
49 | };
50 | 
51 | #endif // ICP_POINT_TO_PLANE_H
52 | 


--------------------------------------------------------------------------------
/src/icp.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright 2011. All rights reserved.
  3 | Institute of Measurement and Control Systems
  4 | Karlsruhe Institute of Technology, Germany
  5 | 
  6 | Authors: Andreas Geiger
  7 | 
  8 | libicp is free software; you can redistribute it and/or modify it under the
  9 | terms of the GNU General Public License as published by the Free Software
 10 | Foundation; either version 2 of the License, or any later version.
 11 | 
 12 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY
 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
 15 | 
 16 | You should have received a copy of the GNU General Public License along with
 17 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin
 18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 
 19 | */
 20 | 
 21 | #include "icp.h"
 22 | using namespace std;
 23 | 
 24 | Icp::Icp (double *M,const int32_t M_num,const int32_t dim) 
 25 | :m_dim(dim), m_max_iter(200), m_min_delta(1e-4) 
 26 | {
 27 |   
 28 | 	// check for correct dimensionality
 29 | 	if (dim!=2 && dim!=3) {
 30 | 		cout << "ERROR: LIBICP works only for data of dimensionality 2 or 3" << endl;
 31 | 		m_kd_tree = 0;
 32 | 		return;
 33 | 	}
 34 | 	// check for minimum number of points
 35 | 	if (M_num<5) {
 36 | 		cout << "ERROR: LIBICP works only with at least 5 model points" << endl;
 37 | 		m_kd_tree = 0;
 38 | 		return;
 39 | 	}
 40 | 
 41 | 	// copy model points to M_data
 42 | 	m_kd_data.resize(boost::extents[M_num][dim]);
 43 | 	for (int32_t m=0; m<M_num; m++)
 44 | 		for (int32_t n=0; n<dim; n++)
 45 | 		  m_kd_data[m][n] = (float)M[m*dim+n];
 46 | 
 47 | 	// build a kd tree from the model point cloud
 48 | 	m_kd_tree = new kdtree::KDTree(m_kd_data);
 49 | }
 50 | 
 51 | Icp::~Icp () {
 52 |   if (m_kd_tree)
 53 |     delete m_kd_tree;
 54 | }
 55 | 
 56 | double Icp::fit( double *T,const int32_t T_num,Matrix &R,Matrix &t,double indist/*=-1*/ )
 57 | {
 58 | 	// make sure we have a model tree
 59 | 	if (!m_kd_tree) {
 60 | 		cout << "ERROR: No model available." << endl;
 61 | 		return 0;
 62 | 	}
 63 | 
 64 | 	// check for minimum number of points
 65 | 	if (T_num<5) {
 66 | 		cout << "ERROR: Icp works only with at least 5 template points" << endl;
 67 | 		return 0;
 68 | 	}
 69 | 
 70 | 	// set active points
 71 | 	vector<int32_t> active;
 72 | 	if (indist<=0) {
 73 | 	active.clear();
 74 | 	for (int32_t i=0; i<T_num; i++)
 75 | 	  active.push_back(i);
 76 | 	} else {
 77 | 	active = getInliers(T,T_num,R,t,indist);
 78 | 	}
 79 | 
 80 | 	// run icp
 81 | 	fitIterate(T,T_num,R,t,indist);
 82 | 
 83 | 	return getResidual(T,T_num,R,t,m_active);
 84 | }
 85 | 
 86 | void Icp::fitIterate( double *T,const int32_t T_num,Matrix &R,Matrix &t, double indist /*= -1*/ )
 87 | {
 88 | 	if(indist<=0){
 89 | 		m_active.clear();m_active.resize(T_num);
 90 | 		for(int32_t i=0;i<T_num;i++){
 91 | 			m_active[i] = i;
 92 | 		}
 93 | 		m_inlier_ratio = 1;
 94 | 	}
 95 | 	double delta = 1000;
 96 | 	int32_t iter;
 97 | 	for(iter=0; iter<m_max_iter && delta>m_min_delta; iter++){
 98 | 		if(indist>0){
 99 | 			indist = std::max(indist*0.9,0.05);
100 | 			m_active = getInliers(T,T_num,R,t,indist);
101 | 			m_inlier_ratio = (double)m_active.size()/T_num;
102 | 		}
103 | 		delta=fitStep(T,T_num,R,t,m_active);
104 | 	}
105 | }
106 | 


--------------------------------------------------------------------------------
/src/icp.h:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright 2011. All rights reserved.
 3 | Institute of Measurement and Control Systems
 4 | Karlsruhe Institute of Technology, Germany
 5 | 
 6 | Authors: Andreas Geiger
 7 | 
 8 | libicp is free software; you can redistribute it and/or modify it under the
 9 | terms of the GNU General Public License as published by the Free Software
10 | Foundation; either version 2 of the License, or any later version.
11 | 
12 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY
13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
14 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
15 | 
16 | You should have received a copy of the GNU General Public License along with
17 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin
18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 
19 | */
20 | 
21 | #ifndef ICP_H
22 | #define ICP_H
23 | 
24 | #include <stdio.h>
25 | #include <string.h>
26 | #include <stdlib.h>
27 | #include <iostream>
28 | #include <vector>
29 | 
30 | #include "matrix.h"
31 | #include "kdtree.h"
32 | 
33 | class Icp {
34 | 
35 | public:
36 | 
37 | 	// constructor
38 | 	// input: M ....... pointer to first model point
39 | 	//        M_num ... number of model points
40 | 	//        dim   ... dimensionality of model points (2 or 3)
41 | 	Icp (double *M,const int32_t M_num,const int32_t dim);
42 | 
43 | 	// deconstructor
44 | 	virtual ~Icp ();
45 | 
46 | 	// set maximum number of iterations (1. stopping criterion)
47 | 	void setMaxIterations   (int32_t val) { m_max_iter  = val; }
48 | 
49 | 	// set minimum delta of rot/trans parameters (2. stopping criterion)
50 | 	void setMinDeltaParam   (double  val) { m_min_delta = val; }
51 | 
52 | 	double getInlierRatio(){
53 | 		return m_inlier_ratio;
54 | 	}
55 | 
56 | 	double getInlierCount(){
57 | 		return m_active.size();
58 | 	}
59 | 
60 | 	// fit template to model yielding R,t (M = R*T + t)
61 | 	// input:  T ....... pointer to first template point
62 | 	//         T_num ... number of template points
63 | 	//         R ....... initial rotation matrix
64 | 	//         t ....... initial translation vector
65 | 	//         indist .. inlier distance (if <=0: use all points)
66 | 	// output: R ....... final rotation matrix
67 | 	//         t ....... final translation vector
68 | 	double fit(double *T,const int32_t T_num,Matrix &R,Matrix &t,double indist=-1);
69 | 
70 |   
71 | private:
72 | 	// iterative fitting
73 | 	void fitIterate(double *T,const int32_t T_num,Matrix &R,Matrix &t, double indist = -1);
74 | 
75 | 	// inherited classes need to overwrite these functions
76 | 	virtual double               fitStep(double *T,const int32_t T_num,Matrix &R,Matrix &t,const std::vector<int32_t> &active) = 0;
77 | 	virtual std::vector<int32_t> getInliers(double *T,const int32_t T_num,const Matrix &R,const Matrix &t,const double indist) = 0;
78 | 	
79 | 	virtual double getResidual(double *T,const int32_t T_num,const Matrix &R,const Matrix &t,const std::vector<int> &active)=0;
80 |   
81 | protected:
82 |   
83 | 	// kd tree of model points
84 | 	kdtree::KDTree*     m_kd_tree;
85 | 	kdtree::KDTreeArray m_kd_data;
86 | 
87 | 	int32_t m_dim;       // dimensionality of model + template data (2 or 3)
88 | 	int32_t m_max_iter;  // max number of iterations
89 | 	double  m_min_delta; // min parameter delta
90 | 
91 | 	std::vector<int>	m_active; //inliers 
92 | 
93 | 	double	m_inlier_ratio;  // active.size()/ T_num
94 | 	double  m_residual;      // residual of icp
95 | 	Matrix	m_covariance;	 // covariance of the result
96 | };
97 | 
98 | #endif // ICP_H
99 | 


--------------------------------------------------------------------------------
/src/demo.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright 2011. All rights reserved.
  3 | Institute of Measurement and Control Systems
  4 | Karlsruhe Institute of Technology, Germany
  5 | 
  6 | This file is part of libicp.
  7 | Authors: Andreas Geiger
  8 | Updated by Chelsea Scott
  9 | 
 10 | libicp is free software; you can redistribute it and/or modify it under the
 11 | terms of the GNU General Public License as published by the Free Software
 12 | Foundation; either version 3 of the License, or any later version.
 13 | 
 14 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY
 15 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 16 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
 17 | 
 18 | You should have received a copy of the GNU General Public License along with
 19 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin
 20 | Street, Fifth Floor, Boston, MA 02110-1301, USA
 21 | */
 22 | 
 23 | // Demo program showing how libicp can be used
 24 | 
 25 | #include <iostream>
 26 | #include "icpPointToPlane.h"
 27 | #include <list>
 28 | #include <array>
 29 | #include <fstream>  // std::ifstream
 30 | #include <iostream> // std::cout
 31 | #include <stdio.h> // printf/scanf family
 32 | #include "lasreader.hpp"
 33 | #include <vector>
 34 | #include <string>
 35 | 
 36 | using namespace std;
 37 | 
 38 | int main (int argc, char** argv) {
 39 | 
 40 |     if (argc != 3)  {
 41 |         printf("enter 3 arguments only eg. icp input_filename output_filename\n");
 42 |         return 0;
 43 |     }
 44 | 
 45 |     char* inputFileName = argv[1];
 46 |     char* outputFileName = argv[2];
 47 | 
 48 |     //output file is disp.txt
 49 |     //FILE* outfile = fopen("disp.txt","w");
 50 |     FILE* outfile = fopen(outputFileName,"w");
 51 | 
 52 |     //tiles.txt contains the coordinates of the core points
 53 |     //FILE* myfile = fopen("tiles.txt","r");
 54 |     FILE* myfile = fopen(inputFileName,"r");
 55 |     int x, y;
 56 | 
 57 |     //loop through tile.txt
 58 |     while (fscanf(myfile, "%d %d", &x,&y)==2){
 59 | 
 60 |         // write name of compare and reference tile to read
 61 |         char compare_file[50];
 62 |         char reference_file[50];
 63 |         snprintf(compare_file, 50,"las_diff/compare_%d_%d.las",x,y);
 64 |         snprintf(reference_file,50,"las_diff/reference_%d_%d.las",x,y);
 65 | 
 66 |         //read the compare tile
 67 |         LASreadOpener lasreadopener;
 68 |         lasreadopener.set_file_name(compare_file);
 69 |         LASreader* lasreader=lasreadopener.open();
 70 | 
 71 |         int npts = lasreader->npoints;
 72 |         double* M = (double*)malloc(npts*3*sizeof(double));
 73 |         int numM = npts;
 74 | 
 75 |         LASpoint* p;
 76 |         int i=0;
 77 | 
 78 |         while (lasreader -> read_point()) {
 79 |             p = &lasreader->point;
 80 |             M[i] = p->get_x();
 81 |             M[i+1] = p->get_y();
 82 |             M[i+2] = p->get_z();
 83 |             i += 3;
 84 |             }
 85 | 
 86 |         lasreader->close();
 87 |         delete lasreader;
 88 | 
 89 |         //read the reference file
 90 |         LASreadOpener nreadopener;
 91 |         nreadopener.set_file_name(reference_file);
 92 |         LASreader* nreader = nreadopener.open();
 93 |         npts=nreader->npoints;
 94 |         int numN = npts;
 95 | 
 96 |         double* N = (double*)malloc(npts*3*sizeof(double));
 97 |         i=0;
 98 |         while (nreader->read_point()){
 99 |             p= &nreader->point;
100 |             N[i] = p-> get_x();
101 |             N[i+1] = p->get_y();
102 |             N[i+2] = p->get_z();
103 |             i += 3;
104 |         }
105 | 
106 |         nreader->close();
107 |           delete nreader;
108 | 
109 |         // find the mean (x,y,z) of the compare dataset.
110 |         double sum1=0;
111 |         double sum2=0;
112 |         double sum3=0;
113 | 
114 |         for (int k=0; k<numM; k+=1) {
115 |             sum1 += M[k*3];
116 |             sum2 += M[k*3+1];
117 |             sum3 += M[k*3+2];
118 |         }
119 | 
120 |         sum1 = sum1/numM;
121 |         sum2 = sum2/numM;
122 |         sum3 = sum3/numM;
123 | 
124 |         // subtract the mean from the reference and the compare
125 |         for (int k=0; k<numM; k+=1) {
126 |             M[k*3] = M[k*3] - sum1;
127 |             M[k*3+1] = M[k*3+1] - sum2;
128 |             M[k*3+2] = M[k*3+2] - sum3;
129 |         }
130 | 
131 |         for (int k=0; k<numN; k+=1) {
132 |             N[k*3] = N[k*3] - sum1;
133 |             N[k*3+1] = N[k*3+1] - sum2;
134 |             N[k*3+2] = N[k*3+2] - sum3;
135 |         }
136 | 
137 |         // start with identity as initial transformation
138 |         Matrix R = Matrix::eye(3);
139 |         Matrix t(3,1);
140 | 
141 | 
142 |         // run point-to-plane ICP (-1 = no outlier threshold)
143 |         // Modified by Chelsea to add the "if" condition
144 |         // orig: demo-bk-2020-01-21.cpp
145 | 
146 |         if (numN > 50 && numM > 50) {
147 |             IcpPointToPlane icp(N,numN,3);
148 |             double residual = icp.fit(M,numM,R,t,-1);
149 | 
150 |             // write the output file
151 |             fprintf(outfile, "%d %d %f %f %f %f %f %f %f %f %f %f %f %f\n",x,y, t.val[0][0], t.val[1][0], t.val[2][0], R.val[0][0], R.val[0][1], R.val[0][2], R.val[1][0], R.val[1][1], R.val[1][2], R.val[2][0], R.val[2][1], R.val[2][2] );
152 |         }
153 |         // free memory
154 |         free(M);
155 |         free(N);
156 |     }
157 |     fclose(outfile);
158 |     fclose(myfile);
159 |     // success
160 |     return 0;
161 | }
162 | 


--------------------------------------------------------------------------------
/README.TXT:
--------------------------------------------------------------------------------
  1 | ####################################################################################
  2 | # Copyright 2011. All rights reserved.                                             #
  3 | # Institute of Measurement and Control Systems                                     #
  4 | # Karlsruhe Institute of Technology, Germany                                       #
  5 | #                                                                                  #
  6 | # This file is part of libicp.                                                     #
  7 | # Authors:  Andreas Geiger                                                         #
  8 | #           Please send any bugreports to geiger@kit.edu   
  9 | # LASReaders Update: Chelsea Scott - cpscott1@asu.edu            
 10 | #                                                                                  #
 11 | # libicp is free software; you can redistribute it and/or modify it under the      #
 12 | # terms of the GNU General Public License as published by the Free Software        #
 13 | # Foundation; either version 3 of the License, or any later version.               #
 14 | #                                                                                  #
 15 | # libicp is distributed in the hope that it will be useful, but WITHOUT ANY        #
 16 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A  #
 17 | # PARTICULAR PURPOSE. See the GNU General Public License for more details.         #
 18 | #                                                                                  #
 19 | # You should have received a copy of the GNU General Public License along with     #
 20 | # libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin         #
 21 | # Street, Fifth Floor, Boston, MA 02110-1301, USA                                  #
 22 | ####################################################################################
 23 | 
 24 | +++++++++++++++++++++++++++++++++++
 25 | +          INTRODUCTION           +
 26 | +++++++++++++++++++++++++++++++++++
 27 | 
 28 | Libicp (LIBrary for Iterative Closest Point fitting) is a cross-platfrom C++
 29 | library with MATLAB wrappers for fitting 2d or 3d point clouds with respect to
 30 | each other. Currently it implements the SVD-based point-to-point algorithm as well
 31 | as the linearized point-to-plane algorithm. It also supports outlier rejection
 32 | and is accelerated by the use of kd trees as well as a coarse matching stage
 33 | using only a subset of all points.
 34 | 
 35 | If you distribute a software that uses libicp, you have to distribute it under GPL
 36 | with the source code.
 37 | 
 38 | +++++++++++++++++++++++++++++++++++
 39 | +    COMPILING MATLAB WRAPPERS    +
 40 | +++++++++++++++++++++++++++++++++++
 41 | 
 42 | If you want to use libicp directly from MATLAB you can easily do this by using
 43 | the MATLAB wrappers provided. They also include some demo files for testing your
 44 | configuration. First, configure your MATLAB MEX C++ compiler, if it is not yet
 45 | configured (mex -setup). Under Linux you might use g++, under Windows I compiled
 46 | it successfully with the Microsoft Visual Studio Express 2008 compilers.
 47 | 
 48 | 1) Change to the libicp/matlab directory
 49 | 2) After running 'make.m' you should have a MEX file called 'icpMex'
 50 | 3) Now try to run 'demo_2d.m' or 'demo_3d.m'
 51 | 
 52 | +++++++++++++++++++++++++++++++++++
 53 | +     BUILDING A C++ LIBRARY      +
 54 | +++++++++++++++++++++++++++++++++++
 55 | 
 56 | Prerequisites needed for compiling libicp using c++:
 57 | - CMake (available at: http://www.cmake.org/)
 58 | - Boost (available at: http://www.boost.org/)
 59 | 
 60 | Note:
 61 | 
 62 | Please make sure that the boost headers can be found!
 63 | (Add /usr/local/include to your include path if you have
 64 | installed them locally)
 65 | 
 66 | Linux:
 67 | 
 68 | 1) Move to libicp root directory
 69 | 2) Type 'cmake .'
 70 | 3) Type 'make'
 71 | 4) Run './icp' (demo program)
 72 | 
 73 | Windows:
 74 | 
 75 | 1) Start CMake GUI
 76 | 2) Set directories to libicp root directory
 77 | 3) Run configure, configure and generate
 78 | 4) Open the resulting Visual Studio solution with Visual Studio
 79 | 5) Switch to 'Release' mode, build all and run the demo program
 80 | 
 81 | For more information on CMake, have a look at the CMake documentation.
 82 | 
 83 | For more information on the usage of the library, have a look into the MATLAB wrappers and
 84 | into the documentation of the header file icp.h.
 85 | 
 86 | Please send any feedback and bugreports to geiger@kit.edu
 87 | Andreas Geiger
 88 | 
 89 | The demo.cpp script was edited to conduct windowed ICP 3D differencing for this manuscript to Geosphere:  
 90 | 
 91 | Scott, C., Phan, M., Nandigam, V., Crosby, C., and Arrowsmith, J R., 2021, Measuring change at Earth’s surface: On-demand vertical and threedimensional topographic differencing implemented in OpenTopography: Geosphere, v. 17, no. 4, p. 1318–1332, https://doi.org/10.1130/GES02259.1
 92 | 
 93 | Chelsea Scott: cpscott1@asu.edu(corresponding author)
 94 | Minh Phan, Viswanath Nandigam, Christopher Crosby, Ramon Arrowsmith
 95 | 
 96 | To set up to run this script: 
 97 | The following directory and txt file must be created. 
 98 | Create tiled point cloud files of the pre- and post- event data: 
 99 | 
100 | For the compare (pre-event):
101 | las_diff/compare_x1_y1.las
102 | las_diff/compare_x2_y2.las
103 | ...
104 | 
105 | For the reference (post-event):
106 | las_diff/ref_x1_y1.las
107 | las_diff/ref_x2_y2.las
108 | ...
109 | 
110 | tiles.txt in the libicp-master directory
111 | tiles.txt
112 | x1 y1
113 | x2 y2
114 | ...
115 | 
116 | This will output a disp.txt which has the results of the 3D differencing. 
117 | 


--------------------------------------------------------------------------------
/src/matrix.h:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright 2011. All rights reserved.
  3 | Institute of Measurement and Control Systems
  4 | Karlsruhe Institute of Technology, Germany
  5 | 
  6 | Authors: Andreas Geiger
  7 | 
  8 | matrix is free software; you can redistribute it and/or modify it under the
  9 | terms of the GNU General Public License as published by the Free Software
 10 | Foundation; either version 2 of the License, or any later version.
 11 | 
 12 | matrix is distributed in the hope that it will be useful, but WITHOUT ANY
 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
 15 | 
 16 | You should have received a copy of the GNU General Public License along with
 17 | matrix; if not, write to the Free Software Foundation, Inc., 51 Franklin
 18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 
 19 | */
 20 | 
 21 | #ifndef MATRIX_H
 22 | #define MATRIX_H
 23 | 
 24 | #include <stdio.h>
 25 | #include <string.h>
 26 | #include <stdlib.h>
 27 | #include <iostream>
 28 | #include <vector>
 29 | 
 30 | #ifndef _MSC_VER
 31 |   #include <stdint.h>
 32 | #else
 33 |   typedef signed __int8     int8_t;
 34 |   typedef signed __int16    int16_t;
 35 |   typedef signed __int32    int32_t;
 36 |   typedef signed __int64    int64_t;
 37 |   typedef unsigned __int8   uint8_t;
 38 |   typedef unsigned __int16  uint16_t;
 39 |   typedef unsigned __int32  uint32_t;
 40 |   typedef unsigned __int64  uint64_t;
 41 | #endif
 42 | 
 43 | #define endll endl << endl // double end line definition
 44 | 
 45 | typedef double FLOAT;      // double precision
 46 | //typedef float  FLOAT;    // single precision
 47 | 
 48 | class Matrix {
 49 | 
 50 | public:
 51 | 
 52 |   // constructor / deconstructor
 53 |   Matrix ();                                                  // init empty 0x0 matrix
 54 |   Matrix (const int32_t m,const int32_t n);                   // init empty mxn matrix
 55 |   Matrix (const int32_t m,const int32_t n,const FLOAT* val_); // init mxn matrix with values from array 'val'
 56 |   Matrix (const Matrix &M);                                   // creates deepcopy of M
 57 |   ~Matrix ();
 58 | 
 59 |   // assignment operator, copies contents of M
 60 |   Matrix& operator= (const Matrix &M);
 61 | 
 62 |   // copies submatrix of M into array 'val', default values copy whole row/column/matrix
 63 |   void getData(FLOAT* val_,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1);
 64 | 
 65 |   // set or get submatrices of current matrix
 66 |   Matrix getMat(int32_t i1,int32_t j1,int32_t i2=-1,int32_t j2=-1);
 67 |   void   setMat(const Matrix &M,const int32_t i,const int32_t j);
 68 | 
 69 |   // set sub-matrix to scalar (default 0), -1 as end replaces whole row/column/matrix
 70 |   void setVal(FLOAT s,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1);
 71 | 
 72 |   // set (part of) diagonal to scalar, -1 as end replaces whole diagonal
 73 |   void setDiag(FLOAT s,int32_t i1=0,int32_t i2=-1);
 74 | 
 75 |   // clear matrix
 76 |   void zero();
 77 |   
 78 |   // extract columns with given index
 79 |   Matrix extractCols (std::vector<int> idx);
 80 | 
 81 |   // create identity matrix
 82 |   static Matrix eye (const int32_t m);
 83 |   void          eye ();
 84 | 
 85 |   // create matrix with ones
 86 |   static Matrix ones(const int32_t m,const int32_t n);
 87 | 
 88 |   // create diagonal matrix with nx1 or 1xn matrix M as elements
 89 |   static Matrix diag(const Matrix &M);
 90 |   
 91 |   // returns the m-by-n matrix whose elements are taken column-wise from M
 92 |   static Matrix reshape(const Matrix &M,int32_t m,int32_t n);
 93 | 
 94 |   // create 3x3 rotation matrices (convention: http://en.wikipedia.org/wiki/Rotation_matrix)
 95 |   static Matrix rotMatX(const FLOAT &angle);
 96 |   static Matrix rotMatY(const FLOAT &angle);
 97 |   static Matrix rotMatZ(const FLOAT &angle);
 98 | 
 99 |   // simple arithmetic operations
100 |   Matrix  operator+ (const Matrix &M); // add matrix
101 |   Matrix  operator- (const Matrix &M); // subtract matrix
102 |   Matrix  operator* (const Matrix &M); // multiply with matrix
103 |   Matrix  operator* (const FLOAT &s);  // multiply with scalar
104 |   Matrix  operator/ (const Matrix &M); // divide elementwise by matrix (or vector)
105 |   Matrix  operator/ (const FLOAT &s);  // divide by scalar
106 |   Matrix  operator- ();                // negative matrix
107 |   Matrix  operator~ ();                // transpose
108 |   FLOAT   l2norm ();                   // euclidean norm (vectors) / frobenius norm (matrices)
109 |   FLOAT   mean ();                     // mean of all elements in matrix
110 | 
111 |   // complex arithmetic operations
112 |   static Matrix cross (const Matrix &a, const Matrix &b);    // cross product of two vectors
113 |   static Matrix inv (const Matrix &M);                       // invert matrix M
114 |   bool   inv ();                                             // invert this matrix
115 |   FLOAT  det ();                                             // returns determinant of matrix
116 |   bool   solve (const Matrix &M,FLOAT eps=1e-20);            // solve linear system M*x=B, replaces *this and M
117 |   bool   lu(int32_t *idx, FLOAT &d, FLOAT eps=1e-20);        // replace *this by lower upper decomposition
118 |   void   svd(Matrix &U,Matrix &W,Matrix &V);                 // singular value decomposition *this = U*diag(W)*V^T
119 | 
120 |   // print matrix to stream
121 |   friend std::ostream& operator<< (std::ostream& out,const Matrix& M);
122 | 
123 |   // direct data access
124 |   FLOAT   **val;
125 |   int32_t   m,n;
126 | 
127 | private:
128 | 
129 |   void allocateMemory (const int32_t m_,const int32_t n_);
130 |   void releaseMemory ();
131 |   inline FLOAT pythag(FLOAT a,FLOAT b);
132 | 
133 | };
134 | 
135 | #endif // MATRIX_H
136 | 


--------------------------------------------------------------------------------
/src/kdtree.h:
--------------------------------------------------------------------------------
  1 | #ifndef __KDTREE_HPP
  2 | #define __KDTREE_HPP
  3 | 
  4 | // (c) Matthew B. Kennel, Institute for Nonlinear Science, UCSD (2004)
  5 | //
  6 | // Licensed under the Academic Free License version 1.1 found in file LICENSE
  7 | // with additional provisions in that same file.
  8 | //
  9 | // Implement a kd tree for fast searching of points in a fixed data base
 10 | // in k-dimensional Euclidean space.
 11 | 
 12 | #include <vector>
 13 | #include <algorithm>
 14 | 
 15 | #include <boost/multi_array.hpp>
 16 | #include <boost/array.hpp>
 17 | 
 18 | namespace kdtree {
 19 |   
 20 |   typedef boost::multi_array<float, 2>           KDTreeArray;
 21 |   typedef boost::const_multi_array_ref<float, 2> KDTreeROArray;
 22 |   
 23 |   typedef struct {
 24 |     float lower, upper;
 25 |   } interval;
 26 |   
 27 |   // let the compiler know that this is a names of classes.
 28 |   class KDTreeNode;
 29 |   class SearchRecord;  
 30 |   
 31 |   struct KDTreeResult {
 32 |   public:
 33 |     float dis;  // square distance
 34 |     int idx;    // neighbor index
 35 |   };
 36 |   
 37 |   class KDTreeResultVector : public std::vector<KDTreeResult> {
 38 |     // inherit a std::vector<KDTreeResult>
 39 |     // but, optionally maintain it in heap form as a priority
 40 |     // queue.
 41 |   public:
 42 |     
 43 |     //
 44 |     // add one new element to the list of results, and
 45 |     // keep it in heap order.  To keep it in ordinary, as inserted,
 46 |     // order, then simply use push_back() as inherited
 47 |     // via std::vector<>
 48 |     
 49 |     void push_element_and_heapify(KDTreeResult&);
 50 |     float replace_maxpri_elt_return_new_maxpri(KDTreeResult&);
 51 |     
 52 |     float max_value();
 53 |     // return the distance which has the maximum value of all on list,
 54 |     // assuming that ALL insertions were made by
 55 |     // push_element_and_heapify()
 56 |   };
 57 |   
 58 |   class KDTree {
 59 |   public:
 60 |     const KDTreeArray& the_data;
 61 |     // "the_data" is a reference to the underlying multi_array of the
 62 |     // data to be included in the tree.
 63 |     //
 64 |     // NOTE: this structure does *NOT* own the storage underlying this.
 65 |     // Hence, it would be a very bad idea to change the underlying data
 66 |     // during use of the search facilities of this tree.
 67 |     // Also, the user must deallocate the memory underlying it.
 68 |     
 69 |     
 70 |     const int N;          // number of data points
 71 |     int dim;
 72 |     bool sort_results;    // sorting result?
 73 |     const bool rearrange; // are we rearranging?
 74 |     
 75 |   public:
 76 | 
 77 |     // constructor, has optional 'dim_in' feature, to use only
 78 |     // first 'dim_in' components for definition of nearest neighbors.    
 79 |     KDTree(KDTreeArray& data_in, bool rearrange_in = true, int dim_in=-1);
 80 |     
 81 |     // destructor
 82 |     ~KDTree();
 83 |     
 84 |     
 85 |   public:
 86 |     
 87 |     void n_nearest_brute_force(std::vector<float>& qv, int nn, KDTreeResultVector& result);
 88 |     // search for n nearest to a given query vector 'qv' usin
 89 |     // exhaustive slow search.  For debugging, usually.
 90 |     
 91 |     void n_nearest(std::vector<float>& qv, int nn, KDTreeResultVector& result);
 92 |     // search for n nearest to a given query vector 'qv'.
 93 |     
 94 |     void n_nearest_around_point(int idxin, int correltime, int nn,
 95 |             KDTreeResultVector& result);
 96 |     // search for 'nn' nearest to point [idxin] of the input data, excluding
 97 |     // neighbors within correltime
 98 |     
 99 |     void r_nearest(std::vector<float>& qv, float r2, KDTreeResultVector& result);
100 |     // search for all neighbors in ball of size (square Euclidean distance)
101 |     // r2. Return number of neighbors in 'result.size()',
102 |     
103 |     void r_nearest_around_point(int idxin, int correltime, float r2,
104 |             KDTreeResultVector& result);
105 |     // like 'r_nearest', but around existing point, with decorrelation
106 |     // interval.
107 |     
108 |     int r_count(std::vector<float>& qv, float r2);
109 |     // count number of neighbors within square distance r2.
110 |     
111 |     int r_count_around_point(int idxin, int correltime, float r2);
112 |     // like r_count, c
113 |     
114 |     friend class KDTreeNode;
115 |     friend class SearchRecord;
116 |     
117 |   private:
118 |     
119 |     KDTreeNode* root; // the root pointer
120 |     
121 |     const KDTreeArray* data;
122 |     // pointing either to the_data or an internal
123 |     // rearranged data as necessary
124 |     
125 |     std::vector<int> ind;
126 |     // the index for the tree leaves.  Data in a leaf with bounds [l,u] are
127 |     // in  'the_data[ind[l],*] to the_data[ind[u],*]
128 |     
129 |     KDTreeArray rearranged_data;
130 |     // if rearrange is true then this is the rearranged data storage.
131 | 
132 |     static const int bucketsize = 12;  // global constant.
133 |     
134 |   private:
135 |     void set_data(KDTreeArray& din);
136 |     void build_tree(); // builds the tree.  Used upon construction.
137 |     KDTreeNode* build_tree_for_range(int l, int u, KDTreeNode* parent);
138 |     void select_on_coordinate(int c, int k, int l, int u);
139 |     int select_on_coordinate_value(int c, float alpha, int l, int u);
140 |     void spread_in_coordinate(int c, int l, int u, interval& interv);
141 |   };
142 |   
143 |   class KDTreeNode {
144 |   public:
145 |     KDTreeNode(int dim);
146 |     ~KDTreeNode();
147 |     
148 |   private:
149 |     // visible to self and KDTree.
150 |     friend class KDTree;  // allow kdtree to access private data
151 |     
152 |     int cut_dim;                                 // dimension to cut;
153 |     float cut_val, cut_val_left, cut_val_right;  //cut value
154 |     int l, u;  // extents in index array for searching
155 |     
156 |     std::vector<interval> box; // [min,max] of the box enclosing all points
157 |     
158 |     KDTreeNode *left, *right;  // pointers to left and right nodes.
159 |     
160 |     void search(SearchRecord& sr);
161 |     // recursive innermost core routine for searching..
162 |     
163 |     bool box_in_search_range(SearchRecord& sr);
164 |     // return true if the bounding box for this node is within the
165 |     // search range given by the searchvector and maximum ballsize in 'sr'.
166 |     
167 |     void check_query_in_bound(SearchRecord& sr); // debugging only
168 |     
169 |     // for processing final buckets.
170 |     void process_terminal_node(SearchRecord& sr);
171 |     void process_terminal_node_fixedball(SearchRecord& sr); 
172 |   };
173 |   
174 | } // namespace kdtree
175 | 
176 | #endif
177 | 


--------------------------------------------------------------------------------
/src/icpPointToPoint.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright 2011. All rights reserved.
  3 | Institute of Measurement and Control Systems
  4 | Karlsruhe Institute of Technology, Germany
  5 | 
  6 | Authors: Andreas Geiger
  7 | 
  8 | openMP support by Manolis Lourakis, Foundation for Research & Technology - Hellas, Heraklion, Greece
  9 | 
 10 | libicp is free software; you can redistribute it and/or modify it under the
 11 | terms of the GNU General Public License as published by the Free Software
 12 | Foundation; either version 2 of the License, or any later version.
 13 | 
 14 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY
 15 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 16 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
 17 | 
 18 | You should have received a copy of the GNU General Public License along with
 19 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin
 20 | Street, Fifth Floor, Boston, MA 02110-1301, USA 
 21 | */
 22 | 
 23 | #ifdef _OPENMP
 24 | #include <omp.h>
 25 | #endif
 26 | 
 27 | #include "icpPointToPoint.h"
 28 | 
 29 | using namespace std;
 30 | 
 31 | // Also see (3d part): "Least-Squares Fitting of Two 3-D Point Sets" (Arun, Huang and Blostein)
 32 | double IcpPointToPoint::fitStep (double *T,const int32_t T_num,Matrix &R,Matrix &t,const std::vector<int32_t> &active) {
 33 |   
 34 |   int i;
 35 |   int nact = (int)active.size();
 36 | 
 37 |   // init matrix for point correspondences
 38 |   Matrix p_m(nact,m_dim); // model
 39 |   Matrix p_t(nact,m_dim); // template
 40 |   
 41 |   // init mean
 42 |   Matrix mu_m(1,m_dim);
 43 |   Matrix mu_t(1,m_dim);
 44 |   
 45 |   // dimensionality 2
 46 |   if (m_dim==2) {
 47 |     
 48 |     // extract matrix and translation vector
 49 |     double r00 = R.val[0][0]; double r01 = R.val[0][1];
 50 |     double r10 = R.val[1][0]; double r11 = R.val[1][1];
 51 |     double t0  = t.val[0][0]; double t1  = t.val[1][0];
 52 |     double mum0 = 0.0, mum1 = 0.0;
 53 |     double mut0 = 0.0, mut1 = 0.0;
 54 | 
 55 |     // establish correspondences
 56 | #pragma omp parallel for private(i) default(none) shared(T,active,nact,p_m,p_t,r00,r01,r10,r11,t0,t1) reduction(+:mum0,mum1, mut0,mut1) // schedule (dynamic,2)
 57 |     for (i=0; i<nact; i++) {
 58 |       // kd tree query + result
 59 |       std::vector<float>         query(m_dim);
 60 |       kdtree::KDTreeResultVector result;
 61 |   
 62 |       // get index of active point
 63 |       int32_t idx = active[i];
 64 | 
 65 |       // transform point according to R|t
 66 |       query[0] = (float)(r00*T[idx*2+0] + r01*T[idx*2+1] + t0);
 67 |       query[1] = (float)(r10*T[idx*2+0] + r11*T[idx*2+1] + t1);
 68 | 
 69 |       // search nearest neighbor
 70 |       m_kd_tree->n_nearest(query,1,result);
 71 | 
 72 |       // set model point
 73 |       p_m.val[i][0] = m_kd_tree->the_data[result[0].idx][0]; mum0 += p_m.val[i][0];
 74 |       p_m.val[i][1] = m_kd_tree->the_data[result[0].idx][1]; mum1 += p_m.val[i][1];
 75 | 
 76 |       // set template point
 77 |       p_t.val[i][0] = query[0]; mut0 += p_t.val[i][0];
 78 |       p_t.val[i][1] = query[1]; mut1 += p_t.val[i][1];
 79 |     }
 80 |     mu_m.val[0][0] = mum0;
 81 |     mu_m.val[0][1] = mum1;
 82 | 
 83 |     mu_t.val[0][0] = mut0;
 84 |     mu_t.val[0][1] = mut1;
 85 |     
 86 |   // dimensionality 3
 87 |   } else {
 88 |     
 89 |     // extract matrix and translation vector
 90 |     double r00 = R.val[0][0]; double r01 = R.val[0][1]; double r02 = R.val[0][2];
 91 |     double r10 = R.val[1][0]; double r11 = R.val[1][1]; double r12 = R.val[1][2];
 92 |     double r20 = R.val[2][0]; double r21 = R.val[2][1]; double r22 = R.val[2][2];
 93 |     double t0  = t.val[0][0]; double t1  = t.val[1][0]; double t2  = t.val[2][0];
 94 |     double mum0 = 0.0, mum1 = 0.0, mum2 = 0.0;
 95 |     double mut0 = 0.0, mut1 = 0.0, mut2 = 0.0;
 96 | 
 97 |     // establish correspondences
 98 | #pragma omp parallel for private(i) default(none) shared(T,active,nact,p_m,p_t,r00,r01,r02,r10,r11,r12,r20,r21,r22,t0,t1,t2) reduction(+:mum0,mum1,mum2, mut0,mut1,mut2) // schedule (dynamic,2)
 99 |     for (i=0; i<nact; i++) {
100 |       // kd tree query + result
101 |       std::vector<float>         query(m_dim);
102 |       kdtree::KDTreeResultVector result;
103 | 
104 |       // get index of active point
105 |       int32_t idx = active[i];
106 | 
107 |       // transform point according to R|t
108 |       query[0] = (float)(r00*T[idx*3+0] + r01*T[idx*3+1] + r02*T[idx*3+2] + t0);
109 |       query[1] = (float)(r10*T[idx*3+0] + r11*T[idx*3+1] + r12*T[idx*3+2] + t1);
110 |       query[2] = (float)(r20*T[idx*3+0] + r21*T[idx*3+1] + r22*T[idx*3+2] + t2);
111 | 
112 |       // search nearest neighbor
113 |       m_kd_tree->n_nearest(query,1,result);
114 | 
115 |       // set model point
116 |       p_m.val[i][0] = m_kd_tree->the_data[result[0].idx][0]; mum0 += p_m.val[i][0];
117 |       p_m.val[i][1] = m_kd_tree->the_data[result[0].idx][1]; mum1 += p_m.val[i][1];
118 |       p_m.val[i][2] = m_kd_tree->the_data[result[0].idx][2]; mum2 += p_m.val[i][2];
119 | 
120 |       // set template point
121 |       p_t.val[i][0] = query[0]; mut0 += p_t.val[i][0];
122 |       p_t.val[i][1] = query[1]; mut1 += p_t.val[i][1];
123 |       p_t.val[i][2] = query[2]; mut2 += p_t.val[i][2];
124 |     }
125 |     mu_m.val[0][0] = mum0;
126 |     mu_m.val[0][1] = mum1;
127 |     mu_m.val[0][2] = mum2;
128 | 
129 |     mu_t.val[0][0] = mut0;
130 |     mu_t.val[0][1] = mut1;
131 |     mu_t.val[0][2] = mut2;
132 |   }
133 |   
134 |   // subtract mean
135 |   mu_m = mu_m/(double)active.size();
136 |   mu_t = mu_t/(double)active.size();
137 |   Matrix q_m = p_m - Matrix::ones(active.size(),1)*mu_m;
138 |   Matrix q_t = p_t - Matrix::ones(active.size(),1)*mu_t;
139 | 
140 |   // compute relative rotation matrix R and translation vector t
141 |   Matrix H = ~q_t*q_m;
142 |   Matrix U,W,V;
143 |   H.svd(U,W,V);
144 |   Matrix R_ = V*~U;
145 | 
146 |   // fix improper matrix problem
147 |   if (R_.det()<0){
148 |     Matrix B = Matrix::eye(m_dim);
149 |     B.val[m_dim-1][m_dim-1] = R_.det();
150 |     R_ = V*B*~U;
151 |   }
152 | 
153 |   Matrix t_ = ~mu_m - R_*~mu_t;
154 |   
155 |   // compose: R|t = R_|t_ * R|t
156 |   R = R_*R;
157 |   t = R_*t+t_;
158 | 
159 |   // return max delta in parameters
160 |   if (m_dim==2) return max((R_-Matrix::eye(2)).l2norm(),t_.l2norm());
161 |   else        return max((R_-Matrix::eye(3)).l2norm(),t_.l2norm());
162 | }
163 | 
164 | std::vector<int32_t> IcpPointToPoint::getInliers (double *T,const int32_t T_num,const Matrix &R,const Matrix &t,const double indist) {
165 | 
166 |   // init inlier vector + query point + query result
167 |   vector<int32_t>            inliers;
168 |   std::vector<float>         query(m_dim);
169 |   kdtree::KDTreeResultVector neighbor;
170 |   
171 |   // dimensionality 2
172 |   if (m_dim==2) {
173 |   
174 |     // extract matrix and translation vector
175 |     double r00 = R.val[0][0]; double r01 = R.val[0][1];
176 |     double r10 = R.val[1][0]; double r11 = R.val[1][1];
177 |     double t0  = t.val[0][0]; double t1  = t.val[1][0];
178 | 
179 |     // check for all points if they are inliers
180 |     for (int32_t i=0; i<T_num; i++) {
181 | 
182 |       // transform point according to R|t
183 |       query[0] = (float)(r00*T[i*2+0] + r01*T[i*2+1] + t0);
184 |       query[1] = (float)(r10*T[i*2+0] + r11*T[i*2+1] + t1);
185 | 
186 |       // search nearest neighbor
187 |       m_kd_tree->n_nearest(query,1,neighbor);
188 | 
189 |       // check if it is an inlier
190 |       if (neighbor[0].dis<indist)
191 |         inliers.push_back(i);
192 |     }
193 |     
194 |   // dimensionality 3
195 |   } else {
196 |     
197 |     // extract matrix and translation vector
198 |     double r00 = R.val[0][0]; double r01 = R.val[0][1]; double r02 = R.val[0][2];
199 |     double r10 = R.val[1][0]; double r11 = R.val[1][1]; double r12 = R.val[1][2];
200 |     double r20 = R.val[2][0]; double r21 = R.val[2][1]; double r22 = R.val[2][2];
201 |     double t0  = t.val[0][0]; double t1  = t.val[1][0]; double t2  = t.val[2][0];
202 | 
203 |     // check for all points if they are inliers
204 |     for (int32_t i=0; i<T_num; i++) {
205 | 
206 |       // transform point according to R|t
207 |       query[0] = (float)(r00*T[i*3+0] + r01*T[i*3+1] + r02*T[i*3+2] + t0);
208 |       query[1] = (float)(r10*T[i*3+0] + r11*T[i*3+1] + r12*T[i*3+2] + t1);
209 |       query[2] = (float)(r20*T[i*3+0] + r21*T[i*3+1] + r22*T[i*3+2] + t2);
210 | 
211 |       // search nearest neighbor
212 |       m_kd_tree->n_nearest(query,1,neighbor);
213 | 
214 |       // check if it is an inlier
215 |       if (neighbor[0].dis<indist)
216 |         inliers.push_back(i);
217 |     }
218 |   }
219 |   
220 |   // return vector with inliers
221 |   return inliers;
222 | }
223 | 
224 | double IcpPointToPoint::getResidual( double *T,const int32_t T_num,const Matrix &R,const Matrix &t,const std::vector<int> &active )
225 | {
226 | 	if(active.empty()) return 0;
227 | 	int nact = active.size();
228 | 	double residual = 0;
229 | 
230 | 	if (m_dim==2) {
231 | 		// extract matrix and translation vector
232 | 		double r00 = R.val[0][0]; double r01 = R.val[0][1];
233 | 		double r10 = R.val[1][0]; double r11 = R.val[1][1];
234 | 		double t0  = t.val[0][0]; double t1  = t.val[1][0];
235 | 		for (int i=0; i<nact; i++) {
236 | 			// kd tree query + result
237 | 			std::vector<float>         query(m_dim);
238 | 			kdtree::KDTreeResultVector result;
239 | 
240 | 			// get index of active point
241 | 			int32_t idx = active[i];
242 | 
243 | 			// transform point according to R|t
244 | 			query[0] = (float)(r00*T[idx*2+0] + r01*T[idx*2+1] + t0);
245 | 			query[1] = (float)(r10*T[idx*2+0] + r11*T[idx*2+1] + t1);
246 | 
247 | 			// search nearest neighbor
248 | 			m_kd_tree->n_nearest(query,1,result);
249 | 			residual += result[0].dis;
250 | 		}
251 | 		// dimensionality 3
252 | 	} else {
253 | 		// extract matrix and translation vector
254 | 		double r00 = R.val[0][0]; double r01 = R.val[0][1]; double r02 = R.val[0][2];
255 | 		double r10 = R.val[1][0]; double r11 = R.val[1][1]; double r12 = R.val[1][2];
256 | 		double r20 = R.val[2][0]; double r21 = R.val[2][1]; double r22 = R.val[2][2];
257 | 		double t0  = t.val[0][0]; double t1  = t.val[1][0]; double t2  = t.val[2][0];
258 | 		for (int i=0; i<nact; i++) {
259 | 			// kd tree query + result
260 | 			std::vector<float>         query(m_dim);
261 | 			kdtree::KDTreeResultVector result;
262 | 			// get index of active point
263 | 			int32_t idx = active[i];
264 | 			// transform point according to R|t
265 | 			query[0] = (float)(r00*T[idx*3+0] + r01*T[idx*3+1] + r02*T[idx*3+2] + t0);
266 | 			query[1] = (float)(r10*T[idx*3+0] + r11*T[idx*3+1] + r12*T[idx*3+2] + t1);
267 | 			query[2] = (float)(r20*T[idx*3+0] + r21*T[idx*3+1] + r22*T[idx*3+2] + t2);
268 | 			// search nearest neighbor
269 | 			m_kd_tree->n_nearest(query,1,result);
270 | 			residual += result[0].dis;
271 | 		}
272 | 	}
273 | 	residual /= nact;
274 | 	return residual;
275 | }
276 | 


--------------------------------------------------------------------------------
/src/icpPointToPlane.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright 2011. All rights reserved.
  3 | Institute of Measurement and Control Systems
  4 | Karlsruhe Institute of Technology, Germany
  5 | 
  6 | Authors: Andreas Geiger
  7 | 
  8 | openMP support by Manolis Lourakis, Foundation for Research & Technology - Hellas, Heraklion, Greece
  9 | 
 10 | libicp is free software; you can redistribute it and/or modify it under the
 11 | terms of the GNU General Public License as published by the Free Software
 12 | Foundation; either version 2 of the License, or any later version.
 13 | 
 14 | libicp is distributed in the hope that it will be useful, but WITHOUT ANY
 15 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 16 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
 17 | 
 18 | You should have received a copy of the GNU General Public License along with
 19 | libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin
 20 | Street, Fifth Floor, Boston, MA 02110-1301, USA 
 21 | */
 22 | 
 23 | #ifdef _OPENMP
 24 | #include <omp.h>
 25 | #endif
 26 | 
 27 | #include "icpPointToPlane.h"
 28 | 
 29 | using namespace std;
 30 | 
 31 | // Also see (3d part): "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration" (Kok-Lim Low)
 32 | double IcpPointToPlane::fitStep (double *T,const int32_t T_num,Matrix &R,Matrix &t,const std::vector<int32_t> &active) {
 33 | 
 34 |   int32_t i;
 35 |   int32_t nact = (int)active.size();
 36 | 
 37 |   // init matrix for point32_t correspondences
 38 |   Matrix p_m(nact,m_dim); // model
 39 |   Matrix p_t(nact,m_dim); // template
 40 |   
 41 |   // dimensionality 2
 42 |   if (m_dim==2) {
 43 | 
 44 |     // extract matrix and translation vector
 45 |     double r00 = R.val[0][0]; double r01 = R.val[0][1];
 46 |     double r10 = R.val[1][0]; double r11 = R.val[1][1];
 47 |     double t0  = t.val[0][0]; double t1  = t.val[1][0];
 48 | 
 49 |     // init A and b
 50 |     Matrix A(nact,3);
 51 |     Matrix b(nact,1);
 52 | 
 53 |     // establish correspondences
 54 | #pragma omp parallel for private(i) default(none) shared(T,active,nact,p_m,p_t,A,b,r00,r01,r10,r11,t0,t1) // schedule (dynamic,2)
 55 |     for (i=0; i<nact; i++) {
 56 |       // kd tree query + result
 57 |       std::vector<float>         query(m_dim);
 58 |       kdtree::KDTreeResultVector result;
 59 | 
 60 |       // get index of active point
 61 |       int32_t idx = active[i];
 62 | 
 63 |       // transform point according to R|t
 64 |       query[0] = (float)(r00*T[idx*2+0] + r01*T[idx*2+1] + t0);
 65 |       query[1] = (float)(r10*T[idx*2+0] + r11*T[idx*2+1] + t1);
 66 | 
 67 |       // search nearest neighbor
 68 |       m_kd_tree->n_nearest(query,1,result);
 69 | 
 70 |       // model point
 71 |       double dx = m_kd_tree->the_data[result[0].idx][0];
 72 |       double dy = m_kd_tree->the_data[result[0].idx][1];
 73 | 
 74 |       // model point normal
 75 |       double nx = M_normal[result[0].idx*2+0];
 76 |       double ny = M_normal[result[0].idx*2+1];
 77 | 
 78 |       // template point
 79 |       double sx = query[0];
 80 |       double sy = query[1];
 81 | 
 82 |       // setup least squares system
 83 |       A.val[i][0] = ny*sx-nx*sy;
 84 |       A.val[i][1] = nx;
 85 |       A.val[i][2] = ny;
 86 |       b.val[i][0] = nx*dx+ny*dy-nx*sx-ny*sy;    
 87 |     }
 88 | 
 89 |     // linear least square matrices
 90 |     Matrix A_ = ~A*A;
 91 |     Matrix b_ = ~A*b;
 92 | 
 93 |     // solve linear system
 94 |     if (b_.solve(A_)) {
 95 | 
 96 |       // rotation matrix
 97 |       Matrix R_ = Matrix::eye(2);
 98 |       R_.val[0][1] = -b_.val[0][0];
 99 |       R_.val[1][0] = +b_.val[0][0];
100 | 
101 |       // orthonormalized rotation matrix
102 |       Matrix U,W,V;
103 |       R_.svd(U,W,V);
104 |       R_ = U*~V;  
105 | 
106 |       // fix improper matrix problem
107 |       if (R_.det()<0){
108 |         Matrix B = Matrix::eye(m_dim);
109 |         B.val[m_dim-1][m_dim-1] = R_.det();
110 |         R_ = V*B*~U;
111 |       }
112 | 
113 |       // translation vector
114 |       Matrix t_(2,1);
115 |       t_.val[0][0] = b_.val[1][0];
116 |       t_.val[1][0] = b_.val[2][0];
117 | 
118 |       // compose: R|t = R_|t_ * R|t
119 |       R = R_*R;
120 |       t = R_*t+t_;
121 | 	  
122 |       return max((R_-Matrix::eye(2)).l2norm(),t_.l2norm());
123 |     }
124 |    
125 |   // dimensionality 3
126 |   } else {
127 |     
128 |     // extract matrix and translation vector
129 |     double r00 = R.val[0][0]; double r01 = R.val[0][1]; double r02 = R.val[0][2];
130 |     double r10 = R.val[1][0]; double r11 = R.val[1][1]; double r12 = R.val[1][2];
131 |     double r20 = R.val[2][0]; double r21 = R.val[2][1]; double r22 = R.val[2][2];
132 |     double t0  = t.val[0][0]; double t1  = t.val[1][0]; double t2  = t.val[2][0];
133 | 
134 |     // init A and b
135 |     Matrix A(nact,6);
136 |     Matrix b(nact,1);
137 | 
138 |     // establish correspondences
139 | #pragma omp parallel for private(i) default(none) shared(T,active,nact,p_m,p_t,A,b,r00,r01,r02,r10,r11,r12,r20,r21,r22,t0,t1,t2) // schedule (dynamic,2)
140 |     for (i=0; i<nact; i++) {
141 |       // kd tree query + result
142 |       std::vector<float>         query(m_dim);
143 |       kdtree::KDTreeResultVector result;
144 | 
145 |       // get index of active point
146 |       int32_t idx = active[i];
147 | 
148 |       // transform point according to R|t
149 |       query[0] = (float)(r00*T[idx*3+0] + r01*T[idx*3+1] + r02*T[idx*3+2] + t0);
150 |       query[1] = (float)(r10*T[idx*3+0] + r11*T[idx*3+1] + r12*T[idx*3+2] + t1);
151 |       query[2] = (float)(r20*T[idx*3+0] + r21*T[idx*3+1] + r22*T[idx*3+2] + t2);
152 | 
153 |       // search nearest neighbor
154 |       m_kd_tree->n_nearest(query,1,result);
155 | 
156 |       // model point
157 |       double dx = m_kd_tree->the_data[result[0].idx][0];
158 |       double dy = m_kd_tree->the_data[result[0].idx][1];
159 |       double dz = m_kd_tree->the_data[result[0].idx][2];
160 | 
161 |       // model point normal
162 |       double nx = M_normal[result[0].idx*3+0];
163 |       double ny = M_normal[result[0].idx*3+1];
164 |       double nz = M_normal[result[0].idx*3+2];
165 | 
166 |       // template point
167 |       double sx = query[0];
168 |       double sy = query[1];
169 |       double sz = query[2];
170 | 
171 |       // setup least squares system
172 |       A.val[i][0] = nz*sy-ny*sz;
173 |       A.val[i][1] = nx*sz-nz*sx;
174 |       A.val[i][2] = ny*sx-nx*sy;
175 |       A.val[i][3] = nx;
176 |       A.val[i][4] = ny;
177 |       A.val[i][5] = nz;
178 |       b.val[i][0] = nx*dx+ny*dy+nz*dz-nx*sx-ny*sy-nz*sz;    
179 |     }
180 | 
181 |     // linear least square matrices
182 |     Matrix A_ = ~A*A;
183 |     Matrix b_ = ~A*b;
184 | 
185 |     // solve linear system
186 |     if (b_.solve(A_)) {
187 | 
188 |       // rotation matrix
189 |       Matrix R_ = Matrix::eye(3);
190 |       R_.val[0][1] = -b_.val[2][0];
191 |       R_.val[1][0] = +b_.val[2][0];
192 |       R_.val[0][2] = +b_.val[1][0];
193 |       R_.val[2][0] = -b_.val[1][0];
194 |       R_.val[1][2] = -b_.val[0][0];
195 |       R_.val[2][1] = +b_.val[0][0];
196 | 
197 |       // orthonormalized rotation matrix
198 |       Matrix U,W,V;
199 |       R_.svd(U,W,V);
200 |       R_ = U*~V;  
201 | 
202 |       // fix improper matrix problem
203 |       if (R_.det()<0){
204 |         Matrix B = Matrix::eye(m_dim);
205 |         B.val[m_dim-1][m_dim-1] = R_.det();
206 |         R_ = V*B*~U;
207 |       }
208 | 
209 |       // translation vector
210 |       Matrix t_(3,1);
211 |       t_.val[0][0] = b_.val[3][0];
212 |       t_.val[1][0] = b_.val[4][0];
213 |       t_.val[2][0] = b_.val[5][0];
214 | 
215 |       // compose: R|t = R_|t_ * R|t
216 |       R = R_*R;
217 |       t = R_*t+t_;
218 |       return max((R_-Matrix::eye(3)).l2norm(),t_.l2norm());
219 |     }
220 |   }
221 |   
222 |   // failure
223 |   return 0;
224 | }
225 | 
226 | std::vector<int32_t> IcpPointToPlane::getInliers (double *T,const int32_t T_num,const Matrix &R,const Matrix &t,const double indist) {
227 |   
228 |   // init inlier vector + query point + query result
229 |   vector<int32_t>            inliers;
230 |   std::vector<float>         query(m_dim);
231 |   kdtree::KDTreeResultVector neighbor;
232 |   
233 |   // dimensionality 2
234 |   if (m_dim==2) {
235 |   
236 |     // extract matrix and translation vector
237 |     double r00 = R.val[0][0]; double r01 = R.val[0][1];
238 |     double r10 = R.val[1][0]; double r11 = R.val[1][1];
239 |     double t0  = t.val[0][0]; double t1  = t.val[1][0];
240 | 
241 |     // check for all points if they are inliers
242 |     for (int32_t i=0; i<T_num; i++) {
243 | 
244 |       // transform point according to R|t
245 |       double sx = r00*T[i*2+0] + r01*T[i*2+1] + t0; query[0] = (float)sx;
246 |       double sy = r10*T[i*2+0] + r11*T[i*2+1] + t1; query[1] = (float)sy;
247 | 
248 |       // search nearest neighbor
249 |       m_kd_tree->n_nearest(query,1,neighbor);
250 | 
251 |       // model point
252 |       double dx = m_kd_tree->the_data[neighbor[0].idx][0];
253 |       double dy = m_kd_tree->the_data[neighbor[0].idx][1];
254 | 
255 |       // model point normal
256 |       double nx = M_normal[neighbor[0].idx*2+0];
257 |       double ny = M_normal[neighbor[0].idx*2+1];
258 | 
259 |       // check if it is an inlier
260 |       if (/*neighbor[0].dis<3*indist && */abs((sx-dx)*nx+(sy-dy)*ny)<indist)
261 |         inliers.push_back(i);
262 |     }
263 |     
264 |   // dimensionality 3
265 |   } else {
266 |     
267 |     // extract matrix and translation vector
268 |     double r00 = R.val[0][0]; double r01 = R.val[0][1]; double r02 = R.val[0][2];
269 |     double r10 = R.val[1][0]; double r11 = R.val[1][1]; double r12 = R.val[1][2];
270 |     double r20 = R.val[2][0]; double r21 = R.val[2][1]; double r22 = R.val[2][2];
271 |     double t0  = t.val[0][0]; double t1  = t.val[1][0]; double t2  = t.val[2][0];
272 | 
273 |     // check for all points if they are inliers
274 |     for (int32_t i=0; i<T_num; i++) {
275 | 
276 |       // transform point according to R|t
277 |       double sx = r00*T[i*3+0] + r01*T[i*3+1] + r02*T[i*3+2] + t0; query[0] = (float)sx;
278 |       double sy = r10*T[i*3+0] + r11*T[i*3+1] + r12*T[i*3+2] + t1; query[1] = (float)sy;
279 |       double sz = r20*T[i*3+0] + r21*T[i*3+1] + r22*T[i*3+2] + t2; query[2] = (float)sz;
280 | 
281 |       // search nearest neighbor
282 |       m_kd_tree->n_nearest(query,1,neighbor);
283 | 
284 |       // model point
285 |       double dx = m_kd_tree->the_data[neighbor[0].idx][0];
286 |       double dy = m_kd_tree->the_data[neighbor[0].idx][1];
287 |       double dz = m_kd_tree->the_data[neighbor[0].idx][2];
288 | 
289 |       // model point normal
290 |       double nx = M_normal[neighbor[0].idx*3+0];
291 |       double ny = M_normal[neighbor[0].idx*3+1];
292 |       double nz = M_normal[neighbor[0].idx*3+2];
293 | 
294 |       // check if it is an inlier
295 |       if (abs((sx-dx)*nx+(sy-dy)*ny+(sz-dz))*nz<indist)
296 |         inliers.push_back(i);
297 |     }
298 |   }
299 |   
300 |   // return vector with inliers
301 |   return inliers;
302 | }
303 | 
304 | void IcpPointToPlane::computeNormal (const kdtree::KDTreeResultVector &neighbors,double *M_normal,const double flatness) {
305 |   
306 |   // dimensionality 2
307 |   if (m_dim==2) {
308 |     
309 |     // extract neighbors
310 |     Matrix P(neighbors.size(),2);
311 |     Matrix mu(1,2);
312 |     for (uint32_t i=0; i<neighbors.size(); i++) {
313 |       double x = m_kd_tree->the_data[neighbors[i].idx][0];
314 |       double y = m_kd_tree->the_data[neighbors[i].idx][1];
315 |       P.val[i][0] = x;
316 |       P.val[i][1] = y;
317 |       mu.val[0][0] += x;
318 |       mu.val[0][1] += y;
319 |     }
320 |     // zero mean
321 |     mu       = mu/(double)neighbors.size();
322 |     Matrix Q = P - Matrix::ones(neighbors.size(),1)*mu;
323 | 
324 |     // principal component analysis
325 |     Matrix H = ~Q*Q;
326 |     Matrix U,W,V;
327 |     H.svd(U,W,V);
328 | 
329 |     // normal
330 |     M_normal[0] = U.val[0][1];
331 |     M_normal[1] = U.val[1][1];
332 |   
333 |   // dimensionality 3
334 |   } else {
335 |     
336 |     // extract neighbors
337 |     Matrix P(neighbors.size(),3);
338 |     Matrix mu(1,3);
339 |     for (uint32_t i=0; i<neighbors.size(); i++) {
340 |       double x = m_kd_tree->the_data[neighbors[i].idx][0];
341 |       double y = m_kd_tree->the_data[neighbors[i].idx][1];
342 |       double z = m_kd_tree->the_data[neighbors[i].idx][2];
343 |       P.val[i][0] = x;
344 |       P.val[i][1] = y;
345 |       P.val[i][2] = z;
346 |       mu.val[0][0] += x;
347 |       mu.val[0][1] += y;
348 |       mu.val[0][2] += z;
349 |     }
350 | 
351 |     // zero mean
352 |     mu       = mu/(double)neighbors.size();
353 |     Matrix Q = P - Matrix::ones(neighbors.size(),1)*mu;
354 | 
355 |     // principal component analysis
356 |     Matrix H = ~Q*Q;
357 |     Matrix U,W,V;
358 |     H.svd(U,W,V);
359 | 
360 |     // normal
361 |     M_normal[0] = U.val[0][2];
362 |     M_normal[1] = U.val[1][2];
363 |     M_normal[2] = U.val[2][2];
364 |   }
365 | }
366 | 
367 | double* IcpPointToPlane::computeNormals (const int32_t num_neighbors,const double flatness) {
368 |   double *M_normal = (double*)malloc(m_kd_tree->N*m_dim*sizeof(double));
369 |   kdtree::KDTreeResultVector neighbors;
370 |   for (int32_t i=0; i<m_kd_tree->N; i++) {
371 |     m_kd_tree->n_nearest_around_point(i,0,num_neighbors,neighbors);
372 |     // m_kd_tree->r_nearest_around_point(i,0,0.05,neighbors);
373 |     if (m_dim==2) computeNormal(neighbors,M_normal+i*2,flatness);
374 |     else        computeNormal(neighbors,M_normal+i*3,flatness);
375 |   }
376 | 
377 |   return M_normal;
378 | }
379 | 
380 | double IcpPointToPlane::getResidual( double *T,const int32_t T_num,const Matrix &R,const Matrix &t,const std::vector<int> &active )
381 | {
382 | 	if(active.empty()) return 0;
383 | 	int nact = active.size();
384 | 	double residual = 0;
385 | 
386 | 	std::vector<float>         query(m_dim);
387 | 	kdtree::KDTreeResultVector result;
388 | 
389 | 	if (m_dim==2) {
390 | 		// extract matrix and translation vector
391 | 		double r00 = R.val[0][0]; double r01 = R.val[0][1];
392 | 		double r10 = R.val[1][0]; double r11 = R.val[1][1];
393 | 		double t0  = t.val[0][0]; double t1  = t.val[1][0];
394 | 		for (int32_t i=0; i<nact; i++) {
395 | 			// kd tree query + result
396 | 			int32_t idx = active[i];
397 | 			
398 | 			// transform point according to R|t
399 | 			double tx = r00*T[idx*2+0] + r01*T[idx*2+1] + t0;
400 | 			double ty = r10*T[idx*2+0] + r11*T[idx*2+1] + t1;
401 | 			query[0] = (float)tx;
402 | 			query[1] = (float)ty;
403 | 
404 | 			// search nearest neighbor
405 | 			m_kd_tree->n_nearest(query,1,result);
406 | 			// model point
407 | 			double mx = m_kd_tree->the_data[result[0].idx][0];
408 | 			double my = m_kd_tree->the_data[result[0].idx][1];
409 | 			// model point normal
410 | 			double nx = M_normal[result[0].idx*2+0];
411 | 			double ny = M_normal[result[0].idx*2+1];
412 | 			
413 | 			residual += abs(nx * (mx-tx) + ny * (my-ty));
414 | 
415 | 		}
416 | 		// dimensionality 3
417 | 	} else {
418 | 		// extract matrix and translation vector
419 | 		double r00 = R.val[0][0]; double r01 = R.val[0][1]; double r02 = R.val[0][2];
420 | 		double r10 = R.val[1][0]; double r11 = R.val[1][1]; double r12 = R.val[1][2];
421 | 		double r20 = R.val[2][0]; double r21 = R.val[2][1]; double r22 = R.val[2][2];
422 | 		double t0  = t.val[0][0]; double t1  = t.val[1][0]; double t2  = t.val[2][0];
423 | 		for (int32_t i=0; i<nact; i++) {
424 | 			// kd tree query + result
425 | 			std::vector<float>         query(m_dim);
426 | 			kdtree::KDTreeResultVector result;
427 | 			// get index of active point
428 | 			int32_t idx = active[i];
429 | 			// transform point according to R|t
430 | 			double tx = r00*T[idx*3+0] + r01*T[idx*3+1] + r02*T[idx*3+2] + t0;
431 | 			double ty = r10*T[idx*3+0] + r11*T[idx*3+1] + r12*T[idx*3+2] + t1;
432 | 			double tz = r20*T[idx*3+0] + r21*T[idx*3+1] + r22*T[idx*3+2] + t2;
433 | 			query[0] = (float)tx;
434 | 			query[1] = (float)ty;
435 | 			query[2] = (float)tz;
436 | 			// search nearest neighbor
437 | 			m_kd_tree->n_nearest(query,1,result);
438 | 			//
439 | 			// model point
440 | 			double mx = m_kd_tree->the_data[result[0].idx][0];
441 | 			double my = m_kd_tree->the_data[result[0].idx][1];
442 | 			double mz = m_kd_tree->the_data[result[0].idx][2];
443 | 
444 | 			// model point normal
445 | 			double nx = M_normal[result[0].idx*3+0];
446 | 			double ny = M_normal[result[0].idx*3+1];
447 | 			double nz = M_normal[result[0].idx*3+2];
448 | 
449 | 			residual += nx * (mx-tx) + ny * (my-ty) + nz * (mz-tz);
450 | 		}
451 | 	}
452 | 	residual /= nact;
453 | 	return residual;
454 | }
455 | 
456 | 
457 | 


--------------------------------------------------------------------------------
/src/kdtree.cpp:
--------------------------------------------------------------------------------
  1 | //
  2 | // (c) Matthew B. Kennel, Institute for Nonlinear Science, UCSD (2004)
  3 | //
  4 | // Licensed under the Academic Free License version 1.1 found in file LICENSE
  5 | // with additional provisions in that same file.
  6 | 
  7 | 
  8 | #include "kdtree.h"
  9 | 
 10 | #include <stdio.h>
 11 | #include <algorithm>
 12 | #include <iostream>
 13 | 
 14 | namespace kdtree {
 15 | // utility
 16 |   
 17 |   inline float squared(const float x) {
 18 |     return(x*x);
 19 |   }
 20 |   
 21 |   inline void swap(int& a, int&b) {
 22 |     int tmp;
 23 |     tmp = a;
 24 |     a = b;
 25 |     b = tmp;
 26 |   }
 27 |   
 28 |   inline void swap(float& a, float&b) {
 29 |     float tmp;
 30 |     tmp = a;
 31 |     a = b;
 32 |     b = tmp;
 33 |   }
 34 |   
 35 |   //
 36 |   //       KDTREERESULT implementation
 37 |   //
 38 |   inline bool operator<(const KDTreeResult& e1, const KDTreeResult& e2) {
 39 |     return (e1.dis < e2.dis);
 40 |   }
 41 |   
 42 |   //
 43 |   //       KDTREE2_RESULT_VECTOR implementation
 44 |   //
 45 |   float KDTreeResultVector::max_value() {
 46 |     return( (*begin()).dis ); // very first element
 47 |   }
 48 |   
 49 |   void KDTreeResultVector::push_element_and_heapify(KDTreeResult& e) {
 50 |     push_back(e); // what a vector does.
 51 |     push_heap( begin(), end() ); // and now heapify it, with the new elt.
 52 |   }
 53 |   
 54 |   float KDTreeResultVector::replace_maxpri_elt_return_new_maxpri(KDTreeResult& e) {
 55 |     // remove the maximum priority element on the queue and replace it
 56 |     // with 'e', and return its priority.
 57 |     //
 58 |     // here, it means replacing the first element [0] with e, and re heapifying.
 59 |     
 60 |     pop_heap( begin(), end() );
 61 |     pop_back();
 62 |     push_back(e); // insert new
 63 |     push_heap(begin(), end() );  // and heapify.
 64 |     return( (*this)[0].dis );
 65 |   }
 66 |   
 67 |   //
 68 |   //        KDTREE2 implementation
 69 |   //
 70 | 
 71 |   // constructor
 72 |   KDTree::KDTree(KDTreeArray& data_in, bool rearrange_in, int dim_in)
 73 |   : the_data(data_in),
 74 |           N  ( data_in.shape()[0] ),
 75 |           dim( data_in.shape()[1] ),
 76 |           sort_results(false),
 77 |           rearrange(rearrange_in),
 78 |           root(NULL),
 79 |           data(NULL),
 80 |           ind(N) {
 81 |     //
 82 |     // initialize the constant references using this unusual C++
 83 |     // feature.
 84 |     //
 85 |     if (dim_in > 0)
 86 |       dim = dim_in;
 87 |     
 88 |     build_tree();
 89 |     
 90 |     if (rearrange) {
 91 |       // if we have a rearranged tree.
 92 |       // allocate the memory for it.
 93 |       rearranged_data.resize( boost::extents[N][dim] );
 94 |       
 95 |       // permute the data for it.
 96 |       for (int i=0; i<N; i++) {
 97 |         for (int j=0; j<dim; j++) {
 98 |           rearranged_data[i][j] = the_data[ind[i]][j];
 99 |           // wouldn't F90 be nice here?
100 |         }
101 |       }
102 |       data = &rearranged_data;
103 |     } else {
104 |       data = &the_data;
105 |     }
106 |   }
107 |   
108 |   // destructor
109 |   KDTree::~KDTree() {
110 |     delete root;
111 |   }
112 |   
113 |   // building routines
114 |   void KDTree::build_tree() {
115 |     for (int i=0; i<N; i++) ind[i] = i;
116 |     root = build_tree_for_range(0, N-1, NULL);
117 |   }
118 |   
119 |   KDTreeNode* KDTree::build_tree_for_range(int l, int u, KDTreeNode* parent) {
120 |     
121 |     // recursive function to build
122 |     KDTreeNode* node = new KDTreeNode(dim);
123 |     // the newly created node.
124 |     
125 |     if (u<l) {
126 |       return(NULL); // no data in this node.
127 |     }
128 |     
129 |     if ((u-l) <= bucketsize) {
130 |       // create a terminal node.
131 |       
132 |       // always compute true bounding box for terminal node.
133 |       for (int i=0;i<dim;i++) {
134 |         spread_in_coordinate(i, l, u, node->box[i]);
135 |       }
136 |       
137 |       node->cut_dim = 0;
138 |       node->cut_val = 0.0;
139 |       node->l = l;
140 |       node->u = u;
141 |       node->left = node->right = NULL;
142 |       
143 |       
144 |     } else {
145 |       //
146 |       // Compute an APPROXIMATE bounding box for this node.
147 |       // if parent == NULL, then this is the root node, and
148 |       // we compute for all dimensions.
149 |       // Otherwise, we copy the bounding box from the parent for
150 |       // all coordinates except for the parent's cut dimension.
151 |       // That, we recompute ourself.
152 |       //
153 |       int c = -1;
154 |       float maxspread = 0.0;
155 |       int m;
156 |       
157 |       for (int i=0;i<dim;i++) {
158 |         if ((parent == NULL) || (parent->cut_dim == i)) {
159 |           spread_in_coordinate(i, l, u, node->box[i]);
160 |         } else {
161 |           node->box[i] = parent->box[i];
162 |         }
163 |         float spread = node->box[i].upper - node->box[i].lower;
164 |         if (spread>maxspread) {
165 |           maxspread = spread;
166 |           c=i;
167 |         }
168 |       }
169 |       
170 |       //
171 |       // now, c is the identity of which coordinate has the greatest spread
172 |       //
173 |       
174 |       if (false) {
175 |         m = (l+u)/2;
176 |         select_on_coordinate(c, m, l, u);
177 |       } else {
178 |         float sum;
179 |         float average;
180 |         
181 |         if (true) {
182 |           sum = 0.0;
183 |           for (int k=l; k <= u; k++) {
184 |             sum += the_data[ind[k]][c];
185 |           }
186 |           average = sum / static_cast<float> (u-l+1);
187 |         } else {
188 |           // average of top and bottom nodes.
189 |           average = (node->box[c].upper + node->box[c].lower)*0.5F;
190 |         }
191 |         
192 |         m = select_on_coordinate_value(c, average, l, u);
193 |       }
194 |       
195 |       
196 |       // move the indices around to cut on dim 'c'.
197 |       node->cut_dim=c;
198 |       node->l = l;
199 |       node->u = u;
200 |       
201 |       node->left = build_tree_for_range(l, m, node);
202 |       node->right = build_tree_for_range(m+1, u, node);
203 |       
204 |       if (node->right == NULL) {
205 |         for (int i=0; i<dim; i++)
206 |           node->box[i] = node->left->box[i];
207 |         node->cut_val = node->left->box[c].upper;
208 |         node->cut_val_left = node->cut_val_right = node->cut_val;
209 |       } else if (node->left == NULL) {
210 |         for (int i=0; i<dim; i++)
211 |           node->box[i] = node->right->box[i];
212 |         node->cut_val =  node->right->box[c].upper;
213 |         node->cut_val_left = node->cut_val_right = node->cut_val;
214 |       } else {
215 |         node->cut_val_right = node->right->box[c].lower;
216 |         node->cut_val_left  = node->left->box[c].upper;
217 |         node->cut_val = (node->cut_val_left + node->cut_val_right) / 2.0F;
218 |         //
219 |         // now recompute true bounding box as union of subtree boxes.
220 |         // This is now faster having built the tree, being logarithmic in
221 |         // N, not linear as would be from naive method.
222 |         //
223 |         for (int i=0; i<dim; i++) {
224 |           node->box[i].upper = std::max(node->left->box[i].upper,
225 |                   node->right->box[i].upper);
226 |           
227 |           node->box[i].lower = std::min(node->left->box[i].lower,
228 |                   node->right->box[i].lower);
229 |         }
230 |       }
231 |     }
232 |     return(node);
233 |   }
234 |   
235 |   void KDTree::spread_in_coordinate(int c, int l, int u, interval& interv) {
236 |     // return the minimum and maximum of the indexed data between l and u in
237 |     // smin_out and smax_out.
238 |     
239 |     float smin, smax;
240 |     float lmin, lmax;
241 |     int i;
242 |     
243 |     smin = the_data[ind[l]][c];
244 |     smax = smin;
245 |     
246 |     // process two at a time.
247 |     for (i=l+2; i<= u; i+=2) {
248 |       lmin = the_data[ind[i-1]] [c];
249 |       lmax = the_data[ind[i]  ] [c];
250 |       
251 |       if (lmin > lmax) {
252 |         swap(lmin, lmax);
253 |         //      float t = lmin;
254 |         //      lmin = lmax;
255 |         //      lmax = t;
256 |       }
257 |       
258 |       if (smin > lmin) smin = lmin;
259 |       if (smax <lmax) smax = lmax;
260 |     }
261 |     // is there one more element?
262 |     if (i == u+1) {
263 |       float last = the_data[ind[u]] [c];
264 |       if (smin>last) smin = last;
265 |       if (smax<last) smax = last;
266 |     }
267 |     interv.lower = smin;
268 |     interv.upper = smax;
269 |   }
270 |   
271 |   
272 |   void KDTree::select_on_coordinate(int c, int k, int l, int u) {
273 |     //
274 |     //  Move indices in ind[l..u] so that the elements in [l .. k]
275 |     //  are less than the [k+1..u] elmeents, viewed across dimension 'c'.
276 |     //
277 |     while (l < u) {
278 |       int t = ind[l];
279 |       int m = l;
280 |       
281 |       for (int i=l+1; i<=u; i++) {
282 |         if ( the_data[ ind[i] ] [c] < the_data[t][c]) {
283 |           m++;
284 |           swap(ind[i], ind[m]);
285 |         }
286 |       } // for i
287 |       swap(ind[l], ind[m]);
288 |       
289 |       if (m <= k) l = m+1;
290 |       if (m >= k) u = m-1;
291 |     } // while loop
292 |   }
293 |   
294 |   int KDTree::select_on_coordinate_value(int c, float alpha, int l, int u) {
295 |     //
296 |     //  Move indices in ind[l..u] so that the elements in [l .. return]
297 |     //  are <= alpha, and hence are less than the [return+1..u]
298 |     //  elments, viewed across dimension 'c'.
299 |     //
300 |     int lb = l, ub = u;
301 |     
302 |     while (lb < ub) {
303 |       if (the_data[ind[lb]][c] <= alpha) {
304 |         lb++; // good where it is.
305 |       } else {
306 |         swap(ind[lb], ind[ub]);
307 |         ub--;
308 |       }
309 |     }
310 |     
311 |     // here ub=lb
312 |     if (the_data[ind[lb]][c] <= alpha)
313 |       return(lb);
314 |     else
315 |       return(lb-1);
316 |     
317 |   }
318 |   
319 |   // search record substructure
320 |   //
321 |   // one of these is created for each search.
322 |   // this holds useful information to be used
323 |   // during the search
324 |   
325 |   static const float infinity = 1.0e38F;
326 |   
327 |   class SearchRecord {
328 |     
329 |   private:
330 |     friend class KDTree;
331 |     friend class KDTreeNode;
332 |     
333 |     std::vector<float>& qv;
334 |     int dim;
335 |     bool rearrange;
336 |     unsigned int nn; // , nfound;
337 |     float ballsize;
338 |     int centeridx, correltime;
339 |     
340 |     KDTreeResultVector& result;  // results
341 |     const KDTreeArray* data;
342 |     const std::vector<int>& ind;
343 |     // constructor
344 |     
345 |   public:
346 |     SearchRecord(std::vector<float>& qv_in, KDTree& tree_in,
347 |             KDTreeResultVector& result_in) :
348 |               qv(qv_in),
349 |                       result(result_in),
350 |                       data(tree_in.data),
351 |                       ind(tree_in.ind) {
352 |                 dim = tree_in.dim;
353 |                 rearrange = tree_in.rearrange;
354 |                 ballsize = infinity;
355 |                 nn = 0;
356 |               };
357 |               
358 |   };
359 |   
360 |   void KDTree::n_nearest_brute_force(std::vector<float>& qv, int nn, KDTreeResultVector& result) {
361 |     
362 |     result.clear();
363 |     
364 |     for (int i=0; i<N; i++) {
365 |       float dis = 0.0;
366 |       KDTreeResult e;
367 |       for (int j=0; j<dim; j++) {
368 |         dis += squared( the_data[i][j] - qv[j]);
369 |       }
370 |       e.dis = dis;
371 |       e.idx = i;
372 |       result.push_back(e);
373 |     }
374 |     sort(result.begin(), result.end() );
375 |   }
376 |   
377 |   
378 |   void KDTree::n_nearest(std::vector<float>& qv, int nn, KDTreeResultVector& result) {
379 |     SearchRecord sr(qv, *this, result);
380 |     std::vector<float> vdiff(dim, 0.0);
381 |     
382 |     result.clear();
383 |     
384 |     sr.centeridx = -1;
385 |     sr.correltime = 0;
386 |     sr.nn = nn;
387 |     
388 |     root->search(sr);
389 |     
390 |     if (sort_results) sort(result.begin(), result.end()); 
391 |   }  
392 |   
393 |   void KDTree::n_nearest_around_point(int idxin, int correltime, int nn,
394 |           KDTreeResultVector& result) {
395 |     
396 |     std::vector<float> qv(dim); //  query vector
397 |     result.clear();
398 |     for (int i=0; i<dim; i++) {
399 |       qv[i] = the_data[idxin][i];
400 |     }
401 |     // copy the query vector.
402 |     
403 |     {
404 |       SearchRecord sr(qv, *this, result);
405 |       // construct the search record.
406 |       sr.centeridx = idxin;
407 |       sr.correltime = correltime;
408 |       sr.nn = nn;
409 |       root->search(sr);
410 |     }
411 |     
412 |     if (sort_results) sort(result.begin(), result.end());
413 |   }
414 |   
415 |   
416 |   void KDTree::r_nearest(std::vector<float>& qv, float r2, KDTreeResultVector& result) {
417 |   // search for all within a ball of a certain radius
418 |     SearchRecord sr(qv, *this, result);
419 |     std::vector<float> vdiff(dim, 0.0);
420 |     
421 |     result.clear();
422 |     
423 |     sr.centeridx = -1;
424 |     sr.correltime = 0;
425 |     sr.nn = 0;
426 |     sr.ballsize = r2;
427 |     
428 |     root->search(sr);
429 |     
430 |     if (sort_results) sort(result.begin(), result.end());
431 |   }
432 |   
433 |   int KDTree::r_count(std::vector<float>& qv, float r2) {
434 |   // search for all within a ball of a certain radius
435 |     {
436 |       KDTreeResultVector result;
437 |       SearchRecord sr(qv, *this, result);
438 |       
439 |       sr.centeridx = -1;
440 |       sr.correltime = 0;
441 |       sr.nn = 0;
442 |       sr.ballsize = r2;
443 |       
444 |       root->search(sr);
445 |       return(result.size());
446 |     }
447 |   }
448 |   
449 |   void KDTree::r_nearest_around_point(int idxin, int correltime, float r2,
450 |           KDTreeResultVector& result) {
451 |     std::vector<float> qv(dim);  //  query vector
452 |     
453 |     result.clear();
454 |     
455 |     for (int i=0; i<dim; i++) {
456 |       qv[i] = the_data[idxin][i];
457 |     }
458 |     // copy the query vector.
459 |     
460 |     {
461 |       SearchRecord sr(qv, *this, result);
462 |       // construct the search record.
463 |       sr.centeridx = idxin;
464 |       sr.correltime = correltime;
465 |       sr.ballsize = r2;
466 |       sr.nn = 0;
467 |       root->search(sr);
468 |     }
469 |     if (sort_results) sort(result.begin(), result.end());
470 |   }
471 |   
472 |   int KDTree::r_count_around_point(int idxin, int correltime, float r2) {
473 |     std::vector<float> qv(dim);  //  query vector
474 |     
475 |     for (int i=0; i<dim; i++) {
476 |       qv[i] = the_data[idxin][i];
477 |     }
478 |     // copy the query vector.
479 |     
480 |     {
481 |       KDTreeResultVector result;
482 |       SearchRecord sr(qv, *this, result);
483 |       // construct the search record.
484 |       sr.centeridx = idxin;
485 |       sr.correltime = correltime;
486 |       sr.ballsize = r2;
487 |       sr.nn = 0;
488 |       root->search(sr);
489 |       return(result.size());
490 |     }
491 |   }
492 |   
493 |   //
494 |   //        KDTREE_NODE implementation
495 |   //
496 | 
497 |   // constructor
498 |   KDTreeNode::KDTreeNode(int dim) : box(dim) {
499 |     left = right = NULL;
500 |     //
501 |     // all other construction is handled for real in the
502 |     // KDTree building operations.
503 |     //
504 |   }
505 |   
506 |   // destructor
507 |   KDTreeNode::~KDTreeNode() {
508 |     if (left != NULL) delete left;
509 |     if (right != NULL) delete right;
510 |     // maxbox and minbox
511 |     // will be automatically deleted in their own destructors.
512 |   }
513 |   
514 |   
515 |   void KDTreeNode::search(SearchRecord& sr) {
516 |     // the core search routine.
517 |     // This uses true distance to bounding box as the
518 |     // criterion to search the secondary node.
519 |     //
520 |     // This results in somewhat fewer searches of the secondary nodes
521 |     // than 'search', which uses the vdiff vector,  but as this
522 |     // takes more computational time, the overall performance may not
523 |     // be improved in actual run time.
524 |     
525 |     if ( (left == NULL) && (right == NULL)) {
526 |       // we are on a terminal node
527 |       if (sr.nn == 0) {
528 |         process_terminal_node_fixedball(sr);
529 |       } else {
530 |         process_terminal_node(sr);
531 |       }
532 |     } else {
533 |       KDTreeNode *ncloser, *nfarther;
534 |       
535 |       float extra;
536 |       float qval = sr.qv[cut_dim];
537 |       // value of the wall boundary on the cut dimension.
538 |       if (qval < cut_val) {
539 |         ncloser = left;
540 |         nfarther = right;
541 |         extra = cut_val_right-qval;
542 |       } else {
543 |         ncloser = right;
544 |         nfarther = left;
545 |         extra = qval-cut_val_left;
546 |       };
547 |       
548 |       if (ncloser != NULL) ncloser->search(sr);
549 |       
550 |       if ((nfarther != NULL) && (squared(extra) < sr.ballsize)) {
551 |         // first cut
552 |         if (nfarther->box_in_search_range(sr)) {
553 |           nfarther->search(sr);
554 |         }
555 |       }
556 |     }
557 |   }
558 | 
559 |   inline float dis_from_bnd(float x, float amin, float amax) {
560 |     if (x > amax) {
561 |       return(x-amax);
562 |     } else if (x < amin)
563 |       return (amin-x);
564 |     else
565 |       return 0.0;
566 |   }
567 |   
568 |   inline bool KDTreeNode::box_in_search_range(SearchRecord& sr) {
569 |     // does the bounding box, represented by minbox[*],maxbox[*]
570 |     // have any point which is within 'sr.ballsize' to 'sr.qv'??
571 |     
572 |     int dim = sr.dim;
573 |     float dis2 =0.0;
574 |     float ballsize = sr.ballsize;
575 |     for (int i=0; i<dim;i++) {
576 |       dis2 += squared(dis_from_bnd(sr.qv[i], box[i].lower, box[i].upper));
577 |       if (dis2 > ballsize)
578 |         return(false);
579 |     }
580 |     return(true);
581 |   }
582 |   
583 |   void KDTreeNode::process_terminal_node(SearchRecord& sr) {
584 |     int centeridx  = sr.centeridx;
585 |     int correltime = sr.correltime;
586 |     unsigned int nn = sr.nn;
587 |     int dim        = sr.dim;
588 |     float ballsize = sr.ballsize;
589 |     //
590 |     bool rearrange = sr.rearrange;
591 |     const KDTreeArray& data = *sr.data;
592 |     
593 |     const bool debug = false;
594 |     
595 |     if (debug) {
596 | //       printf("Processing terminal node %d, %d\n", l, u);
597 | //       std::cout << "Query vector = [";
598 | //       for (int i=0; i<dim; i++) std::cout << sr.qv[i] << ',';
599 | //       std::cout << "]\n";
600 | //       std::cout << "nn = " << nn << '\n';
601 | //       check_query_in_bound(sr);
602 |     }
603 |     
604 |     for (int i=l; i<=u;i++) {
605 |       int indexofi;  // sr.ind[i];
606 |       float dis;
607 |       bool early_exit;
608 |       
609 |       if (rearrange) {
610 |         early_exit = false;
611 |         dis = 0.0;
612 |         for (int k=0; k<dim; k++) {
613 |           dis += squared(data[i][k] - sr.qv[k]);
614 |           if (dis > ballsize) {
615 |             early_exit=true;
616 |             break;
617 |           }
618 |         }
619 |         if(early_exit) continue; // next iteration of mainloop
620 |         // why do we do things like this?  because if we take an early
621 |         // exit (due to distance being too large) which is common, then
622 |         // we need not read in the actual point index, thus saving main
623 |         // memory bandwidth.  If the distance to point is less than the
624 |         // ballsize, though, then we need the index.
625 |         //
626 |         indexofi = sr.ind[i];
627 |       } else {
628 |         //
629 |         // but if we are not using the rearranged data, then
630 |         // we must always
631 |         indexofi = sr.ind[i];
632 |         early_exit = false;
633 |         dis = 0.0;
634 |         for (int k=0; k<dim; k++) {
635 |           dis += squared(data[indexofi][k] - sr.qv[k]);
636 |           if (dis > ballsize) {
637 |             early_exit= true;
638 |             break;
639 |           }
640 |         }
641 |         if(early_exit) continue; // next iteration of mainloop
642 |       } // end if rearrange.
643 |       
644 |       if (centeridx > 0) {
645 |         // we are doing decorrelation interval
646 |         if (abs(indexofi-centeridx) < correltime) continue; // skip this point.
647 |       }
648 |       
649 |       // here the point must be added to the list.
650 |       //
651 |       // two choices for any point.  The list so far is either
652 |       // undersized, or it is not.
653 |       //
654 |       if (sr.result.size() < nn) {
655 |         KDTreeResult e;
656 |         e.idx = indexofi;
657 |         e.dis = dis;
658 |         sr.result.push_element_and_heapify(e);
659 |         if (debug) std::cout << "unilaterally pushed dis=" << dis;
660 |         if (sr.result.size() == nn) ballsize = sr.result.max_value();
661 |         // Set the ball radius to the largest on the list (maximum priority).
662 |         if (debug) {
663 |           std::cout << " ballsize = " << ballsize << "\n";
664 |           std::cout << "sr.result.size() = "  << sr.result.size() << '\n';
665 |         }
666 |       } else {
667 |         //
668 |         // if we get here then the current node, has a squared
669 |         // distance smaller
670 |         // than the last on the list, and belongs on the list.
671 |         //
672 |         KDTreeResult e;
673 |         e.idx = indexofi;
674 |         e.dis = dis;
675 |         ballsize = sr.result.replace_maxpri_elt_return_new_maxpri(e);
676 |         if (debug) {
677 |           std::cout << "Replaced maximum dis with dis=" << dis <<
678 |                   " new ballsize =" << ballsize << '\n';
679 |         }
680 |       }
681 |     } // main loop
682 |     sr.ballsize = ballsize;
683 |   }
684 |   
685 |   void KDTreeNode::process_terminal_node_fixedball(SearchRecord& sr) {
686 |     int centeridx  = sr.centeridx;
687 |     int correltime = sr.correltime;
688 |     int dim        = sr.dim;
689 |     float ballsize = sr.ballsize;
690 |     //
691 |     bool rearrange = sr.rearrange;
692 |     const KDTreeArray& data = *sr.data;
693 |     
694 |     for (int i=l; i<=u;i++) {
695 |       int indexofi = sr.ind[i];
696 |       float dis;
697 |       bool early_exit;
698 |       
699 |       if (rearrange) {
700 |         early_exit = false;
701 |         dis = 0.0;
702 |         for (int k=0; k<dim; k++) {
703 |           dis += squared(data[i][k] - sr.qv[k]);
704 |           if (dis > ballsize) {
705 |             early_exit=true;
706 |             break;
707 |           }
708 |         }
709 |         if(early_exit) continue; // next iteration of mainloop
710 |         // why do we do things like this?  because if we take an early
711 |         // exit (due to distance being too large) which is common, then
712 |         // we need not read in the actual point index, thus saving main
713 |         // memory bandwidth.  If the distance to point is less than the
714 |         // ballsize, though, then we need the index.
715 |         //
716 |         indexofi = sr.ind[i];
717 |       } else {
718 |         //
719 |         // but if we are not using the rearranged data, then
720 |         // we must always
721 |         indexofi = sr.ind[i];
722 |         early_exit = false;
723 |         dis = 0.0;
724 |         for (int k=0; k<dim; k++) {
725 |           dis += squared(data[indexofi][k] - sr.qv[k]);
726 |           if (dis > ballsize) {
727 |             early_exit= true;
728 |             break;
729 |           }
730 |         }
731 |         if(early_exit) continue; // next iteration of mainloop
732 |       } // end if rearrange.
733 |       
734 |       if (centeridx > 0) {
735 |         // we are doing decorrelation interval
736 |         if (abs(indexofi-centeridx) < correltime) continue; // skip this point.
737 |       }
738 |       
739 |       {
740 |         KDTreeResult e;
741 |         e.idx = indexofi;
742 |         e.dis = dis;
743 |         sr.result.push_back(e);
744 |       }
745 |       
746 |     }
747 |   }
748 | } // namespace kdtee
749 | 


--------------------------------------------------------------------------------
/src/matrix.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright 2011. All rights reserved.
  3 | Institute of Measurement and Control Systems
  4 | Karlsruhe Institute of Technology, Germany
  5 | 
  6 | Authors: Andreas Geiger
  7 | 
  8 | matrix is free software; you can redistribute it and/or modify it under the
  9 | terms of the GNU General Public License as published by the Free Software
 10 | Foundation; either version 2 of the License, or any later version.
 11 | 
 12 | matrix is distributed in the hope that it will be useful, but WITHOUT ANY
 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
 15 | 
 16 | You should have received a copy of the GNU General Public License along with
 17 | matrix; if not, write to the Free Software Foundation, Inc., 51 Franklin
 18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 
 19 | */
 20 | 
 21 | #include "matrix.h"
 22 | #include <math.h>
 23 | 
 24 | #define SWAP(a,b) {temp=a;a=b;b=temp;}
 25 | #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
 26 | static FLOAT sqrarg;
 27 | #define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)
 28 | static FLOAT maxarg1,maxarg2;
 29 | #define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
 30 | static int32_t iminarg1,iminarg2;
 31 | #define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ? (iminarg1) : (iminarg2))
 32 | 
 33 | 
 34 | using namespace std;
 35 | 
 36 | Matrix::Matrix () {
 37 |   m   = 0;
 38 |   n   = 0;
 39 |   val = 0;
 40 | }
 41 | 
 42 | Matrix::Matrix (const int32_t m_,const int32_t n_) {
 43 |   allocateMemory(m_,n_);
 44 | }
 45 | 
 46 | Matrix::Matrix (const int32_t m_,const int32_t n_,const FLOAT* val_) {
 47 |   allocateMemory(m_,n_);
 48 |   int32_t k=0;
 49 |   for (int32_t i=0; i<m_; i++)
 50 |     for (int32_t j=0; j<n_; j++)
 51 |       val[i][j] = val_[k++];
 52 | }
 53 | 
 54 | Matrix::Matrix (const Matrix &M) {
 55 |   allocateMemory(M.m,M.n);
 56 |   for (int32_t i=0; i<M.m; i++)
 57 |     memcpy(val[i],M.val[i],M.n*sizeof(FLOAT));
 58 | }
 59 | 
 60 | Matrix::~Matrix () {
 61 |   releaseMemory();
 62 | }
 63 | 
 64 | Matrix& Matrix::operator= (const Matrix &M) {
 65 |   if (this!=&M) {
 66 |     if (M.m!=m || M.n!=n) {
 67 |       releaseMemory();
 68 |       allocateMemory(M.m,M.n);
 69 |     }
 70 |     if (M.n>0)
 71 |       for (int32_t i=0; i<M.m; i++)
 72 |         memcpy(val[i],M.val[i],M.n*sizeof(FLOAT));
 73 |   }
 74 |   return *this;
 75 | }
 76 | 
 77 | void Matrix::getData(FLOAT* val_,int32_t i1,int32_t j1,int32_t i2,int32_t j2) {
 78 |   if (i2==-1) i2 = m-1;
 79 |   if (j2==-1) j2 = n-1;
 80 |   int32_t k=0;
 81 |   for (int32_t i=i1; i<=i2; i++)
 82 |     for (int32_t j=j1; j<=j2; j++)
 83 |       val_[k++] = val[i][j];
 84 | }
 85 | 
 86 | Matrix Matrix::getMat(int32_t i1,int32_t j1,int32_t i2,int32_t j2) {
 87 |   if (i2==-1) i2 = m-1;
 88 |   if (j2==-1) j2 = n-1;
 89 |   if (i1<0 || i2>=m || j1<0 || j2>=n || i2<i1 || j2<j1) {
 90 |     cerr << "ERROR: Cannot get submatrix [" << i1 << ".." << i2 <<
 91 |         "] x [" << j1 << ".." << j2 << "]" <<
 92 |         " of a (" << m << "x" << n << ") matrix." << endl;
 93 |     exit(0);
 94 |   }
 95 |   Matrix M(i2-i1+1,j2-j1+1);
 96 |   for (int32_t i=0; i<M.m; i++)
 97 |     for (int32_t j=0; j<M.n; j++)
 98 |       M.val[i][j] = val[i1+i][j1+j];
 99 |   return M;
100 | }
101 | 
102 | void Matrix::setMat(const Matrix &M,const int32_t i1,const int32_t j1) {
103 |   if (i1<0 || j1<0 || i1+M.m>m || j1+M.n>n) {
104 |     cerr << "ERROR: Cannot set submatrix [" << i1 << ".." << i1+M.m-1 <<
105 |         "] x [" << j1 << ".." << j1+M.n-1 << "]" <<
106 |         " of a (" << m << "x" << n << ") matrix." << endl;
107 |     exit(0);
108 |   }
109 |   for (int32_t i=0; i<M.m; i++)
110 |     for (int32_t j=0; j<M.n; j++)
111 |       val[i1+i][j1+j] = M.val[i][j];
112 | }
113 | 
114 | void Matrix::setVal(FLOAT s,int32_t i1,int32_t j1,int32_t i2,int32_t j2) {
115 |   if (i2==-1) i2 = m-1;
116 |   if (j2==-1) j2 = n-1;
117 |   if (i2<i1 || j2<j1) {
118 |     cerr << "ERROR in setVal: Indices must be ordered (i1<=i2, j1<=j2)." << endl;
119 |     exit(0);
120 |   }
121 |   for (int32_t i=i1; i<=i2; i++)
122 |     for (int32_t j=j1; j<=j2; j++)
123 |       val[i][j] = s;
124 | }
125 | 
126 | void Matrix::setDiag(FLOAT s,int32_t i1,int32_t i2) {
127 |   if (i2==-1) i2 = min(m-1,n-1);
128 |   for (int32_t i=i1; i<=i2; i++)
129 |     val[i][i] = s;
130 | }
131 | 
132 | void Matrix::zero() {
133 |   setVal(0);
134 | }
135 | 
136 | Matrix Matrix::extractCols (vector<int> idx) {
137 |   Matrix M(m,idx.size());
138 |   for (int32_t j=0; j<M.n; j++)
139 |     if (idx[j]<n)
140 |       for (int32_t i=0; i<m; i++)
141 |         M.val[i][j] = val[i][idx[j]];
142 |   return M;
143 | }
144 | 
145 | Matrix Matrix::eye (const int32_t m) {
146 |   Matrix M(m,m);
147 |   for (int32_t i=0; i<m; i++)
148 |     M.val[i][i] = 1;
149 |   return M;
150 | }
151 | 
152 | void Matrix::eye () {
153 |   for (int32_t i=0; i<m; i++)
154 |     for (int32_t j=0; j<n; j++)
155 |       val[i][j] = 0;
156 |   for (int32_t i=0; i<min(m,n); i++)
157 |     val[i][i] = 1;
158 | }
159 | 
160 | Matrix Matrix::ones (const int32_t m,const int32_t n) {
161 |   Matrix M(m,n);
162 |   for (int32_t i=0; i<m; i++)
163 |     for (int32_t j=0; j<n; j++)
164 |       M.val[i][j] = 1;
165 |   return M;
166 | }
167 | 
168 | Matrix Matrix::diag (const Matrix &M) {
169 |   if (M.m>1 && M.n==1) {
170 |     Matrix D(M.m,M.m);
171 |     for (int32_t i=0; i<M.m; i++)
172 |       D.val[i][i] = M.val[i][0];
173 |     return D;
174 |   } else if (M.m==1 && M.n>1) {
175 |     Matrix D(M.n,M.n);
176 |     for (int32_t i=0; i<M.n; i++)
177 |       D.val[i][i] = M.val[0][i];
178 |     return D;
179 |   }
180 |   cout << "ERROR: Trying to create diagonal matrix from vector of size (" << M.m << "x" << M.n << ")" << endl;
181 |   exit(0);
182 | }
183 | 
184 | Matrix Matrix::reshape(const Matrix &M,int32_t m_,int32_t n_) {
185 |   if (M.m*M.n != m_*n_) {
186 |     cerr << "ERROR: Trying to reshape a matrix of size (" << M.m << "x" << M.n <<
187 |             ") to size (" << m_ << "x" << n_ << ")" << endl;
188 |     exit(0);
189 |   }
190 |   Matrix M2(m_,n_);
191 |   for (int32_t k=0; k<m_*n_; k++) {
192 |     int32_t i1 = k/M.n;
193 |     int32_t j1 = k%M.n;
194 |     int32_t i2 = k/n_;
195 |     int32_t j2 = k%n_;
196 |     M2.val[i2][j2] = M.val[i1][j1];
197 |   }
198 |   return M2;
199 | }
200 | 
201 | Matrix Matrix::rotMatX (const FLOAT &angle) {
202 |   FLOAT s = sin(angle);
203 |   FLOAT c = cos(angle);
204 |   Matrix R(3,3);
205 |   R.val[0][0] = +1;
206 |   R.val[1][1] = +c;
207 |   R.val[1][2] = -s;
208 |   R.val[2][1] = +s;
209 |   R.val[2][2] = +c;
210 |   return R;
211 | }
212 | 
213 | Matrix Matrix::rotMatY (const FLOAT &angle) {
214 |   FLOAT s = sin(angle);
215 |   FLOAT c = cos(angle);
216 |   Matrix R(3,3);
217 |   R.val[0][0] = +c;
218 |   R.val[0][2] = +s;
219 |   R.val[1][1] = +1;
220 |   R.val[2][0] = -s;
221 |   R.val[2][2] = +c;
222 |   return R;
223 | }
224 | 
225 | Matrix Matrix::rotMatZ (const FLOAT &angle) {
226 |   FLOAT s = sin(angle);
227 |   FLOAT c = cos(angle);
228 |   Matrix R(3,3);
229 |   R.val[0][0] = +c;
230 |   R.val[0][1] = -s;
231 |   R.val[1][0] = +s;
232 |   R.val[1][1] = +c;
233 |   R.val[2][2] = +1;
234 |   return R;
235 | }
236 | 
237 | Matrix Matrix::operator+ (const Matrix &M) {
238 |   const Matrix &A = *this;
239 |   const Matrix &B = M;
240 |   if (A.m!=B.m || A.n!=B.n) {
241 |     cerr << "ERROR: Trying to add matrices of size (" << A.m << "x" << A.n <<
242 |         ") and (" << B.m << "x" << B.n << ")" << endl;
243 |     exit(0);
244 |   }
245 |   Matrix C(A.m,A.n);
246 |   for (int32_t i=0; i<m; i++)
247 |     for (int32_t j=0; j<n; j++)
248 |       C.val[i][j] = A.val[i][j]+B.val[i][j];
249 |   return C;
250 | }
251 | 
252 | Matrix Matrix::operator- (const Matrix &M) {
253 |   const Matrix &A = *this;
254 |   const Matrix &B = M;
255 |   if (A.m!=B.m || A.n!=B.n) {
256 |     cerr << "ERROR: Trying to subtract matrices of size (" << A.m << "x" << A.n <<
257 |         ") and (" << B.m << "x" << B.n << ")" << endl;
258 |     exit(0);
259 |   }
260 |   Matrix C(A.m,A.n);
261 |   for (int32_t i=0; i<m; i++)
262 |     for (int32_t j=0; j<n; j++)
263 |       C.val[i][j] = A.val[i][j]-B.val[i][j];
264 |   return C;
265 | }
266 | 
267 | Matrix Matrix::operator* (const Matrix &M) {
268 |   const Matrix &A = *this;
269 |   const Matrix &B = M;
270 |   if (A.n!=B.m) {
271 |     cerr << "ERROR: Trying to multiply matrices of size (" << A.m << "x" << A.n <<
272 |         ") and (" << B.m << "x" << B.n << ")" << endl;
273 |     exit(0);
274 |   }
275 |   Matrix C(A.m,B.n);
276 |   for (int32_t i=0; i<A.m; i++)
277 |     for (int32_t j=0; j<B.n; j++)
278 |       for (int32_t k=0; k<A.n; k++)
279 |         C.val[i][j] += A.val[i][k]*B.val[k][j];
280 |   return C;
281 | }
282 | 
283 | Matrix Matrix::operator* (const FLOAT &s) {
284 |   Matrix C(m,n);
285 |   for (int32_t i=0; i<m; i++)
286 |     for (int32_t j=0; j<n; j++)
287 |       C.val[i][j] = val[i][j]*s;
288 |   return C;
289 | }
290 | 
291 | Matrix Matrix::operator/ (const Matrix &M) {
292 |   const Matrix &A = *this;
293 |   const Matrix &B = M;
294 |   
295 |   if (A.m==B.m && A.n==B.n) {
296 |     Matrix C(A.m,A.n);
297 |     for (int32_t i=0; i<A.m; i++)
298 |       for (int32_t j=0; j<A.n; j++)
299 |         if (B.val[i][j]!=0)
300 |           C.val[i][j] = A.val[i][j]/B.val[i][j];
301 |     return C;
302 |     
303 |   } else if (A.m==B.m && B.n==1) {
304 |     Matrix C(A.m,A.n);
305 |     for (int32_t i=0; i<A.m; i++)
306 |       for (int32_t j=0; j<A.n; j++)
307 |         if (B.val[i][0]!=0)
308 |           C.val[i][j] = A.val[i][j]/B.val[i][0];
309 |     return C;
310 |     
311 |   } else if (A.n==B.n && B.m==1) {
312 |     Matrix C(A.m,A.n);
313 |     for (int32_t i=0; i<A.m; i++)
314 |       for (int32_t j=0; j<A.n; j++)
315 |         if (B.val[0][j]!=0)
316 |           C.val[i][j] = A.val[i][j]/B.val[0][j];
317 |     return C;
318 |     
319 |   } else {
320 |     cerr << "ERROR: Trying to divide matrices of size (" << A.m << "x" << A.n <<
321 |         ") and (" << B.m << "x" << B.n << ")" << endl;
322 |     exit(0);
323 |   } 
324 | }
325 | 
326 | Matrix Matrix::operator/ (const FLOAT &s) {
327 |   if (fabs(s)<1e-20) {
328 |     cerr << "ERROR: Trying to divide by zero!" << endl;
329 |     exit(0);
330 |   }
331 |   Matrix C(m,n);
332 |   for (int32_t i=0; i<m; i++)
333 |     for (int32_t j=0; j<n; j++)
334 |       C.val[i][j] = val[i][j]/s;
335 |   return C;
336 | }
337 | 
338 | Matrix Matrix::operator- () {
339 |   Matrix C(m,n);
340 |   for (int32_t i=0; i<m; i++)
341 |     for (int32_t j=0; j<n; j++)
342 |       C.val[i][j] = -val[i][j];
343 |   return C;
344 | }
345 | 
346 | Matrix Matrix::operator~ () {
347 |   Matrix C(n,m);
348 |   for (int32_t i=0; i<m; i++)
349 |     for (int32_t j=0; j<n; j++)
350 |       C.val[j][i] = val[i][j];
351 |   return C;
352 | }
353 | 
354 | FLOAT Matrix::l2norm () {
355 |   FLOAT norm = 0;
356 |   for (int32_t i=0; i<m; i++)
357 |     for (int32_t j=0; j<n; j++)
358 |       norm += val[i][j]*val[i][j];
359 |   return sqrt(norm);
360 | }
361 | 
362 | FLOAT Matrix::mean () {
363 |   FLOAT mean = 0;
364 |   for (int32_t i=0; i<m; i++)
365 |     for (int32_t j=0; j<n; j++)
366 |       mean += val[i][j];
367 |   return mean/(FLOAT)(m*n);
368 | }
369 | 
370 | Matrix Matrix::cross (const Matrix &a, const Matrix &b) {
371 |   if (a.m!=3 || a.n!=1 || b.m!=3 || b.n!=1) {
372 |     cerr << "ERROR: Cross product vectors must be of size (3x1)" << endl;
373 |     exit(0);
374 |   }
375 |   Matrix c(3,1);
376 |   c.val[0][0] = a.val[1][0]*b.val[2][0]-a.val[2][0]*b.val[1][0];
377 |   c.val[1][0] = a.val[2][0]*b.val[0][0]-a.val[0][0]*b.val[2][0];
378 |   c.val[2][0] = a.val[0][0]*b.val[1][0]-a.val[1][0]*b.val[0][0];
379 |   return c;
380 | }
381 | 
382 | Matrix Matrix::inv (const Matrix &M) {
383 |   if (M.m!=M.n) {
384 |     cerr << "ERROR: Trying to invert matrix of size (" << M.m << "x" << M.n << ")" << endl;
385 |     exit(0);
386 |   }
387 |   Matrix A(M);
388 |   Matrix B = eye(M.m);
389 |   B.solve(A);
390 |   return B;
391 | }
392 | 
393 | bool Matrix::inv () {
394 |   if (m!=n) {
395 |     cerr << "ERROR: Trying to invert matrix of size (" << m << "x" << n << ")" << endl;
396 |     exit(0);
397 |   }
398 |   Matrix A(*this);
399 |   eye();
400 |   solve(A);
401 |   return true;
402 | }
403 | 
404 | FLOAT Matrix::det () {
405 |   
406 |   if (m != n) {
407 |     cerr << "ERROR: Trying to compute determinant of a matrix of size (" << m << "x" << n << ")" << endl;
408 |     exit(0);
409 |   }
410 |     
411 |   Matrix A(*this);
412 |   int32_t *idx = (int32_t*)malloc(m*sizeof(int32_t));
413 |   FLOAT d;
414 |   A.lu(idx,d);
415 |   for( int32_t i=0; i<m; i++)
416 |     d *= A.val[i][i];
417 |   free(idx);
418 |   return d;
419 | }
420 | 
421 | bool Matrix::solve (const Matrix &M, FLOAT eps) {
422 |   
423 |   // substitutes
424 |   const Matrix &A = M;
425 |   Matrix &B       = *this;
426 |   
427 |   if (A.m != A.n || A.m != B.m || A.m<1 || B.n<1) {
428 |     cerr << "ERROR: Trying to eliminate matrices of size (" << A.m << "x" << A.n <<
429 |             ") and (" << B.m << "x" << B.n << ")" << endl;
430 |     exit(0);
431 |   }
432 |   
433 |   // index vectors for bookkeeping on the pivoting
434 |   int32_t* indxc = new int32_t[m];
435 |   int32_t* indxr = new int32_t[m];
436 |   int32_t* ipiv  = new int32_t[m];
437 |   
438 |   // loop variables
439 |   int32_t i, icol, irow, j, k, l, ll;
440 |   FLOAT big, dum, pivinv, temp;
441 |   
442 |   // initialize pivots to zero
443 |   for (j=0;j<m;j++) ipiv[j]=0;
444 |   
445 |   // main loop over the columns to be reduced
446 |   for (i=0;i<m;i++) {
447 |     
448 |     big=0.0;
449 |     
450 |     // search for a pivot element
451 |     for (j=0;j<m;j++)
452 |       if (ipiv[j]!=1)
453 |         for (k=0;k<m;k++)
454 |           if (ipiv[k]==0)
455 |             if (fabs(A.val[j][k])>=big) {
456 |       big=fabs(A.val[j][k]);
457 |       irow=j;
458 |       icol=k;
459 |             }
460 |     ++(ipiv[icol]);
461 |     
462 |     // We now have the pivot element, so we interchange rows, if needed, to put the pivot
463 |     // element on the diagonal. The columns are not physically interchanged, only relabeled.
464 |     if (irow != icol) {
465 |       for (l=0;l<m;l++) SWAP(A.val[irow][l], A.val[icol][l])
466 |       for (l=0;l<n;l++) SWAP(B.val[irow][l], B.val[icol][l])
467 |     }
468 |     
469 |     indxr[i]=irow; // We are now ready to divide the pivot row by the
470 |     indxc[i]=icol; // pivot element, located at irow and icol.
471 |     
472 |     // check for singularity
473 |     if (fabs(A.val[icol][icol]) < eps) {
474 |       delete[] indxc;
475 |       delete[] indxr;
476 |       delete[] ipiv;
477 |       return false;
478 |     }
479 |     
480 |     pivinv=1.0/A.val[icol][icol];
481 |     A.val[icol][icol]=1.0;
482 |     for (l=0;l<m;l++) A.val[icol][l] *= pivinv;
483 |     for (l=0;l<n;l++) B.val[icol][l] *= pivinv;
484 |     
485 |     // Next, we reduce the rows except for the pivot one
486 |     for (ll=0;ll<m;ll++)
487 |       if (ll!=icol) {
488 |       dum = A.val[ll][icol];
489 |       A.val[ll][icol] = 0.0;
490 |       for (l=0;l<m;l++) A.val[ll][l] -= A.val[icol][l]*dum;
491 |       for (l=0;l<n;l++) B.val[ll][l] -= B.val[icol][l]*dum;
492 |       }
493 |   }
494 |   
495 |   // This is the end of the main loop over columns of the reduction. It only remains to unscramble
496 |   // the solution in view of the column interchanges. We do this by interchanging pairs of
497 |   // columns in the reverse order that the permutation was built up.
498 |   for (l=m-1;l>=0;l--) {
499 |     if (indxr[l]!=indxc[l])
500 |       for (k=0;k<m;k++)
501 |         SWAP(A.val[k][indxr[l]], A.val[k][indxc[l]])
502 |   }
503 |   
504 |   // success
505 |   delete[] indxc;
506 |   delete[] indxr;
507 |   delete[] ipiv;
508 |   return true;
509 | }
510 | 
511 | // Given a matrix a[1..n][1..n], this routine replaces it by the LU decomposition of a rowwise
512 | // permutation of itself. a and n are input. a is output, arranged as in equation (2.3.14) above;
513 | // indx[1..n] is an output vector that records the row permutation effected by the partial
514 | // pivoting; d is output as ±1 depending on whether the number of row interchanges was even
515 | // or odd, respectively. This routine is used in combination with lubksb to solve linear equations
516 | // or invert a matrix.
517 | 
518 | bool Matrix::lu(int32_t *idx, FLOAT &d, FLOAT eps) {
519 |   
520 |   if (m != n) {
521 |     cerr << "ERROR: Trying to LU decompose a matrix of size (" << m << "x" << n << ")" << endl;
522 |     exit(0);
523 |   }
524 |   
525 |   int32_t i,imax,j,k;
526 |   FLOAT   big,dum,sum,temp;
527 |   FLOAT* vv = (FLOAT*)malloc(n*sizeof(FLOAT)); // vv stores the implicit scaling of each row.
528 |   d = 1.0;
529 |   for (i=0; i<n; i++) { // Loop over rows to get the implicit scaling information.
530 |     big = 0.0;
531 |     for (j=0; j<n; j++)
532 |       if ((temp=fabs(val[i][j]))>big)
533 |         big = temp;
534 |     if (big == 0.0) { // No nonzero largest element.
535 |       free(vv);
536 |       return false;
537 |     }
538 |     vv[i] = 1.0/big; // Save the scaling.
539 |   }
540 |   for (j=0; j<n; j++) { // This is the loop over columns of Crout’s method.
541 |     for (i=0; i<j; i++) { // This is equation (2.3.12) except for i = j.
542 |       sum = val[i][j];
543 |       for (k=0; k<i; k++)
544 |         sum -= val[i][k]*val[k][j];
545 |       val[i][j] = sum;
546 |     }
547 |     big = 0.0; // Initialize the search for largest pivot element.
548 |     for (i=j; i<n; i++) {
549 |       sum = val[i][j];
550 |       for (k=0; k<j; k++)
551 |         sum -= val[i][k]*val[k][j];
552 |       val[i][j] = sum;
553 |       if ( (dum=vv[i]*fabs(sum))>=big) {
554 |         big  = dum;
555 |         imax = i;
556 |       }
557 |     }
558 |     if (j!=imax) { // Do we need to interchange rows?
559 |       for (k=0; k<n; k++) { // Yes, do so...
560 |         dum          = val[imax][k];
561 |         val[imax][k] = val[j][k];
562 |         val[j][k]    = dum;
563 |       }
564 |       d = -d;     // ...and change the parity of d.
565 |       vv[imax]=vv[j]; // Also interchange the scale factor.
566 |     }
567 |     idx[j] = imax;
568 |     if (j!=n-1) { // Now, finally, divide by the pivot element.
569 |       dum = 1.0/val[j][j];
570 |       for (i=j+1; i<n; i++)
571 |         val[i][j] *= dum;
572 |     }
573 |   } // Go back for the next column in the reduction.
574 |   
575 |   // success
576 |   free(vv);
577 |   return true;
578 | }
579 | 
580 | // Given a matrix M/A[1..m][1..n], this routine computes its singular value decomposition, M/A =
581 | // U·W·V T. Thematrix U replaces a on output. The diagonal matrix of singular values W is output
582 | // as a vector w[1..n]. Thematrix V (not the transpose V T ) is output as v[1..n][1..n].
583 | void Matrix::svd(Matrix &U2,Matrix &W,Matrix &V) {
584 | 
585 |   Matrix U = Matrix(*this);
586 |   U2 = Matrix(m,m);
587 |   V  = Matrix(n,n);
588 | 
589 |   FLOAT* w   = (FLOAT*)malloc(n*sizeof(FLOAT));
590 |   FLOAT* rv1 = (FLOAT*)malloc(n*sizeof(FLOAT));
591 | 
592 |   int32_t flag,i,its,j,jj,k,l,nm;
593 |   FLOAT   anorm,c,f,g,h,s,scale,x,y,z;
594 | 
595 |   g = scale = anorm = 0.0; // Householder reduction to bidiagonal form.
596 |   for (i=0;i<n;i++) {
597 |     l = i+1;
598 |     rv1[i] = scale*g;
599 |     g = s = scale = 0.0;
600 |     if (i < m) {
601 |       for (k=i;k<m;k++) scale += fabs(U.val[k][i]);
602 |       if (scale) {
603 |         for (k=i;k<m;k++) {
604 |           U.val[k][i] /= scale;
605 |           s += U.val[k][i]*U.val[k][i];
606 |         }
607 |         f = U.val[i][i];
608 |         g = -SIGN(sqrt(s),f);
609 |         h = f*g-s;
610 |         U.val[i][i] = f-g;
611 |         for (j=l;j<n;j++) {
612 |           for (s=0.0,k=i;k<m;k++) s += U.val[k][i]*U.val[k][j];
613 |           f = s/h;
614 |           for (k=i;k<m;k++) U.val[k][j] += f*U.val[k][i];
615 |         }
616 |         for (k=i;k<m;k++) U.val[k][i] *= scale;
617 |       }
618 |     }
619 |     w[i] = scale*g;
620 |     g = s = scale = 0.0;
621 |     if (i<m && i!=n-1) {
622 |       for (k=l;k<n;k++) scale += fabs(U.val[i][k]);
623 |       if (scale) {
624 |         for (k=l;k<n;k++) {
625 |           U.val[i][k] /= scale;
626 |           s += U.val[i][k]*U.val[i][k];
627 |         }
628 |         f = U.val[i][l];
629 |         g = -SIGN(sqrt(s),f);
630 |         h = f*g-s;
631 |         U.val[i][l] = f-g;
632 |         for (k=l;k<n;k++) rv1[k] = U.val[i][k]/h;
633 |         for (j=l;j<m;j++) {
634 |           for (s=0.0,k=l;k<n;k++) s += U.val[j][k]*U.val[i][k];
635 |           for (k=l;k<n;k++) U.val[j][k] += s*rv1[k];
636 |         }
637 |         for (k=l;k<n;k++) U.val[i][k] *= scale;
638 |       }
639 |     }
640 |     anorm = FMAX(anorm,(fabs(w[i])+fabs(rv1[i])));
641 |   }
642 |   for (i=n-1;i>=0;i--) { // Accumulation of right-hand transformations.
643 |     if (i<n-1) {
644 |       if (g) {
645 |         for (j=l;j<n;j++) // Double division to avoid possible underflow.
646 |           V.val[j][i]=(U.val[i][j]/U.val[i][l])/g;
647 |         for (j=l;j<n;j++) {
648 |           for (s=0.0,k=l;k<n;k++) s += U.val[i][k]*V.val[k][j];
649 |           for (k=l;k<n;k++) V.val[k][j] += s*V.val[k][i];
650 |         }
651 |       }
652 |       for (j=l;j<n;j++) V.val[i][j] = V.val[j][i] = 0.0;
653 |     }
654 |     V.val[i][i] = 1.0;
655 |     g = rv1[i];
656 |     l = i;
657 |   }
658 |   for (i=IMIN(m,n)-1;i>=0;i--) { // Accumulation of left-hand transformations.
659 |     l = i+1;
660 |     g = w[i];
661 |     for (j=l;j<n;j++) U.val[i][j] = 0.0;
662 |     if (g) {
663 |       g = 1.0/g;
664 |       for (j=l;j<n;j++) {
665 |         for (s=0.0,k=l;k<m;k++) s += U.val[k][i]*U.val[k][j];
666 |         f = (s/U.val[i][i])*g;
667 |         for (k=i;k<m;k++) U.val[k][j] += f*U.val[k][i];
668 |       }
669 |       for (j=i;j<m;j++) U.val[j][i] *= g;
670 |     } else for (j=i;j<m;j++) U.val[j][i]=0.0;
671 |     ++U.val[i][i];
672 |   }
673 |   for (k=n-1;k>=0;k--) { // Diagonalization of the bidiagonal form: Loop over singular values,
674 |     for (its=0;its<30;its++) { // and over allowed iterations.
675 |       flag = 1;
676 |       for (l=k;l>=0;l--) { // Test for splitting.
677 |         nm = l-1;
678 |         if ((FLOAT)(fabs(rv1[l])+anorm) == anorm) { flag = 0; break; }
679 |         if ((FLOAT)(fabs( w[nm])+anorm) == anorm) { break; }
680 |       }
681 |       if (flag) {
682 |         c = 0.0; // Cancellation of rv1[l], if l > 1.
683 |         s = 1.0;
684 |         for (i=l;i<=k;i++) {
685 |           f = s*rv1[i];
686 |           rv1[i] = c*rv1[i];
687 |           if ((FLOAT)(fabs(f)+anorm) == anorm) break;
688 |           g = w[i];
689 |           h = pythag(f,g);
690 |           w[i] = h;
691 |           h = 1.0/h;
692 |           c = g*h;
693 |           s = -f*h;
694 |           for (j=0;j<m;j++) {
695 |             y = U.val[j][nm];
696 |             z = U.val[j][i];
697 |             U.val[j][nm] = y*c+z*s;
698 |             U.val[j][i]  = z*c-y*s;
699 |           }
700 |         }
701 |       }
702 |       z = w[k];
703 |       if (l==k) { // Convergence.
704 |         if (z<0.0) { // Singular value is made nonnegative.
705 |           w[k] = -z;
706 |           for (j=0;j<n;j++) V.val[j][k] = -V.val[j][k];
707 |         }
708 |         break;
709 |       }
710 |       if (its == 29)
711 |         cerr << "ERROR in SVD: No convergence in 30 iterations" << endl;
712 |       x = w[l]; // Shift from bottom 2-by-2 minor.
713 |       nm = k-1;
714 |       y = w[nm];
715 |       g = rv1[nm];
716 |       h = rv1[k];
717 |       f = ((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
718 |       g = pythag(f,1.0);
719 |       f = ((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
720 |       c = s = 1.0; // Next QR transformation:
721 |       for (j=l;j<=nm;j++) {
722 |         i = j+1;
723 |         g = rv1[i];
724 |         y = w[i];
725 |         h = s*g;
726 |         g = c*g;
727 |         z = pythag(f,h);
728 |         rv1[j] = z;
729 |         c = f/z;
730 |         s = h/z;
731 |         f = x*c+g*s;
732 |         g = g*c-x*s;
733 |         h = y*s;
734 |         y *= c;
735 |         for (jj=0;jj<n;jj++) {
736 |           x = V.val[jj][j];
737 |           z = V.val[jj][i];
738 |           V.val[jj][j] = x*c+z*s;
739 |           V.val[jj][i] = z*c-x*s;
740 |         }
741 |         z = pythag(f,h);
742 |         w[j] = z; // Rotation can be arbitrary if z = 0.
743 |         if (z) {
744 |           z = 1.0/z;
745 |           c = f*z;
746 |           s = h*z;
747 |         }
748 |         f = c*g+s*y;
749 |         x = c*y-s*g;
750 |         for (jj=0;jj<m;jj++) {
751 |           y = U.val[jj][j];
752 |           z = U.val[jj][i];
753 |           U.val[jj][j] = y*c+z*s;
754 |           U.val[jj][i] = z*c-y*s;
755 |         }
756 |       }
757 |       rv1[l] = 0.0;
758 |       rv1[k] = f;
759 |       w[k] = x;
760 |     }
761 |   }
762 |   
763 |   // sort singular values and corresponding columns of u and v
764 |   // by decreasing magnitude. Also, signs of corresponding columns are
765 |   // flipped so as to maximize the number of positive elements.
766 |   int32_t s2,inc=1;
767 |   FLOAT   sw;
768 |   FLOAT* su = (FLOAT*)malloc(m*sizeof(FLOAT));
769 |   FLOAT* sv = (FLOAT*)malloc(n*sizeof(FLOAT));
770 |   do { inc *= 3; inc++; } while (inc <= n);
771 |   do {
772 |     inc /= 3;
773 |     for (i=inc;i<n;i++) {
774 |       sw = w[i];
775 |       for (k=0;k<m;k++) su[k] = U.val[k][i];
776 |       for (k=0;k<n;k++) sv[k] = V.val[k][i];
777 |       j = i;
778 |       while (w[j-inc] < sw) {
779 |         w[j] = w[j-inc];
780 |         for (k=0;k<m;k++) U.val[k][j] = U.val[k][j-inc];
781 |         for (k=0;k<n;k++) V.val[k][j] = V.val[k][j-inc];
782 |         j -= inc;
783 |         if (j < inc) break;
784 |       }
785 |       w[j] = sw;
786 |       for (k=0;k<m;k++) U.val[k][j] = su[k];
787 |       for (k=0;k<n;k++) V.val[k][j] = sv[k];
788 |     }
789 |   } while (inc > 1);
790 |   for (k=0;k<n;k++) { // flip signs
791 |     s2=0;
792 |     for (i=0;i<m;i++) if (U.val[i][k] < 0.0) s2++;
793 |     for (j=0;j<n;j++) if (V.val[j][k] < 0.0) s2++;
794 |     if (s2 > (m+n)/2) {
795 |       for (i=0;i<m;i++) U.val[i][k] = -U.val[i][k];
796 |       for (j=0;j<n;j++) V.val[j][k] = -V.val[j][k];
797 |     }
798 |   }
799 | 
800 |   // create vector and copy singular values
801 |   W = Matrix(min(m,n),1,w);
802 |   
803 |   // extract mxm submatrix U
804 |   U2.setMat(U.getMat(0,0,m-1,min(m-1,n-1)),0,0);
805 | 
806 |   // release temporary memory
807 |   free(w);
808 |   free(rv1);
809 |   free(su);
810 |   free(sv);
811 | }
812 | 
813 | ostream& operator<< (ostream& out,const Matrix& M) {
814 |   if (M.m==0 || M.n==0) {
815 |     out << "[empty matrix]";
816 |   } else {
817 |     char buffer[1024];
818 |     for (int32_t i=0; i<M.m; i++) {
819 |       for (int32_t j=0; j<M.n; j++) {
820 |         sprintf(buffer,"%12.7f ",M.val[i][j]);
821 |         out << buffer;
822 |       }
823 |       if (i<M.m-1)
824 |         out << endl;
825 |     }
826 |   }
827 |   return out;
828 | }
829 | 
830 | void Matrix::allocateMemory (const int32_t m_,const int32_t n_) {
831 |   m = abs(m_); n = abs(n_);
832 |   if (m==0 || n==0) {
833 |     val = 0;
834 |     return;
835 |   }
836 |   val    = (FLOAT**)malloc(m*sizeof(FLOAT*));
837 |   val[0] = (FLOAT*)calloc(m*n,sizeof(FLOAT));
838 |   for(int32_t i=1; i<m; i++)
839 |     val[i] = val[i-1]+n;
840 | }
841 | 
842 | void Matrix::releaseMemory () {
843 |   if (val!=0) {
844 |     free(val[0]);
845 |     free(val);
846 |   }
847 | }
848 | 
849 | FLOAT Matrix::pythag(FLOAT a,FLOAT b) {
850 |   FLOAT absa,absb;
851 |   absa = fabs(a);
852 |   absb = fabs(b);
853 |   if (absa > absb)
854 |     return absa*sqrt(1.0+SQR(absb/absa));
855 |   else
856 |     return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb)));
857 | }
858 | 
859 | 


--------------------------------------------------------------------------------