template <typename PointT> pcl::cloud_composer::CloudItem*
pcl::cloud_composer::CloudItem::createCloudItemFromTemplate (const QString name, typename PointCloud<PointT>::Ptr cloud_ptr)
{
pcl::PCLPointCloud2::Ptr cloud_blob = boost::make_shared <pcl::PCLPointCloud2> ();
toPCLPointCloud2 (*cloud_ptr, *cloud_blob);
CloudItem* cloud_item = new CloudItem ( name, cloud_blob, Eigen::Vector4f (), Eigen::Quaternionf (), false);
cloud_item->setData (QVariant::fromValue(cloud_ptr), ItemDataRole::CLOUD_TEMPLATED);
cloud_item->setPointType<PointT> ();
return cloud_item;
}
void HMesh::writeMesh(string file_name)
{
PolygonMesh triangles;
PointCloud<PointXYZ> cloud;
for(int i = 0; i < vertices_.size(); i++)
cloud.push_back(PointXYZ(vertices_[i]->x, vertices_[i]->y, vertices_[i]->z));
toPCLPointCloud2(cloud, triangles.cloud);
for(int i = 0; i < faces_.size(); i++)
{
pcl::Vertices triangle;
triangle.vertices.push_back(vert_map_[faces_[i]->startEdge_->start]);
triangle.vertices.push_back(vert_map_[faces_[i]->startEdge_->end]);
triangle.vertices.push_back(vert_map_[faces_[i]->startEdge_->next->end]);
triangles.polygons.push_back(triangle);
}
pcl::io::savePLYFile(file_name,triangles);
}
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
PointCloudSource &output, const Matrix4 &guess)
{
// Point cloud containing the correspondences of each point in <input, indices>
PointCloudSourcePtr input_transformed (new PointCloudSource);
nr_iterations_ = 0;
converged_ = false;
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// If the guessed transformation is non identity
if (guess != Matrix4::Identity ())
{
input_transformed->resize (input_->size ());
// Apply guessed transformation prior to search for neighbours
transformCloud (*input_, *input_transformed, guess);
}
else
*input_transformed = *input_;
transformation_ = Matrix4::Identity ();
// Make blobs if necessary
determineRequiredBlobData ();
PCLPointCloud2::Ptr target_blob (new PCLPointCloud2);
if (need_target_blob_)
pcl::toPCLPointCloud2 (*target_, *target_blob);
// Pass in the default target for the Correspondence Estimation/Rejection code
correspondence_estimation_->setInputTarget (target_);
if (correspondence_estimation_->requiresTargetNormals ())
correspondence_estimation_->setTargetNormals (target_blob);
// Correspondence Rejectors need a binary blob
for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
{
registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
if (rej->requiresTargetPoints ())
rej->setTargetPoints (target_blob);
if (rej->requiresTargetNormals () && target_has_normals_)
rej->setTargetNormals (target_blob);
}
convergence_criteria_->setMaximumIterations (max_iterations_);
convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
// Repeat until convergence
do
{
// Get blob data if needed
PCLPointCloud2::Ptr input_transformed_blob;
if (need_source_blob_)
{
input_transformed_blob.reset (new PCLPointCloud2);
toPCLPointCloud2 (*input_transformed, *input_transformed_blob);
}
// Save the previously estimated transformation
previous_transformation_ = transformation_;
// Set the source each iteration, to ensure the dirty flag is updated
correspondence_estimation_->setInputSource (input_transformed);
if (correspondence_estimation_->requiresSourceNormals ())
correspondence_estimation_->setSourceNormals (input_transformed_blob);
// Estimate correspondences
if (use_reciprocal_correspondence_)
correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
else
correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
//if (correspondence_rejectors_.empty ())
CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
{
registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
if (rej->requiresSourcePoints ())
rej->setSourcePoints (input_transformed_blob);
if (rej->requiresSourceNormals () && source_has_normals_)
rej->setSourceNormals (input_transformed_blob);
rej->setInputCorrespondences (temp_correspondences);
rej->getCorrespondences (*correspondences_);
// Modify input for the next iteration
if (i < correspondence_rejectors_.size () - 1)
*temp_correspondences = *correspondences_;
}
size_t cnt = correspondences_->size ();
// Check whether we have enough correspondences
if (static_cast<int> (cnt) < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
converged_ = false;
break;
}
// Estimate the transform
transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
// Tranform the data
transformCloud (*input_transformed, *input_transformed, transformation_);
// Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
++nr_iterations_;
// Update the vizualization of icp convergence
//if (update_visualizer_ != 0)
// update_visualizer_(output, source_indices_good, *target_, target_indices_good );
converged_ = static_cast<bool> ((*convergence_criteria_));
}
while (!converged_);
// Transform the input cloud using the final transformation
PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n",
final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
// Copy all the values
output = *input_;
// Transform the XYZ + normals
transformCloud (*input_, output, final_transformation_);
}
//template <typename PointT> std::vector< typename pcl::gpu::StandaloneMarchingCubes<PointT>::MeshPtr >
template <typename PointT> void
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::getMeshesFromTSDFVector (const std::vector<PointCloudPtr> &tsdf_clouds, const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &tsdf_offsets)
{
std::vector< MeshPtr > meshes_vector;
int max_iterations = std::min( tsdf_clouds.size (), tsdf_offsets.size () ); //Safety check
PCL_INFO ("There are %d cubes to be processed \n", max_iterations);
float cell_size = volume_size_ / voxels_x_;
int mesh_counter = 0;
for(int i = 0; i < max_iterations; ++i)
{
PCL_INFO ("Processing cube number %d\n", i);
//Making cloud local
Eigen::Affine3f cloud_transform;
float originX = (tsdf_offsets[i]).x();
float originY = (tsdf_offsets[i]).y();
float originZ = (tsdf_offsets[i]).z();
cloud_transform.linear ().setIdentity ();
cloud_transform.translation ()[0] = -originX;
cloud_transform.translation ()[1] = -originY;
cloud_transform.translation ()[2] = -originZ;
transformPointCloud (*tsdf_clouds[i], *tsdf_clouds[i], cloud_transform);
//Get mesh
MeshPtr tmp = getMeshFromTSDFCloud (*tsdf_clouds[i]);
if(tmp != nullptr)
{
meshes_vector.push_back (tmp);
mesh_counter++;
}
else
{
PCL_INFO ("This cloud returned no faces, we skip it!\n");
continue;
}
//Making cloud global
cloud_transform.translation ()[0] = originX * cell_size;
cloud_transform.translation ()[1] = originY * cell_size;
cloud_transform.translation ()[2] = originZ * cell_size;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp_ptr (new pcl::PointCloud<pcl::PointXYZ>);
fromPCLPointCloud2 ( (meshes_vector.back () )->cloud, *cloud_tmp_ptr);
transformPointCloud (*cloud_tmp_ptr, *cloud_tmp_ptr, cloud_transform);
toPCLPointCloud2 (*cloud_tmp_ptr, (meshes_vector.back () )->cloud);
std::stringstream name;
name << "mesh_" << mesh_counter << ".ply";
PCL_INFO ("Saving mesh...%d \n", mesh_counter);
pcl::io::savePLYFile (name.str (), *(meshes_vector.back ()));
}
return;
}