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;
}
