ICP.h
#pragma once
#include <iostream>
#include <vector>
#include <ctime>
#include "KdTree.h"
void icp(std::vector<Eigen::Vector3f>& source, std::vector<Eigen::Vector3f>& target, int maxIterations, float eps);
inline void computeCloudMean(std::vector<Eigen::Vector3f>& cloud, Eigen::Vector3f& mean)
{
mean[0] = 0.0;
mean[1] = 0.0;
mean[2] = 0.0;
for (int i = 0; i < cloud.size(); i++)
{
for (int j = 0; j < 3; j++)
{
mean[j] += cloud[i][j];
}
}
mean[0] /= (float)cloud.size();
mean[1] /= (float)cloud.size();
mean[2] /= (float)cloud.size();
}
inline void rotate(Eigen::Vector3f& p, Eigen::Matrix3f& rotationMatrix, Eigen::Vector3f& result)
{
result[0] = p[0] * rotationMatrix(0, 0) + p[1] * rotationMatrix(1, 0) + p[2] * rotationMatrix(2, 0);
result[1] = p[0] * rotationMatrix(0, 1) + p[1] * rotationMatrix(1, 1) + p[2] * rotationMatrix(2, 1);
result[2] = p[0] * rotationMatrix(0, 2) + p[1] * rotationMatrix(1, 2) + p[2] * rotationMatrix(2, 2);
}
inline void translate(Eigen::Vector3f& p, Eigen::Vector3f& translationVector, Eigen::Vector3f& result)
{
result[0] = p[0] + translationVector[0];
result[1] = p[1] + translationVector[1];
result[2] = p[2] + translationVector[2];
}
inline float error(Eigen::Vector3f ps, Eigen::Vector3f pd, Eigen::Matrix3f r, Eigen::Vector3f t)
{
Eigen::Vector3f res;
rotate(pd, r, res);
float err = pow(ps[0] - res[0] - t[0], 2.0) + pow(ps[1] - res[1] - t[1], 2.0) + pow(ps[2] - res[2] - t[2], 2.0);
return err;
}
KdTree.h
#pragma once
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#define SORT_ON_X 0
#define SORT_ON_Y 1
#define SORT_ON_Z 2
class KdTree
{
public:
KdTree(std::vector<Eigen::Vector3f>& pointCloud);
KdTree(std::vector<Eigen::Vector3f>& pointCloud, int start, int end, int sortOn);
virtual ~KdTree();
void build(std::vector<Eigen::Vector3f>& pointCloud, int start, int end, int sortOn);
bool isLeaf();
float split();
KdTree* getChild(Eigen::Vector3f searchPoint);
void search(Eigen::Vector3f& p, Eigen::Vector3f& result);
void radiusSearch(Eigen::Vector3f& p, float* searchRadius, Eigen::Vector3f& result);
private:
KdTree* __children[2];
Eigen::Vector3f __node;
int __sortOn;
int __start;
int __end;
void insertionSort(std::vector<Eigen::Vector3f>& pointCloud, int start, int end, int sortOn);
void mergeSort(std::vector<Eigen::Vector3f>& pointCloud, int start, int end);
void merge(std::vector<Eigen::Vector3f>& pointCloud, int left, int mid, int right);
int getNextSortOn(int sortOn);
Eigen::Vector3f* tempArray;
};
ICP.cpp
#include "ICP.h"
void icp(std::vector<Eigen::Vector3f>& source, std::vector<Eigen::Vector3f>& target, int maxIterations, float eps)
{
Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
Eigen::Vector3f translation = Eigen::Vector3f::Zero();
Eigen::Vector3f dynamicMid = Eigen::Vector3f::Zero();
Eigen::Vector3f staticMid = Eigen::Vector3f::Zero();
// copy the static point cloud
std::vector<Eigen::Vector3f> target_copy;
for (int i = 0; i < target.size(); i++)
{
target_copy.push_back(target[i]);
}
// create the kd tree
KdTree* tree = new KdTree(target_copy);
computeCloudMean(target, staticMid);
computeCloudMean(source, dynamicMid);
Eigen::Vector3f p, q;
Eigen::Vector3f pm, qm;
float cost = std::numeric_limits<float>::infinity();
std::srand(0);
for (int iter = 0; iter < maxIterations && abs(cost) > eps; iter++)
{
std::cout << iter << " " << abs(cost) << std::endl;
cost = 0.0;
Eigen::Matrix3f W = Eigen::Matrix3f::Zero();
computeCloudMean(source, dynamicMid);
for (int i = 0; i < source.size(); i++)
{
p = source[i];
tree->search(p, q); // get the closest point in the static point cloud
pm = p - dynamicMid;
qm = q - staticMid;
W += qm * pm.transpose();
cost += error(q, p, rotationMatrix, translation);
}
Eigen::JacobiSVD<Eigen::MatrixXf> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f U = svd.matrixU();
Eigen::Matrix3f V = svd.matrixV();
rotationMatrix = V * U.transpose();
Eigen::Vector3f t = Eigen::Vector3f::Zero();
rotate(dynamicMid, rotationMatrix, t);
t *= -1;
translate(staticMid, t, translation);
//update the point cloud
for (int i = 0; i < source.size(); i++)
{
rotate(source[i], rotationMatrix, p);
translate(p, translation, source[i]);
}
}
target_copy.clear();
delete tree;
}
KdTree.cpp
#include "KdTree.h"
KdTree::KdTree(std::vector<Eigen::Vector3f> &pointCloud)
{
tempArray = new Eigen::Vector3f[pointCloud.size()];
build(pointCloud, 0, pointCloud.size(), 0);
delete tempArray;
}
KdTree::KdTree(std::vector<Eigen::Vector3f> &pointCloud, int start, int end, int sortOn)
{
build(pointCloud, start, end, sortOn);
}
KdTree::~KdTree()
{
delete __children[0];
delete __children[1];
//delete __node;
}
void KdTree::build(std::vector<Eigen::Vector3f> &pointCloud, int start, int end, int sortOn)
{
__children[0] = nullptr;
__children[1] = nullptr;
//__node = nullptr;
__sortOn = sortOn;
__start = start;
__end = end;
if (end - start == 1)
{
__node = pointCloud[start];
return;
}
//insertionSort(pointCloud, start, end, sortOn);
mergeSort(pointCloud, start, end);
int numPoints = (end - start);
int mid = (int)floor((float)numPoints*0.5) + start;
__node = pointCloud[mid];
int numPointsHigh = (end - mid);
int numPointsLow = (mid - start);
if (numPointsHigh > 0)
{
__children[1] = new KdTree(pointCloud, mid, end, getNextSortOn(sortOn));
}
if (numPointsLow > 0)
{
__children[0] = new KdTree(pointCloud, start, mid, getNextSortOn(sortOn));
}
}
void KdTree::insertionSort(std::vector<Eigen::Vector3f> &pointCloud, int start, int end, int sortOn)
{
for (int i = start; i < end; i++)
{
for (int j = i + 1; j < end; j++)
{
if (pointCloud[j][sortOn] < pointCloud[i][sortOn])
{
Eigen::Vector3f temp = pointCloud[i];
pointCloud[i] = pointCloud[j];
pointCloud[j] = temp;
}
}
}
}
int KdTree::getNextSortOn(int sortOn)
{
switch (sortOn)
{
case SORT_ON_X:
return SORT_ON_Y;
break;
case SORT_ON_Y:
return SORT_ON_Z;
break;
case SORT_ON_Z:
return SORT_ON_X;
break;
}
}
bool KdTree::isLeaf()
{
if (__children[0] == nullptr && __children[1] == nullptr)
return true;
return false;
}
float KdTree::split()
{
return __node[__sortOn];
}
KdTree* KdTree::getChild(Eigen::Vector3f searchPoint)
{
if (searchPoint[__sortOn] >= split())
{
return __children[1];
}
else
{
return __children[0];
}
}
void KdTree::radiusSearch(Eigen::Vector3f& p, float* radius, Eigen::Vector3f& result)
{
if (isLeaf())
{
float d = sqrt(pow(__node[0] - p[0], 2.0) + pow(__node[1] - p[1], 2.0) + pow(__node[2] - p[2], 2.0));
if (d < *radius)
{
*radius = d;
result[0] = __node[0];
result[1] = __node[1];
result[2] = __node[2];
return;
}
}
else
{
if (abs(__node[__sortOn] - p[__sortOn]) < *radius)
{
__children[0]->radiusSearch(p, radius, result);
__children[1]->radiusSearch(p, radius, result);
}
else
{
if (p[__sortOn] >= __node[__sortOn])
{
__children[1]->radiusSearch(p, radius, result);
}
else
{
__children[0]->radiusSearch(p, radius, result);
}
}
}
}
void KdTree::search(Eigen::Vector3f& p, Eigen::Vector3f& result)
{
// get closets node
KdTree* tree = this;
while (!tree->isLeaf())
{
tree = tree->getChild(p);
}
result[0] = tree->__node.x();
result[1] = tree->__node.y();
result[2] = tree->__node.z();
float radius = sqrt(pow(p[0] - result[0], 2.0) + pow(p[1] - result[1], 2.0) + pow(p[2] - result[2], 2.0));
radiusSearch(p, &radius, result);
}
void KdTree::mergeSort(std::vector<Eigen::Vector3f> &pointCloud, int start, int end)
{
int mid;
if (start > end)
{
mid = (int)floor((end + start)*0.5);
mergeSort(pointCloud, start, mid);
mergeSort(pointCloud, mid + 1, end);
merge(pointCloud, start, mid + 1, end);
}
}
void KdTree::merge(std::vector<Eigen::Vector3f> &pointCloud, int left, int mid, int right)
{
int i, leftEnd, numElements, tempPos;
leftEnd = mid;
tempPos = 0;
numElements = right - left;
while (left < leftEnd && mid <= right)
{
if (pointCloud[left][__sortOn] <= pointCloud[mid][__sortOn])
{
tempArray[tempPos] = pointCloud[left];
tempPos++;
left++;
}
else
{
tempArray[tempPos] = pointCloud[mid];
tempPos++;
mid++;
}
}
while (left < leftEnd)
{
tempArray[tempPos] = pointCloud[left];
left++;
tempPos++;
}
while (mid <= right)
{
tempArray[tempPos] = pointCloud[mid];
mid++;
tempPos++;
}
for (int i = tempPos - 1; i >= 0; i--)
{
pointCloud[right] = tempArray[i];
right--;
}
}
main.cpp
#include "ICP.h"
#include <fstream>
int main()
{
std::vector<Eigen::Vector3f> source, target;
float x, y, z;
std::ifstream infile;
infile.open("bunny1.txt");
while (infile >> x >> y >> z)
{
source.push_back(Eigen::Vector3f(x, y, z));
}
infile.close();
infile.open("bunny2.txt");
while (infile >> x >> y >> z)
{
target.push_back(Eigen::Vector3f(x, y, z));
}
infile.close();
icp(source, target, 100, 1e-6);
float error = 0.0f;
std::fstream outfile;
outfile.open("icp.txt", 'w');
for (int i = 0; i < source.size(); i++)
{
error += pow(source[i][0] - target[i][0], 2) + pow(source[i][1] - target[i][1], 2) + pow(source[i][2] - target[i][2], 2);
outfile << source[i][0] << " " << source[i][1] << " " << source[i][2] << std::endl;
}
error /= (float)source.size();
std::cout << error << std::endl;
outfile.close();
return 0;
}