UWB和lidar坐标系下轨迹对齐[附源码]

目录

代码： 

理论推导： 

代码： 

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>

#ifndef M_PI
#define M_PI       3.14159265358979323846   // pi
#endif // !M_PI

void GetTransform(
	const std::vector<Eigen::Vector3f> &xs,
	const std::vector<Eigen::Vector3f> &ys,
	Eigen::Matrix4f &transformation_);

int main()
{
	int N = 30;
	double w_sigma = 1.0;
	double inv_sigma = 1.0 / w_sigma;
	//cv::RNG rng;
	cv::RNG rng((unsigned)time(NULL));
	std::vector<Eigen::Vector3f> ori_data,aft_data;
	for (int i = 0; i < N; i++)
	{
		double a = rng.uniform(-100, 100);
		double b = rng.uniform(-100, 100);
		double c = rng.uniform(-100, 100);
		ori_data.push_back(Eigen::Vector3f(a, b, c));
		std::cout << "add points:" << a << " " << b << " " << c << std::endl;
	}

	Eigen::AngleAxisf r_z(M_PI / 3, Eigen::Vector3f(0, 0, 1));
	Eigen::AngleAxisf r_x(M_PI / 4, Eigen::Vector3f(1, 0, 0));
	Eigen::Isometry3f T = Eigen::Isometry3f::Identity();
	T.rotate(r_x*r_z);
	T.pretranslate(Eigen::Vector3f(1, 3, 4));
	std::cout << "Transform matrix=\n" << T.matrix() << std::endl;

	for (int i = 0; i < N; i++)
	{
		Eigen::Vector3f v = ori_data[i];
		aft_data.push_back(T*v);
	}
	Eigen::Matrix4f transform;
	GetTransform(aft_data, ori_data, transform);
	std::cout << "Compute Matrix=\n" << transform << std::endl;

	system("pause");
	return 0;
}

// 通过SVD分解计算R和t
void GetTransform(
	const std::vector<Eigen::Vector3f> &xs,
	const std::vector<Eigen::Vector3f> &ys,
	Eigen::Matrix4f &transformation_) 
{
	const size_t N = xs.size();

	// TODO: find centroids of mu_x and mu_y:
	// 计算均值
	Eigen::Vector3f mu_x = Eigen::Vector3f::Zero();
	Eigen::Vector3f mu_y = Eigen::Vector3f::Zero();
	for (size_t i = 0; i < N; ++i) {
		mu_x += xs.at(i);
		mu_y += ys.at(i);
	}
	mu_x /= N;
	mu_y /= N;
	// TODO: build H:
	// 构建H
	Eigen::Matrix3f H = Eigen::Matrix3f::Zero();
	for (size_t i = 0; i < N; ++i) {
		H += (ys.at(i) - mu_y) * (xs.at(i) - mu_x).transpose();
	}

	// TODO: solve R:
	// 求解R
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
	Eigen::Matrix3f R = svd.matrixV()*svd.matrixU().transpose();

	// TODO: solve t:
	// 求解t
	Eigen::Vector3f t = mu_x - R * mu_y;

	// TODO: set output:
	// 组织输出形式
	transformation_.setIdentity();
	transformation_.block(0, 0, 3, 3) = R;
	transformation_.block(0, 3, 3, 1) = t;
}

输出如下：

 

理论推导：