bool SOFM2D::initGrid(const dmatrix& data) { bool b=true; int i,j; const parameters& param=getParameters(); if (param.initType == parameters::Linear) { //eigenvalues already calculated? if (eva1==0.) { varianceFunctor<double> varFunc; dmatrix cov; varFunc.covarianceMatrixOfRows(data, cov); jacobi<double> eigenFunc; jacobi<double>::parameters jp; jp.sort=true; eigenFunc.setParameters(jp); dvector eva; dmatrix eve; b = eigenFunc.apply(cov, eva, eve); if (b) { eva1=eva.at(0); eva2=eva.at(1); eve1=eve.getColumnCopy(0); eve2=eve.getColumnCopy(1); } else { setStatusString("could not find eigenvalues using random points\n"); selectRandomPoints(data, sizeX*sizeY, grid); return false; } } meansFunctor<double> meanFunc; dvector mean; meanFunc.meanOfRows(data, mean); double x,y; dvector deltaX(eve1); deltaX.multiply(eva1/sizeX); dvector deltaY(eve2); deltaY.multiply(eva2/sizeY); dvector delta; for (i=0, y=-(double(sizeY-1)); i<sizeY; i++, y++) { for (j=0, x=-(double(sizeX-1)); j<sizeX; j++, x++) { delta.addScaled(x,deltaX,y,deltaY); grid.getRow(i*sizeX+j).add(mean,delta); } } } else { selectRandomPoints(data, sizeX*sizeY, grid); } return b; }
int main(int argc, char **argv) { if (argc < 5) { std::cout << "Usage: " << argv[0] << " path_to_scans/ output.ply icp_iters subsample_probability" << std::endl; return 1; } // Command line parameters string pc_filepath = argv[1]; string pc_file_out_ply = argv[2]; int icp_num_iters = std::atoi(argv[3]); double probability = std::atof(argv[4]); if (pc_filepath.c_str()[pc_filepath.length() - 1] != '/') { pc_filepath += "/"; } std::ifstream file(pc_filepath + "config.txt"); string line; getline(file, line); std::istringstream in(line); int num_images; in >> num_images; Mat pc_a = load_kinect_frame(pc_filepath + "image_0.png", pc_filepath + "depth_0.txt"); Mat allSamples = selectRandomPoints(pc_a, probability); // Used for accumulating the rigid transformation matrix Mat transformation = Mat::eye(4, 4, CV_32F); Mat rotation, translation; clock_t time; for (int image_num = 1; image_num < num_images; ++image_num) { std::cout << "REGISTERING IMAGE " << image_num << std::endl; time = clock(); // Load the point cloud to be registered string str_num = std::to_string(image_num); Mat pc_b = load_kinect_frame(pc_filepath + "image_" + str_num + ".png", pc_filepath + "depth_" + str_num + ".txt"); Mat pc_b_sample = selectRandomPoints(pc_b, probability); // Apply the previous transformations to b so that it is positioned near // the last accumulated points extractRigidTransform(transformation, rotation, translation); pc_b_sample = applyTransformation(pc_b_sample, rotation, translation); pc_b = applyTransformation(pc_b, rotation, translation); // Perform the specified number of icp iterations for (int i = 0; i < icp_num_iters; ++i) { // Find the nearest neighbor pairs in the two point clouds Mat a, b; nearest_neighbors(allSamples, pc_b_sample, a, b); // Find the optimal rotation and translation matrix to transform b onto a Mat r, t; rigid_transform_3D(b, a, r, t); // Apply the rigid transformation to the b point cloud pc_b_sample = applyTransformation(pc_b_sample, r, t); pc_b = applyTransformation(pc_b, r, t); transformation *= getRigidTransform(r, t); } // Combine the two point clouds and save to disk Mat combined, combinedSample; vconcat(pc_a, pc_b, combined); vconcat(allSamples, pc_b_sample, combinedSample); pc_a = combined; allSamples = combinedSample; save_point_cloud(combined, pc_file_out_ply); std::cout << "complete " << ((float)(clock() - time)) / CLOCKS_PER_SEC << std::endl; } return 0; }
// implements the Fuzzy C Means algorithm bool fuzzyCMeans::train(const dmatrix& data) { bool ok=true; int t=0; // create the distance functor according to the paramter norm distanceFunctor<double>* distFunc = 0; switch (getParameters().norm) { case parameters::L1: distFunc = new l1Distance<double>; break; case parameters::L2: distFunc = new l2Distance<double>; break; default: break; } int nbOfClusters=getParameters().nbOfClusters; int nbOfPoints=data.rows(); if(nbOfClusters>nbOfPoints) { setStatusString("more Clusters than points"); ok = false; } double q=getParameters().fuzzifier; if (q<=1) { setStatusString("q has to be bigger than 1"); ok = false; } // select some points of the given data to initialise the centroids selectRandomPoints(data,nbOfClusters,centroids); // initialize variables centroids.resize(nbOfClusters,data.columns(),0.0); dmatrix memberships(nbOfPoints, nbOfClusters, 0.0); double terminationCriterion=0; double newDistance; dvector newCenter(data.columns()); dvector currentPoint(data.columns()); dmatrix newCentroids(nbOfClusters,data.columns(),0.0); double sumOfMemberships=0; double membership=0; double dist1; double dist2; int i,j,k,m; do { // calculate new memberships memberships.fill(0.0); // clear old memberships for (i=0; i<nbOfPoints; i++) { for (j=0; j<nbOfClusters; j++) { newDistance=0; dist1=distFunc->apply(data.getRow(i), centroids.getRow(j)); for (k=0; k<nbOfClusters; k++) { dist2=distFunc->apply(data.getRow(i), centroids.getRow(k)); // if distance is 0, normal calculation of membership is not possible. if (dist2!=0) { newDistance+=pow((dist1/dist2),(1/(q-1))); } } // if point and centroid are equal if (newDistance!=0) memberships.at(i,j)=1/newDistance; else { dvector row(memberships.columns(),0.0); memberships.setRow(i,row); memberships.at(i,j)=1; break; } } } t++; // counts the iterations // calculate new centroids based on modified memberships for (m=0; m<nbOfClusters; m++) { newCenter.fill(0.0); sumOfMemberships=0; for (i=0; i<nbOfPoints; i++) { currentPoint=data.getRow(i); membership=pow(memberships.at(i,m),q); sumOfMemberships+=membership; currentPoint.multiply(membership); newCenter.add(currentPoint); } newCenter.divide(sumOfMemberships); newCentroids.setRow(m,newCenter); } terminationCriterion=distFunc->apply(centroids,newCentroids); centroids=newCentroids; } // the termination criterions while ( (terminationCriterion>getParameters().epsilon) && (t<getParameters().maxIterations)); int nbClusters = nbOfClusters; //Put the id information into the result object //Each cluster has the id of its position in the matrix ivector tids(nbClusters); for (i=0; i<nbClusters; i++) { tids.at(i)=i; } outTemplate=outputTemplate(tids); return ok; }