void savePointsPly(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, const string& name)
{
stringstream s;
s.str("");
s<< path + name.c_str();
string filename=s.str();
string filenameNumb=filename+".ply";
ofstream plyfile;
plyfile.open(filenameNumb.c_str());
plyfile << "ply\n";
plyfile << "format ascii 1.0\n";
plyfile << "element vertex " << cloud->width <<"\n";
plyfile << "property float x\n";
plyfile << "property float y\n";
plyfile << "property float z\n";
plyfile << "property uchar diffuse_red\n";
plyfile << "property uchar diffuse_green\n";
plyfile << "property uchar diffuse_blue\n";
plyfile << "end_header\n";
for (unsigned int i=0; i<cloud->width; i++){
plyfile << cloud->at(i).x << " " << cloud->at(i).y << " " << cloud->at(i).z << " " << (int)cloud->at(i).r << " " << (int)cloud->at(i).g << " " << (int)cloud->at(i).b << "\n";
}
plyfile.close();
fprintf(stdout, "Writing finished\n");
}
pcl::CorrespondencesPtr
findingCorrespondence()
{
// Find Model-Scene Correspondences with KdTree
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<DescriptorType> match_search;
match_search.setInputCloud (model_descriptors);
// For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
for (size_t i = 0; i < scene_descriptors->size (); ++i)
{
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
{
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
}
}
std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;
return model_scene_corrs;
}
void showSingleNormal(Body * b, pcl::PointCloud<pcl::PointNormal> & cloud, int normalIndex)
{
b->breakVirtualContacts();
PointContact c(b, b, position(cloud.at(normalIndex).x,cloud.at(normalIndex).y,cloud.at(normalIndex).z),
vec3(cloud.at(normalIndex).normal_x,cloud.at(normalIndex).normal_y,cloud.at(normalIndex).normal_z));
VirtualContact * vc = new VirtualContact(1, 1, &c);
b->addVirtualContact(vc);
b->showFrictionCones(1);
}
void Writer::saveCloudMatrix(const std::string &filename_,
const pcl::PointCloud<pcl::PointNormal>::Ptr &cloud_)
{
cv::Mat items = cv::Mat::zeros(cloud_->size(), 3, CV_32FC1);
for (size_t i = 0; i < cloud_->size(); i++)
{
items.at<float>(i, 0) = cloud_->at(i).x;
items.at<float>(i, 1) = cloud_->at(i).y;
items.at<float>(i, 2) = cloud_->at(i).z;
}
Writer::writeMatrix(filename_, items);
}
//**********************************************************************************************************************
void saveSelectedPointCloud ()
{
//cout << "Indices Size: " << pcl_indices.size () << "\t Cloud Size: " << gCloud->size () << endl;
pcl::PointCloud<PointT>::Ptr nCloud (new pcl::PointCloud<PointT>);
for (int i = 0; i < pcl_indices.size (); i++) {
int index = pcl_indices.at (i);
nCloud->push_back (gCloud->at (index));
cout << "PCL Value # " << gCloud->at (index) << "\t " << nCloud->points[i] << endl;
}
cout << endl << "Indices Size: " << pcl_indices.size () << "\tCopied Size: " << nCloud->size () << endl;
pcl::io::savePCDFileASCII ("/home/krishneel/Desktop/readPCL/selected_cloud.pcd", *nCloud);
}
cv::Mat getImage(const pcl::PointCloud<PointT>::Ptr cloud, float ap_ratio, float fx, float fy, float center_x, float center_y)
{
int img_h = 480 * ap_ratio;
int img_w = 640 * ap_ratio;
cv::Mat img = cv::Mat::zeros(img_h, img_w, CV_8UC3);
for( size_t i = 0 ; i < cloud->size() ; i++ )
{
PointT *pt_ptr = &cloud->at(i);
uint32_t rgb = pt_ptr->rgba;
int img_x = round(((*pt_ptr).x / (*pt_ptr).z * fx + center_x)*ap_ratio);
int img_y = round(((*pt_ptr).y / (*pt_ptr).z * fy + center_y)*ap_ratio);
if( img_x < 0 ) img_x = 0;
if( img_y < 0 ) img_y = 0;
if( img_x >= img_w ) img_x = img_w-1;
if( img_y >= img_h ) img_y = img_h-1;
img.at<uchar>(img_y, img_x*3+2) = (rgb >> 16) & 0x0000ff;
img.at<uchar>(img_y, img_x*3+1) = (rgb >> 8) & 0x0000ff;
img.at<uchar>(img_y, img_x*3+0) = (rgb) & 0x0000ff;
}
return img;
}
// PlaneSegmentationPclRgbAlgorithm Public API
pcl::PointCloud<pcl::PointXYZRGB>::Ptr PlaneSegmentationPclRgbAlgorithm::segmentBiggestPlane (pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb_orig,
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_orig,
pcl::ModelCoefficients::Ptr coefficients)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb_dst (new pcl::PointCloud<pcl::PointXYZRGB>);
// we will need a copy of the pointcloud
*cloud_rgb_dst = *cloud_rgb_orig;
// Plane segmentation indices
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Downsample to improve performance
if (pointcloud_downsample)
getBiggestPlaneInliersDownsampling(inliers, coefficients, cloud_orig);
else
getBiggestPlaneInliers(inliers, coefficients, cloud_orig);
// set all points but inliers to black
for (size_t i = 0; i < cloud_rgb_dst->size(); ++i)
{
cloud_rgb_dst->at(i).rgb = 0;
}
for (size_t i = 0; i < inliers->indices.size(); ++i)
{
cloud_rgb_dst->at(inliers->indices[i]).rgb = cloud_rgb_orig->at(inliers->indices[i]).rgb;
}
// coefficients should be always pointing to the camera. If they are not, they will be inverted
fixPlaneCoefficientsOrientation(coefficients);
return cloud_rgb_dst;
}
void adjustNormalsToViewPoints(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & viewpoints,
pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud)
{
if(viewpoints->size() && cloud.size())
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (viewpoints);
for(unsigned int i=0; i<cloud.size(); ++i)
{
std::vector<int> indices;
std::vector<float> dist;
tree->nearestKSearch(pcl::PointXYZ(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z), 1, indices, dist);
UASSERT(indices.size() == 1);
Eigen::Vector3f v = viewpoints->at(indices[0]).getVector3fMap() - cloud.points[i].getVector3fMap();
Eigen::Vector3f n(cloud.points[i].normal_x, cloud.points[i].normal_y, cloud.points[i].normal_z);
float result = v.dot(n);
if(result < 0)
{
//reverse normal
cloud.points[i].normal_x *= -1.0f;
cloud.points[i].normal_y *= -1.0f;
cloud.points[i].normal_z *= -1.0f;
}
}
}
}
inline bool calcPointsPCL(Mat &depth_img, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, float scale) {
// TODO: dont handle only scale, but also the offset (c_x, c_y) of the given images center to the original image center (for training and roi images!)
cloud.reset(new pcl::PointCloud<pcl::PointXYZ>(depth_img.cols, depth_img.rows));
const float bad_point = 0;//std::numeric_limits<float>::quiet_NaN ();
const float constant_x = M_PER_MM / F_X;
const float constant_y = M_PER_MM / F_Y;
bool is_valid = false;
int centerX = depth_img.cols/2.0;
int centerY = depth_img.rows/2.0;
float x, y, z;
int row, col = 0;
for (row = 0, y = -centerY; row < depth_img.rows; ++row, ++y) {
float* r_ptr = depth_img.ptr<float>(row);
for (col = 0, x = -centerX; col < depth_img.cols; ++col, ++x) {
pcl::PointXYZ newPoint;
z = r_ptr[col];
if(z) {
newPoint.x = (x/scale)*z*constant_x;
newPoint.y = (y/scale)*z*constant_y;
newPoint.z = z*M_PER_MM;
is_valid = true;
} else {
newPoint.x = newPoint.y = newPoint.z = bad_point;
}
cloud->at(col,row) = newPoint;
}
}
return is_valid;
}
void
MultiviewRecognizerWithChangeDetection<PointT>::reconstructionFiltering(typename pcl::PointCloud<PointT>::Ptr observation,
pcl::PointCloud<pcl::Normal>::Ptr observation_normals, const Eigen::Matrix4f &absolute_pose, size_t view_id) {
CloudPtr observation_transformed(new Cloud);
pcl::transformPointCloud(*observation, *observation_transformed, absolute_pose);
CloudPtr cloud_tmp(new Cloud);
std::vector<int> indices;
v4r::ChangeDetector<PointT>::difference(observation_transformed, removed_points_cumulated_history_[view_id],
cloud_tmp, indices, param_.tolerance_for_cloud_diff_);
/* Visualization of changes removal for reconstruction:
Cloud rec_changes;
rec_changes += *cloud_transformed;
v4r::VisualResultsStorage::copyCloudColored(*removed_points_cumulated_history_[view_id], rec_changes, 255, 0, 0);
v4r::VisualResultsStorage::copyCloudColored(*cloud_tmp, rec_changes, 200, 0, 200);
stringstream ss;
ss << view_id;
visResStore.savePcd("reconstruction_changes_" + ss.str(), rec_changes);*/
std::vector<bool> preserved_mask(observation->size(), false);
for (std::vector<int>::iterator i = indices.begin(); i < indices.end(); i++) {
preserved_mask[*i] = true;
}
for (size_t j = 0; j < preserved_mask.size(); j++) {
if (!preserved_mask[j]) {
setNan(observation->at(j));
setNan(observation_normals->at(j));
}
}
PCL_INFO("Points by change detection removed: %d\n", observation->size() - indices.size());
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudXYZtoRGBA(
pcl::PointCloud<pcl::PointXYZ>::ConstPtr inCloud)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr newCloud
(new pcl::PointCloud<pcl::PointXYZRGBA>);
int numPoints = inCloud->points.size();
newCloud->resize(numPoints);
for (int i; i<numPoints; i++) {
pcl::PointXYZRGBA &np = newCloud->at(i);
const pcl::PointXYZ &op = inCloud->at(i);
np.x = op.x;
np.y = op.y;
np.z = op.z;
np.r = 1.0;
np.g = 1.0;
np.b = 1.0;
np.a = 0.0;
}
return newCloud;
}
void Writer::writeOuputData(const pcl::PointCloud<pcl::PointNormal>::Ptr &cloud_,
const std::vector<BandPtr> &bands_,
const DescriptorParamsPtr ¶ms_,
const int targetPoint_)
{
DCHParams *params = dynamic_cast<DCHParams *>(params_.get());
if (params == NULL)
{
LOGW << "Output data generation only for DCH, skipping";
return;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr colorCloud = CloudFactory::createColorCloud(cloud_, Utils::palette12(0));
pcl::io::savePCDFileASCII(OUTPUT_DIR "cloud.pcd", *colorCloud);
(*colorCloud)[targetPoint_].rgba = Utils::getColor(255, 0, 0);
pcl::io::savePCDFileASCII(OUTPUT_DIR "pointPosition.pcd", *colorCloud);
pcl::PointCloud<pcl::PointNormal>::Ptr patch = Extractor::getNeighbors(cloud_, cloud_->at(targetPoint_), params->searchRadius);
pcl::io::savePCDFileASCII(OUTPUT_DIR "patch.pcd", *patch);
std::vector<pcl::PointCloud<pcl::PointNormal>::Ptr> planes = generatePlanes(bands_, params);
for (size_t i = 0; i < bands_.size(); i++)
{
if (!bands_[i]->points->empty())
{
char name[100];
sprintf(name, OUTPUT_DIR "band%d.pcd", (int) i);
pcl::io::savePCDFileASCII(name, *CloudFactory::createColorCloud(bands_[i]->points, Utils::palette12(i + 1)));
sprintf(name, OUTPUT_DIR "planeBand%d.pcd", (int) i);
pcl::io::savePCDFileASCII(name, *planes[i]);
}
}
// Write histogram data
std::vector<Histogram> angleHistograms = DCH::generateAngleHistograms(bands_, params->useProjection);
Writer::writeHistogram("angle_distribution", "Angle Distribution Across the Bands", angleHistograms, DEG2RAD(20), -M_PI / 2, M_PI / 2);
// Write the descriptor to a file
std::ofstream output;
output.open(OUTPUT_DIR "descriptor.dat", std::fstream::out);
output << std::fixed << std::setprecision(4);
for (size_t i = 0; i < bands_.size(); i++)
{
for (size_t j = 0; j < bands_.at(i)->descriptor.size(); j++)
output << bands_.at(i)->descriptor[j] << "\t";
output << "\n";
}
output.close();
}
void
PointCloud2Vector2d (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::on_nurbs::vector_vec2d &data)
{
for (unsigned i = 0; i < cloud->size (); i++)
{
pcl::PointXYZ &p = cloud->at (i);
if (!pcl_isnan (p.x) && !pcl_isnan (p.y))
data.push_back (Eigen::Vector2d (p.x, p.y));
}
}
void
PointCloud2Vector3d (pcl::PointCloud<Point>::Ptr cloud, pcl::on_nurbs::vector_vec3d &data)
{
for (unsigned i = 0; i < cloud->size (); i++)
{
Point &p = cloud->at (i);
if (!pcl_isnan (p.x) && !pcl_isnan (p.y) && !pcl_isnan (p.z))
data.push_back (Eigen::Vector3d (p.x, p.y, p.z));
}
}
position calcPosition (std::vector<cv::Point> contours)
{
position World_Pose;
int cnt_z = 0;
int cnt_y = 0;
int cnt_x = 0;
pcl::PointXYZ p1;
if (globalcloud.size() != 0)
{
for ( int b = 0; b < contours.size(); b++ )
{
p1.x = globalcloud.at(contours[b].x, contours[b].y).x;
p1.y = globalcloud.at(contours[b].x, contours[b].y).y;
p1.z = globalcloud.at(contours[b].x, contours[b].y).z;
if ( !isnan(p1.x))
{
World_Pose.x += p1.x;
cnt_x++;
}
if ( !isnan(p1.y))
{
World_Pose.y += p1.y;
cnt_y++;
}
if ( !isnan(p1.z) && (p1.z > 0))
{
World_Pose.z += p1.z;
cnt_z++;
}
}
if ((cnt_x != 0) && (cnt_y != 0) && (cnt_z != 0))
{
World_Pose.x = World_Pose.x / cnt_x;
World_Pose.y = World_Pose.y / cnt_y;
World_Pose.z = World_Pose.z / cnt_z;
}
// printf(" [%f, %f] ", World_Pose.x, World_Pose.z );
}
return World_Pose;
}
void TestClass::pclToArray(const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr, float* out_points_array)
{
for (size_t i = 0; i < in_pcl_pc_ptr->size(); i++)
{
pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
out_points_array[i * 4 + 0] = point.x;
out_points_array[i * 4 + 1] = point.y;
out_points_array[i * 4 + 2] = point.z;
out_points_array[i * 4 + 3] = point.intensity;
}
}
void cloudToMat(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, cv::Mat &mat) {
#pragma omp parallel for
for (int row = 0; row < cloud->height; row++) {
for (int col = 0; col < cloud->width; col++) {
pcl::PointXYZ point = cloud->at(col, row);
mat.at<float3>(row, col).x = point.x;
mat.at<float3>(row, col).y = point.y;
mat.at<float3>(row, col).z = point.z;
}
}
}
inline void
filter_depth_discontinuity(
const pcl::PointCloud<PointT> &in,
pcl::PointCloud<PointT> &out,
int neighbors = 2,
float threshold = 0.3,
float distance_min = 1,
float distance_max = 300,
float epsilon = 0.5
)
{
//std::cout << "neigbors " << neighbors << " epsilon " << epsilon << " distance_max " << distance_max <<std::endl;
boost::shared_ptr<pcl::search::KdTree<PointT> > tree_n( new pcl::search::KdTree<PointT>() );
tree_n->setInputCloud(in.makeShared());
tree_n->setEpsilon(epsilon);
for(int i = 0; i< in.points.size(); ++i){
std::vector<int> k_indices;
std::vector<float> square_distance;
if ( in.points.at(i).z > distance_max || in.points.at(i).z < distance_min) continue;
//Position in image is known
tree_n->nearestKSearch(in.points.at(i), neighbors, k_indices, square_distance);
//std::cout << "hier " << i << " z " << in.points.at(i).z <<std::endl;
#ifdef USE_SQUERE_DISTANCE
const float point_distance = distance_to_origin<PointT>(in.points.at(i));
#else
const float point_distance = in.points.at(i).z;
#endif
float value = 0; //point_distance;
unsigned int idx = 0;
for(int n = 0; n < k_indices.size(); ++n){
#ifdef USE_SQUERE_DISTANCE
float distance_n = distance_to_origin<PointT>(in.points.at(k_indices.at(n)));
#else
float distance_n = in.points.at(k_indices.at((n))).z;
#endif
if(value < distance_n - point_distance){
idx = k_indices.at(n);
value = distance_n - point_distance;
}
}
if(value > threshold){
out.push_back(in.points.at(i));
out.at(out.size()-1).intensity = sqrt(value);
}
}
}
/* ------------------------------------------------------------------------------------------ */
void makeImage () {
// Compute the maximum depth
double maxDepth = -1, minDepth = 10000000;
for (int h=0; h<point_cloud_ptr->height; h++) {
for (int w=0; w<point_cloud_ptr->width; w++) {
pcl::PointXYZRGBA point = point_cloud_ptr->at(w, h);
if(point.z > maxDepth) maxDepth = point.z;
if(point.z < minDepth) minDepth = point.z;
}
}
// Create the image
im = cv::Mat(point_cloud_ptr->height, 2*point_cloud_ptr->width, CV_8UC3);
if (point_cloud_ptr->empty()) return;
for (int h=0; h<im.rows; h++) {
for (int w=0; w<point_cloud_ptr->width; w++) {
pcl::PointXYZRGBA point = point_cloud_ptr->at(w, h);
Eigen::Vector3i rgb = point.getRGBVector3i();
im.at<cv::Vec3b>(h,w)[0] = rgb[2];
im.at<cv::Vec3b>(h,w)[1] = rgb[1];
im.at<cv::Vec3b>(h,w)[2] = rgb[0];
}
for (int w=0; w<point_cloud_ptr->width; w++) {
pcl::PointXYZRGBA point = point_cloud_ptr->at(w, h);
int val = (int) (255 * ((point.z-minDepth)/(maxDepth-minDepth)));
im.at<cv::Vec3b>(h,w+point_cloud_ptr->width)[0] = val;
im.at<cv::Vec3b>(h,w+point_cloud_ptr->width)[1] = val;
im.at<cv::Vec3b>(h,w+point_cloud_ptr->width)[2] = val;
}
}
// Convert the image to hsv for processing later
cv::cvtColor(im, im_hsv, cv::COLOR_BGR2HSV);
cv::imshow("image", im);
cv::waitKey(1);
}
void AdjustCloudNormal(pcl::PointCloud<PointT>::Ptr cloud, pcl::PointCloud<NormalT>::Ptr cloud_normals)
{
pcl::PointCloud<myPointXYZ>::Ptr center(new pcl::PointCloud<myPointXYZ>());
ComputeCentroid(cloud, center);
myPointXYZ origin = center->at(0);
std::cerr << origin.x << " " << origin.y << " " << origin.z << std::endl;
pcl::transformPointCloud(*cloud, *cloud, Eigen::Vector3f(-origin.x, -origin.y, -origin.z), Eigen::Quaternion<float> (1,0,0,0));
int num = cloud->points.size();
float diffx, diffy, diffz, dist, theta;
for( int j = 0; j < num ; j++ )
{
PointT temp = cloud->at(j);
NormalT temp_normals = cloud_normals->at(j);
diffx = temp.x;
diffy = temp.y;
diffz = temp.z;
dist = sqrt( diffx*diffx + diffy*diffy + diffz*diffz );
theta = acos( (diffx*temp_normals.normal_x + diffy*temp_normals.normal_y + diffz*temp_normals.normal_z)/dist );
if( theta > PI/2)
{
cloud_normals->at(j).normal_x = -cloud_normals->at(j).normal_x;
cloud_normals->at(j).normal_y = -cloud_normals->at(j).normal_y;
cloud_normals->at(j).normal_z = -cloud_normals->at(j).normal_z;
}
}
}
void ldr_to_cloud(const Eigen::MatrixXf& ldr_data, pcl::PointCloud<pcl::PointXYZ>& cloud)
{
cloud.clear();
cloud.is_dense = false;
cloud.points.resize(ldr_data.rows());
for (int r=0; r < ldr_data.rows(); r++)
{
pcl::PointXYZ& p = cloud.at(r);
p.x = ldr_data(r, 0);
p.y = ldr_data(r, 1);
p.z = ldr_data(r, 2);
}
}
void
pcl::StereoMatching::getVisualMap (pcl::PointCloud<pcl::RGB>::Ptr vMap)
{
if ( vMap->width != width_ || vMap->height != height_)
{
vMap->resize(width_*height_);
vMap->width = width_;
vMap->height = height_;
}
if ( vMap->is_dense)
vMap->is_dense = false;
pcl::RGB invalid_val;
invalid_val.r = 0;
invalid_val.g = 255;
invalid_val.b = 0;
float scale = 255.0f / (16.0f * static_cast<float> (max_disp_));
for (int y = 0; y<height_; y++)
{
for (int x = 0; x<width_; x++)
{
if (disp_map_[y * width_+ x] <= 0)
{
vMap->at (x,y) = invalid_val;
}
else
{
unsigned char val = static_cast<unsigned char> (floor (scale*disp_map_[y * width_+ x]));
vMap->at (x, y).r = val;
vMap->at (x, y).g = val;
vMap->at (x, y).b = val;
}
}
}
}
// Find a bounding box around the object to grasp (pc)
// Outputs the shape and the pose of the bounding box
// Inspired from https://github.com/unboundedrobotics/ubr1_preview/blob/master/ubr1_grasping/src/shape_extraction.cpp
BoundingBox findBoundingBox(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& pc){
struct BoundingBox BB;
ros::NodeHandle nh;
ros::Rate r(3);
double x_min = 9999.0;
double x_max = -9999.0;
double y_min = 9999.0;
double y_max = -9999.0;
double z_min = 9999.0;
double z_max = -9999.0;
for(int i = 0; i < pc->size(); i++){
double pc_x = pc->at(i).x;
double pc_y = pc->at(i).y;
double pc_z = pc->at(i).z;
if(pc_x < x_min) x_min = pc_x;
if(pc_y < y_min) y_min = pc_y;
if(pc_z < z_min) z_min = pc_z;
if(pc_x > x_max) x_max = pc_x;
if(pc_y > y_max) y_max = pc_y;
if(pc_z > z_max) z_max = pc_z;
}
BB.x_min = x_min;
BB.x_max = x_max;
BB.y_min = y_min;
BB.y_max = y_max;
BB.z_min = z_min;
BB.z_max = z_max;
return BB;
}
pcl::PointCloud<PointT>::Ptr FilterBoundary(const pcl::PointCloud<PointT>::Ptr cloud)
{
float threshold = 0.04;
int BLen = 5;
int w = cloud->width;
int h = cloud->height;
int num = cloud->size();
cv::Mat depthmap = cv::Mat::ones(h+2*BLen, w+2*BLen, CV_32FC1)*-1000;
for(int i = 0 ; i < num; i++ )
{
if( pcl_isfinite(cloud->at(i).z) == true )
{
int r_idx = i / w + BLen;
int c_idx = i % w + BLen;
depthmap.at<float>(r_idx, c_idx) = cloud->at(i).z;
}
}
pcl::PointCloud<PointT>::Ptr cloud_f(new pcl::PointCloud<PointT>());
for(int i=0 ; i<num; i++ ){
int row = i / w + BLen;
int col = i % w + BLen;
if( pcl_isfinite(cloud->at(i).z) == true )
{
float zCur = depthmap.at<float>(row, col);
if( fabs(depthmap.at<float>(row-BLen, col)-zCur) > threshold || fabs(depthmap.at<float>(row+BLen, col)-zCur) > threshold
|| fabs(zCur-depthmap.at<float>(row, col-BLen)) > threshold || fabs(zCur-depthmap.at<float>(row, col+BLen)) > threshold)
;//Boundary->push_back(cloud_rgb->points[i]);
else
cloud_f->push_back(cloud->at(i));
}
}
return cloud_f;
}
/** Assumptions: No 3d Info available yet */
void Node::projectTo3D(const cv::Mat& depth,
std::vector<cv::KeyPoint>& feature_locations_2d,
std::vector<Eigen::Vector4f>& feature_locations_3d,
const pcl::PointCloud<pcl::PointXYZRGB>& point_cloud){ //TODO: Use this instead of member "pc" or remove argument
std::clock_t starttime=std::clock();
cv::Point2f p2d;
if(feature_locations_3d.size()){
ROS_INFO("There is already 3D Information in the FrameInfo, clearing it");
feature_locations_3d.clear();
}
for(unsigned int i = 0; i < feature_locations_2d.size(); /*increment at end of loop*/){
p2d = feature_locations_2d[i].pt;
if (p2d.x >= depth.cols || p2d.x < 0 || p2d.y >= depth.rows || p2d.y < 0 ||
std::isnan(p2d.x) || std::isnan(p2d.y)){
ROS_WARN_STREAM("Ignoring invalid keypoint: " << p2d); //Does it happen at all? If not, remove this code block
feature_locations_2d.erase(feature_locations_2d.begin()+i);
continue;
}
float depth_val = depth.at<float>((int)p2d.y, (int)p2d.x);
if(std::isnan(depth_val) ) {//No 3d pose would be available
ROS_DEBUG_STREAM("No depth for: " << p2d);
feature_locations_2d.erase(feature_locations_2d.begin()+i);
continue;
}
pcl::PointXYZRGB p3d = point_cloud.at((int) p2d.x,(int) p2d.y);
// Todo: should not happen
if ( isnan(p3d.x) || isnan(p3d.y) || isnan(p3d.z)){
// ROS_INFO("nan in pointcloud! %f, %f",p2d.x, p2d.y);
feature_locations_2d.erase(feature_locations_2d.begin()+i);
continue;
}
feature_locations_3d.push_back(Eigen::Vector4f(p3d.x, p3d.y, p3d.z, 1.0));
i++; //Only increment if no element is removed from vector
}
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}
double OcclusionCulling::calcAvgAccuracy(pcl::PointCloud<pcl::PointXYZ> pointCloud)
{
double avgAccuracy;
double pointError,val,errorSum=0, errorRatio;
for (int j=0; j<pointCloud.size(); j++)
{
val = pointCloud.at(j).z;//depth
pointError= 0.0000285 * val * val;
// errorRatio=pointError/maxAccuracyError;
errorSum += pointError;
}
avgAccuracy=errorSum/pointCloud.size();
return avgAccuracy;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr pclManager::xyz_from_xyzi(pcl::PointCloud<pcl::PointXYZI>::ConstPtr intensityCloud)
{
//Transform a cloud with intensity information to a cloud without intensity
pcl::PointCloud<pcl::PointXYZ>::Ptr xyzCloud(new pcl::PointCloud<pcl::PointXYZ>);
xyzCloud->width = intensityCloud->width;
xyzCloud->height = intensityCloud->height;
xyzCloud->is_dense = false;
xyzCloud->points.resize(xyzCloud->width * xyzCloud->height);
// pcl::PointXYZ temp;
for(int i=0; i<intensityCloud->size(); i++)
{
xyzCloud->points[i].x = intensityCloud->at(i).x;
xyzCloud->points[i].y = intensityCloud->at(i).y;
xyzCloud->points[i].z = intensityCloud->at(i).z;
// temp.x = intensityCloud->at(i).x;
// temp.y = intensityCloud->at(i).y;
// temp.z = intensityCloud->at(i).z;
// xyzCloud->push_back(temp);
}
return xyzCloud;
}
pcl::CorrespondencesPtr Recognizer::flann_matcher(pcl::PointCloud<DescriptorType>::Ptr input_descriptors, pcl::PointCloud<DescriptorType>::Ptr target_descriptors, float match_thresh) {
pcl::KdTreeFLANN<DescriptorType> match_search;
match_search.setInputCloud(input_descriptors);
for(size_t i = 0; i < target_descriptors->size (); ++i) {
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
for (int j = 0; j < 33; j++) { // for each bin
if(pcl_isnan(target_descriptors->at(i).histogram[j]) || !pcl_isfinite(target_descriptors->at(i).histogram[j]))
continue;
}
int found_neighs = match_search.nearestKSearch(target_descriptors->at(i), 1, neigh_indices, neigh_sqr_dists);
if(found_neighs == 1 && neigh_sqr_dists[0] < match_thresh) { // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
(this->corrs)->push_back(corr);
}
}
return this->corrs;
}
pcl::PointCloud<pcl::PointXYZI>::Ptr pclManager::filter_cloud(pcl::PointCloud<pcl::PointXYZI>::Ptr ptcloud, const float &threshX, const float &threshY, const float &threshZ, const float &threshI)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZI>);
//Filter the input cloud with the given values for the 3 coordinates and the intensity
for (size_t i = 0; i < ptcloud->points.size(); ++i)
{
if ((ptcloud->points[i].intensity>threshI)&&(ptcloud->points[i].z<(threshZ))&&(ptcloud->points[i].y<threshY) && (ptcloud->points[i].y>-threshY) && (ptcloud->points[i].x<threshX) && (ptcloud->points[i].x>-threshX))
filtered->push_back(ptcloud->at(i));
}
std::cout << "Filtering " << ptcloud->size() <<" points results in " << filtered->size() << "points" << std::endl;
return filtered;
}
pcl::PointCloud<PointT>::Ptr findNearestObject(){
float min_distance = 10000000000000000;
pcl::PointCloud<PointT>::Ptr neareast_pointcloud(new pcl::PointCloud<PointT>);
int index = 0;
for(int i=0; i<object_vector_centroids.size(); i++){
pcl::PointCloud<PointT>::Ptr object = object_vector_centroids.at(i);
float distance = pcl::euclideanDistance(tracked_object_centroid->at(0),object->at(0));
if(distance < min_distance){
min_distance = distance;
index = i;
}
}
return object_vector.at(index);
}
