template <typename PointT> bool
open_ptrack::detection::GroundplaneEstimation<PointT>::tooManyNaN(PointCloudConstPtr cloud, float max_ratio)
{
int nan_counter = 0;
for(unsigned int i = 0; i < cloud->size(); i++)
{
// If the point has a non-valid coordinate:
if(isnan(cloud->points[i].x) || isnan(cloud->points[i].y) || isnan(cloud->points[i].z) )
{ // update the counter:
nan_counter++;
}
}
// If the nan ratio is over max_ratio:
if( (float) nan_counter/cloud->size() > max_ratio )
return true; // too many NaNs, frame invalid
else
return false;
}
template<typename Point> void
TableObjectCluster<Point>::extractTableRoi2(const PointCloudConstPtr& pc_in,
PointCloudPtr& hull,
Eigen::Vector4f& plane_coeffs,
pcl::PointCloud<Point>& pc_roi)
{
pcl::PointIndices indices;
for(unsigned int i=0; i<pc_in->size(); i++)
indices.indices.push_back(i);
// Project all points
pcl::PointCloud<Point> projected_points;
pcl::SampleConsensusModelPlane<Point> sacmodel (pc_in);
//Eigen::Vector4f e_plane_coeffs(plane_coeffs.values[0],plane_coeffs.values[1],plane_coeffs.values[2],plane_coeffs.values[3]);
sacmodel.projectPoints (indices.indices, plane_coeffs, projected_points, false);
pcl::io::savePCDFileASCII ("/home/goa/tmp/proj.pcd", projected_points);
pcl::io::savePCDFileASCII ("/home/goa/tmp/hull.pcd", *hull);
pcl::PointIndices inliers;
std::cout << "Coeffs:" << plane_coeffs << std::endl;
int ctr=0, ctr2=0;
for(unsigned int i=0; i<pc_in->size(); i++)
{
double distance = pcl::pointToPlaneDistanceSigned (pc_in->points[i], plane_coeffs);
if (distance < height_min_ || distance > height_max_)
continue;
ctr++;
if (!pcl::isXYPointIn2DXYPolygon (projected_points.points[i], *hull))
continue;
ctr2++;
ROS_INFO("Point is in polygon");
inliers.indices.push_back(i);
}
ROS_INFO("Pts in height: %d", ctr);
ROS_INFO("Pts in poly: %d", ctr2);
pcl::ExtractIndices<Point> extract_roi;
extract_roi.setInputCloud (pc_in);
extract_roi.setIndices (boost::make_shared<const pcl::PointIndices> (inliers));
extract_roi.filter (pc_roi);
}
bool RelativePoseEstimatorICPWithNormals<PointT> ::
computeRegistration(Pose3D& relative_pose,
PointCloudConstPtr source_cloud,
PointCloudConstPtr target_cloud,
PointCloudType& aligned_cloud)
{
#ifndef HAVE_PCL_GREATER_THAN_1_2_0
return super::computeRegistration(relative_pose, source_cloud, target_cloud, aligned_cloud);
#else
pcl::IterativeClosestPoint<PointT, PointT> reg;
typedef pcl::registration::TransformationEstimationPointToPlaneLLS<PointT, PointT> PointToPlane;
boost::shared_ptr<PointToPlane> point_to_plane (new PointToPlane);
reg.setTransformationEstimation (point_to_plane);
// typedef TransformationEstimationRGBD<PointT, PointT> TransformRGBD;
// boost::shared_ptr<TransformRGBD> transform_rgbd (new TransformRGBD);
// reg.setTransformationEstimation (transform_rgbd);
#ifdef HAVE_PCL_GREATER_THAN_1_6_0 // rejectors are not well supported before 1.7
boost::shared_ptr<pcl::registration::CorrespondenceRejectorDistance> rejector_distance (new pcl::registration::CorrespondenceRejectorDistance);
rejector_distance->setInputSource<PointT>(source_cloud);
rejector_distance->setInputTarget<PointT>(target_cloud);
rejector_distance->setMaximumDistance(m_distance_threshold);
reg.addCorrespondenceRejector(rejector_distance);
boost::shared_ptr<pcl::registration::CorrespondenceRejectorSurfaceNormal> rejector_normal (new pcl::registration::CorrespondenceRejectorSurfaceNormal);
rejector_normal->setThreshold(cos(M_PI/4.f));
rejector_normal->initializeDataContainer<PointT, PointT>();
rejector_normal->setInputSource<PointT>(source_cloud);
rejector_normal->setInputTarget<PointT>(target_cloud);
rejector_normal->setInputNormals<PointT, PointT>(source_cloud);
rejector_normal->setTargetNormals<PointT, PointT>(target_cloud);
reg.addCorrespondenceRejector(rejector_normal);
boost::shared_ptr<pcl::registration::CorrespondenceRejectorOneToOne> rejector_one_to_one (new pcl::registration::CorrespondenceRejectorOneToOne);
reg.addCorrespondenceRejector(rejector_one_to_one);
typedef pcl::registration::CorrespondenceRejectorSampleConsensus<PointT> RejectorConsensusT;
boost::shared_ptr<RejectorConsensusT> rejector_ransac (new RejectorConsensusT());
rejector_ransac->setInputSource(source_cloud);
rejector_ransac->setInputTarget(target_cloud);
rejector_ransac->setInlierThreshold(m_ransac_outlier_threshold);
rejector_ransac->setMaxIterations(100);
reg.addCorrespondenceRejector(rejector_ransac);
boost::shared_ptr<pcl::registration::CorrespondenceRejectorVarTrimmed> rejector_var_trimmed (new pcl::registration::CorrespondenceRejectorVarTrimmed());
rejector_var_trimmed->setInputSource<PointT>(source_cloud);
rejector_var_trimmed->setInputTarget<PointT>(target_cloud);
rejector_var_trimmed->setMinRatio(0.1f);
rejector_var_trimmed->setMaxRatio(0.75f);
reg.addCorrespondenceRejector(rejector_var_trimmed);
boost::shared_ptr<pcl::registration::CorrespondenceRejectorTrimmed> rejector_trimmed (new pcl::registration::CorrespondenceRejectorTrimmed());
rejector_trimmed->setMinCorrespondences(static_cast<int>(0.1f * source_cloud->size()));
rejector_trimmed->setOverlapRatio(0.5f);
reg.addCorrespondenceRejector(rejector_trimmed);
#endif
#if 0
ntk::Mesh target_mesh;
pointCloudToMesh(target_mesh, *target_cloud);
target_mesh.saveToPlyFile("debug_target.ply");
ntk::Mesh source_mesh;
pointCloudToMesh(source_mesh, *source_cloud);
source_mesh.saveToPlyFile("debug_source.ply");
#endif
reg.setMaximumIterations (m_max_iterations);
reg.setTransformationEpsilon (1e-10);
reg.setMaxCorrespondenceDistance (m_distance_threshold);
reg.setRANSACOutlierRejectionThreshold(m_ransac_outlier_threshold);
#ifdef HAVE_PCL_GREATER_THAN_1_6_0
reg.setInputSource (source_cloud);
#else
reg.setInputCloud (source_cloud);
#endif
reg.setInputTarget (target_cloud);
reg.align (aligned_cloud);
if (!reg.hasConverged())
{
ntk_dbg(1) << "ICP did not converge, ignoring.";
return false;
}
if (0)
{
ntk::Mesh mesh1, mesh2;
pointCloudToMesh(mesh1, aligned_cloud);
pointCloudToMesh(mesh2, *target_cloud);
mesh1.saveToPlyFile("debug_icp_1.ply");
mesh2.saveToPlyFile("debug_icp_2.ply");
}
#if 0
ntk::Mesh debug_mesh;
pointCloudToMesh(debug_mesh, aligned_cloud);
debug_mesh.saveToPlyFile("debug_aligned.ply");
#endif
ntk_dbg_print(reg.getFitnessScore(), 1);
Eigen::Matrix4f t = reg.getFinalTransformation ();
cv::Mat1f T(4,4);
//toOpencv(t,T);
for (int r = 0; r < 4; ++r)
for (int c = 0; c < 4; ++c)
T(r,c) = t(r,c);
relative_pose.setCameraTransform(T);
#endif
return true;
}
