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);
}
Beispiel #4
0
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);
}
Beispiel #5
0
//**********************************************************************************************************************
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);
  
}
Beispiel #6
0
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;
			}
		}
	}
}
Beispiel #9
0
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());
}
Beispiel #11
0
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;
}
Beispiel #12
0
void Writer::writeOuputData(const pcl::PointCloud<pcl::PointNormal>::Ptr &cloud_,
							const std::vector<BandPtr> &bands_,
							const DescriptorParamsPtr &params_,
							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));
  }
}
Beispiel #14
0
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);
		}
	}
}
Beispiel #19
0
	/* ------------------------------------------------------------------------------------------ */
	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;
        }
    }
}
Beispiel #21
0
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);
    }
}
Beispiel #22
0
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;
}
Beispiel #25
0
/** 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;
}
Beispiel #27
0
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;
}
Beispiel #29
0
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;
}
Beispiel #30
0
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);
}