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);
}
void
pcl_ros::PieceExtraction::input_indices_callback (
const PointCloudConstPtr &cloud_transformed, const PointIndicesConstPtr &indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0)
return;
// If cloud is given, check if it's valid
if (!isValid (cloud_transformed))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
return;
}
IndicesPtr indices_ptr;
if (indices)
indices_ptr = boost::make_shared <std::vector<int> > (indices->indices);
// PointCloud cloud_transformed;
// if (!pcl_ros::transformPointCloud (std::string("chess_board"), *cloud, cloud_transformed, tf_listener_))
// {
// NODELET_ERROR ("[%s::computePublish] Error converting output dataset from to chess_board", getName().c_str () );
// return;
// }
impl_.setInputCloud (cloud_transformed->makeShared());
impl_.setIndices (indices_ptr);
std::vector<PointIndices> clusters;
impl_.extract (clusters);
// output ChessBoard
chess_msgs::ChessBoard cb;
for (size_t c = 0; c < clusters.size (); ++c)
{
chess_msgs::ChessPiece p;
p.header.frame_id = cloud_transformed->header.frame_id;
p.header.stamp = ros::Time::now();
// find cluster centroid/color
float x = 0; float y = 0; float z = 0; int color = 0;
for (size_t i = 0; i < clusters[c].indices.size(); i++)
{
int j = clusters[c].indices[i];
x += cloud_transformed->points[j].x;
y += cloud_transformed->points[j].y;
z += cloud_transformed->points[j].z;
unsigned char * rgb = (unsigned char *) &(cloud_transformed->points[j].rgb);
color += (rgb[0] + rgb[1] + rgb[2])/3;
}
x = x/clusters[c].indices.size();
y = y/clusters[c].indices.size();
z = z/clusters[c].indices.size();
// set centroid
p.pose.position.x = x;
p.pose.position.y = y;
p.pose.position.z = z;
// set color
color = color/clusters[c].indices.size();
NODELET_DEBUG ("extract_pieces color %d\n", color);
//printf ("extract_pieces color %d\n", color);
// if(color > 100){
// if(color > 30){
if(color > 100){
p.type = chess_msgs::ChessPiece::WHITE_UNKNOWN;
}else{
p.type = chess_msgs::ChessPiece::BLACK_UNKNOWN;
}
bool new_ = true;
// check if cluster overlaps any other
for (size_t j = 0; j < cb.pieces.size(); j++)
{
if( (fabs(p.pose.position.x-cb.pieces[j].pose.position.x) < 0.012) &&
(fabs(p.pose.position.y-cb.pieces[j].pose.position.y) < 0.012) )
{
cb.pieces[j].pose.position.x = (cb.pieces[j].pose.position.x + p.pose.position.x)/2;
cb.pieces[j].pose.position.y = (cb.pieces[j].pose.position.y + p.pose.position.y)/2;
cb.pieces[j].pose.position.z = (cb.pieces[j].pose.position.z + p.pose.position.z)/2;
new_ = false;
break;
}
}
// add cluster if not overlapping with another
if (new_){
// TODO: add check for outside board, or too large to be a chess piece
cb.pieces.push_back(p);
}
}
pub_output_.publish(cb);
}
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;
}
