コード例 #1
0
ファイル: run_icp.cpp プロジェクト: ulyssesrr/carmen_lcad
int
perform_icp(pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_pointcloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_pointcloud, Eigen::Matrix<double, 4, 4> *correction, pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_pointcloud2)
{
	pcl::PointCloud<pcl::PointXYZRGB> out_pcl_pointcloud;
	pcl::PointCloud<pcl::PointXYZRGB> out_pcl_pointcloud_transformed;

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_filtered = GridFiltering(source_pointcloud, 0.2);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_filtered = GridFiltering(target_pointcloud, 0.2);

	gicp.setInputCloud(source_filtered);
	gicp.setInputTarget(target_filtered);
	gicp.align(out_pcl_pointcloud);
	(*correction) = gicp.getFinalTransformation().cast<double>();

	pcl::transformPointCloud(out_pcl_pointcloud, out_pcl_pointcloud_transformed, (*correction));
	//save_clouds_for_debug(*source_pointcloud, *target_pointcloud, out_pcl_pointcloud_transformed, *source_pointcloud2);

	if(gicp.hasConverged())
	{
		return 1;
	}

	return 0;
}
コード例 #2
0
int runGeneralizedICP(pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_pointcloud,
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_pointcloud,
		Eigen::Matrix<double, 4, 4> source_pointcloud_pose,
		Eigen::Matrix<double, 4, 4> target_pointcloud_pose,
		Eigen::Matrix<double, 4, 4> *out_pose,
		int current_index,
		int before_index)
{

	static int is_first_icp_run = 1;

	Eigen::Matrix<double, 4, 4> out_transform;
	Eigen::Matrix<double, 4, 4> src_transform;
	Eigen::Matrix<double, 4, 4> final_transform;

	/**************************************/
	/********** Initializations ***********/
	/**************************************/

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr src_pcl_pointcloud_corrected =
			boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >(
					new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr tgt_pcl_pointcloud_corrected =
			boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >(
					new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud < pcl::PointXYZRGB > out_pcl_pointcloud;
	pcl::PointCloud < pcl::PointXYZRGB > out_pcl_pointcloud_transformed;

	/**************************************/
	/**************** GICP ****************/
	/**************************************/

//	if (is_first_icp_run) {
//		accumulated_correction_transform =
//				Eigen::Matrix<double, 4, 4>().Identity();
//		is_first_icp_run = 0;
//	}

//	source_pointcloud_pose = accumulated_correction_transform
//			* source_pointcloud_pose;
//	target_pointcloud_pose = accumulated_correction_transform
//			* target_pointcloud_pose;

	pcl::transformPointCloud(*source_pointcloud, *src_pcl_pointcloud_corrected,
			(target_pointcloud_pose.inverse() * source_pointcloud_pose).cast<float>());

	pcl::transformPointCloud(*target_pointcloud, *tgt_pcl_pointcloud_corrected,
			Eigen::Matrix<float, 4, 4>().Identity());

	gicp.setInputCloud(src_pcl_pointcloud_corrected);
	gicp.setInputTarget(tgt_pcl_pointcloud_corrected);
	gicp.align(out_pcl_pointcloud);

	out_transform = //target_pointcloud_pose *
			gicp.getFinalTransformation().cast<double>();
//			* target_pointcloud_pose.inverse();
	//*corrected_pose = final_transform = source_pointcloud_pose;

	src_pcl_pointcloud_corrected->clear();
	tgt_pcl_pointcloud_corrected->clear();

	if (gicp.hasConverged())
	{
		*out_pose = out_transform;
		return 1;
	}

	return 0;
}