只用Eigen库实现的ICP算法

ICP.h

#pragma once

#include 
#include 
#include 
#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 
#include 
#include 

#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 


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

参考:https://github.com/Gregjksmith/Iterative-Closest-Point

你可能感兴趣的:(6D,pose,estimation,算法,点云,配准,ICP)