#include <pcl/registration/ia_ransac.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/search/kdtree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <time.h>
#include<res_c_r_w.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
using namespace cv;
//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src,
PointCloud::Ptr pcd_tgt,
PointCloud::Ptr pcd_final)
{
//int vp_1, vp_2;
// Create a PCLVisualizer object
pcl::visualization::PCLVisualizer viewer("registration Viewer");
//viewer.createViewPort (0.0, 0, 0.5, 1.0, vp_1);
// viewer.createViewPort (0.5, 0, 1.0, 1.0, vp_2);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(pcd_final, 0, 0, 255);
viewer.addPointCloud(pcd_src, src_h, "source cloud");
viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");
viewer.addPointCloud(pcd_final, final_h, "final cloud");
//viewer.addCoordinateSystem(1.0);
while (!viewer.wasStopped())
{
viewer.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
//由旋转平移矩阵计算旋转角度
void matrix2angle(Eigen::Matrix4f &result_trans, Eigen::Vector3f &result_angle)
{
double ax, ay, az;
if (result_trans(2, 0) == 1 || result_trans(2, 0) == -1)
{
az = 0;
double dlta;
dlta = atan2(result_trans(0, 1), result_trans(0, 2));
if (result_trans(2, 0) == -1)
{
ay = M_PI / 2;
ax = az + dlta;
}
else
{
ay = -M_PI / 2;
ax = -az + dlta;
}
}
else
{
ay = -asin(result_trans(2, 0));
ax = atan2(result_trans(2, 1) / cos(ay), result_trans(2, 2) / cos(ay));
az = atan2(result_trans(1, 0) / cos(ay), result_trans(0, 0) / cos(ay));
}
result_angle << ax, ay, az;
}
res_c_r_w res;
void generate_data_ideal(string file_move_hole, string file_fix_hole_no_rand, Point3f tans_vec, Mat& R_idle)
{
vector<Point3f>move_hole,fix_hole_no_rand;
move_hole.clear();
res.txt_read(file_move_hole, move_hole);
//res.cout_3d_point(move_hole);
for (int i = 0; i < move_hole.size(); i++)
{
Mat l = Mat(3, 1, CV_32F, &move_hole[i]);
Mat _mat = Mat(R_idle*l).clone();
Point3f temp = { _mat.at<float>(0, 0), _mat.at<float>(1, 0), _mat.at<float>(2, 0) };
temp += tans_vec;
fix_hole_no_rand.push_back(temp);
}
//res.cout_3d_point(fix_hole);
res.txt_record(file_fix_hole_no_rand, fix_hole_no_rand);
if (fix_hole_no_rand.size() > 6)
{
double dot_d = fix_hole_no_rand[4].dot(fix_hole_no_rand[5]);
cout << dot_d;
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr readCloudTxt(char* Filename, bool convert_to_miter = false)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
ifstream Points_in(Filename);
pcl::PointXYZ tmpoint;
if (Points_in.is_open())
{
while (!Points_in.eof()) //尚未到达文件结尾
{
Points_in >> tmpoint.x >> tmpoint.y >> tmpoint.z;
if (convert_to_miter)
{
tmpoint.x /= 1000;
tmpoint.y /= 1000;
tmpoint.z /= 1000;
}
basic_cloud_ptr->points.push_back(tmpoint);
}
}
basic_cloud_ptr->width = (int)basic_cloud_ptr->points.size();
basic_cloud_ptr->height = 1;
return basic_cloud_ptr;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr readCloudTxt(char* Filename, char* pcd_filename, bool convert_to_miter = false)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr = readCloudTxt(Filename, convert_to_miter);
pcl::PCDWriter writer;
writer.write(pcd_filename, *basic_cloud_ptr);
return basic_cloud_ptr;
}
void main_generate_data()
{
Mat R_idle;
//给一个四元数
//myQuaternion Q_idle = { 0.1f, 0.2f, 0.2f, 0.866025f };
myQuaternion Q_idle = { 0.9449f, 0.1091f, 0.2182f, 0.2182f };
//float Q_idle[4] = { 0.5, 0.5, 0.5, 0.5 };
res.MatrixFromQuaternion(Q_idle, R_idle);
cout << R_idle << endl;
string file_move_hole = "move_hole.txt";
string file_fix_hole_no_rand = "fix_hole.txt";
Point3f tans_vec = { 10, 20, 30 };
generate_data_ideal(file_move_hole, file_fix_hole_no_rand, tans_vec, R_idle);//利用理想矩阵将产生数据用于分析,
vector<Point3f>fix_hole, move_hole;
fix_hole.clear();
//string fix_hole_file = "fix_hole_big_err.txt";
string fix_hole_file = "fix_hole.txt";
res.txt_read(fix_hole_file, fix_hole);
move_hole.clear();
string move_hole_file = "move_hole.txt";
res.txt_read(move_hole_file, move_hole);
Mat init_r, init_t;//svd 初值传给优化客户端,或记录下来
res.my_svd(fix_hole, move_hole, init_r, init_t);
res.xml_write("svd_mat_r.xml", "svd_mat_r", init_r);
res.xml_write("svd_mat_t.xml", "svd_mat_t", init_t);
//将svd结果记录,用于优化初值
float siyunshu[4];
cout << init_r;
//计算位姿记录数据
res.QuaternionFromMatrix(init_r, siyunshu);
string filneme = "op_res.txt";
ofstream out(filneme);
out << siyunshu[0] << " " << siyunshu[1] << " " << siyunshu[2] << " " << siyunshu[3] << endl;
float ttt[3];
ttt[0] = (float)init_t.at<float>(0, 0);
ttt[1] = (float)init_t.at<float>(1, 0);
ttt[2] = (float)init_t.at<float>(2, 0);
out << ttt[0] << " " << ttt[1] << " " << ttt[2] << endl;
out.close();
system("pause");
return;
}
int main(int argc, char** argv)
{
main_generate_data();
readCloudTxt("fix_hole.txt", "fix_hole.pcd", true);
readCloudTxt("move_hole.txt", "move_hole.pcd", true);
//加载点云文件
PointCloud::Ptr cloud_src_o(new PointCloud);//原点云,待配准
pcl::io::loadPCDFile("move_hole.pcd", *cloud_src_o);
PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云
pcl::io::loadPCDFile("fix_hole.pcd", *cloud_tgt_o);
clock_t start = clock();
//去除NAN点
std::vector<int> indices_src; //保存去除的点的索引
pcl::removeNaNFromPointCloud(*cloud_src_o, *cloud_src_o, indices_src);
std::cout << "remove *cloud_src_o nan" << endl;
//下采样滤波
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
voxel_grid.setLeafSize(0.012, 0.012, 0.012);
voxel_grid.setInputCloud(cloud_src_o);
PointCloud::Ptr cloud_src(new PointCloud);
voxel_grid.filter(*cloud_src);
std::cout << "down size *cloud_src_o from " << cloud_src_o->size() << "to" << cloud_src->size() << endl;
pcl::io::savePCDFileASCII("bunny_src_down.pcd", *cloud_src);
//计算表面法线
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_src;
ne_src.setInputCloud(cloud_src);
pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());
ne_src.setSearchMethod(tree_src);
pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);
ne_src.setRadiusSearch(0.02);
ne_src.compute(*cloud_src_normals);
std::vector<int> indices_tgt;
pcl::removeNaNFromPointCloud(*cloud_tgt_o, *cloud_tgt_o, indices_tgt);
std::cout << "remove *cloud_tgt_o nan" << endl;
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_2;
voxel_grid_2.setLeafSize(0.01, 0.01, 0.01);
voxel_grid_2.setInputCloud(cloud_tgt_o);
PointCloud::Ptr cloud_tgt(new PointCloud);
voxel_grid_2.filter(*cloud_tgt);
std::cout << "down size *cloud_tgt_o.pcd from " << cloud_tgt_o->size() << "to" << cloud_tgt->size() << endl;
pcl::io::savePCDFileASCII("bunny_tgt_down.pcd", *cloud_tgt);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt;
ne_tgt.setInputCloud(cloud_tgt);
pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());
ne_tgt.setSearchMethod(tree_tgt);
pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);
//ne_tgt.setKSearch(20);
ne_tgt.setRadiusSearch(0.02);
ne_tgt.compute(*cloud_tgt_normals);
//计算FPFH
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_src;
fpfh_src.setInputCloud(cloud_src);
fpfh_src.setInputNormals(cloud_src_normals);
pcl::search::KdTree<PointT>::Ptr tree_src_fpfh(new pcl::search::KdTree<PointT>);
fpfh_src.setSearchMethod(tree_src_fpfh);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_src(new pcl::PointCloud<pcl::FPFHSignature33>());
fpfh_src.setRadiusSearch(0.05);
fpfh_src.compute(*fpfhs_src);
std::cout << "compute *cloud_src fpfh" << endl;
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_tgt;
fpfh_tgt.setInputCloud(cloud_tgt);
fpfh_tgt.setInputNormals(cloud_tgt_normals);
pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<PointT>);
fpfh_tgt.setSearchMethod(tree_tgt_fpfh);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::FPFHSignature33>());
fpfh_tgt.setRadiusSearch(0.05);
fpfh_tgt.compute(*fpfhs_tgt);
std::cout << "compute *cloud_tgt fpfh" << endl;
//SAC配准
pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
scia.setInputSource(cloud_src);
scia.setInputTarget(cloud_tgt);
scia.setSourceFeatures(fpfhs_src);
scia.setTargetFeatures(fpfhs_tgt);
//scia.setMinSampleDistance(1);
//scia.setNumberOfSamples(2);
//scia.setCorrespondenceRandomness(20);
PointCloud::Ptr sac_result(new PointCloud);
scia.align(*sac_result);
std::cout << "sac has converged:" << scia.hasConverged() << " score: " << scia.getFitnessScore() << endl;
Eigen::Matrix4f sac_trans;
sac_trans = scia.getFinalTransformation();
std::cout << sac_trans << endl;
pcl::io::savePCDFileASCII("bunny_transformed_sac.pcd", *sac_result);
clock_t sac_time = clock();
//icp配准
PointCloud::Ptr icp_result(new PointCloud);
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_src_o);
icp.setInputTarget(cloud_tgt_o);
//Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance(0.004);
// 最大迭代次数
icp.setMaximumIterations(50);
// 两次变化矩阵之间的差值
icp.setTransformationEpsilon(1e-10);
// 均方误差
icp.setEuclideanFitnessEpsilon(0.2);
icp.align(*icp_result, sac_trans);
clock_t end = clock();
cout << "total time: " << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl;
//我把计算法线和点特征直方图的时间也算在SAC里面了
cout << "sac time: " << (double)(sac_time - start) / (double)CLOCKS_PER_SEC << " s" << endl;
cout << "icp time: " << (double)(end - sac_time) / (double)CLOCKS_PER_SEC << " s" << endl;
//std::cout << "ICP has converged:" << icp.hasConverged()
// << " score: " << icp.getFitnessScore() << std::endl;
Eigen::Matrix4f icp_trans;
icp_trans = icp.getFinalTransformation();
//cout<<"ransformationProbability"<<icp.getTransformationProbability()<<endl;
std::cout << icp_trans << endl;
//使用创建的变换对未过滤的输入点云进行变换
pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);
//保存转换的输入点云
pcl::io::savePCDFileASCII("bunny_transformed_sac_ndt.pcd", *icp_result);
//计算误差
Eigen::Vector3f ANGLE_origin;
ANGLE_origin << 0, 0, M_PI / 5;
double error_x, error_y, error_z;
Eigen::Vector3f ANGLE_result;
matrix2angle(icp_trans, ANGLE_result);
error_x = fabs(ANGLE_result(0)) - fabs(ANGLE_origin(0));
error_y = fabs(ANGLE_result(1)) - fabs(ANGLE_origin(1));
error_z = fabs(ANGLE_result(2)) - fabs(ANGLE_origin(2));
cout << "original angle in x y z:\n" << ANGLE_origin << endl;
cout << "error in aixs_x: " << error_x << " error in aixs_y: " << error_y << " error in aixs_z: " << error_z << endl;
//可视化
visualize_pcd(cloud_src_o, cloud_tgt_o, icp_result);
return (0);
}
点云匹配,生成
最新推荐文章于 2024-05-22 12:51:05 发布