只用Eigen库实现的ICP算法
ICP.h
#pragma once
#include KdTree.h
#pragma once
#include 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 参考：https://github.com/Gregjksmith/Iterative-Closest-Point