void PCDReader::Read() {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1) //* load the file
{
cout <<"Błąd"<<endl;
}
out_cloud_xyz.write(cloud_xyz);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyzrgb (new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename, *cloud_xyzrgb) == -1) //* load the file
{
cout <<"Błąd"<<endl;
}
out_cloud_xyzrgb.write(cloud_xyzrgb);
pcl::PointCloud<PointXYZSIFT>::Ptr cloud_xyzsift (new pcl::PointCloud<PointXYZSIFT>);
if (pcl::io::loadPCDFile<PointXYZSIFT> (filename, *cloud_xyzsift) == -1) //* load the file
{
cout <<"Błąd"<<endl;
}
out_cloud_xyzsift.write(cloud_xyzsift);
}
void PCDReader::Read() {
CLOG(LTRACE) << "PCDReader::Read\n";
// Try to read the cloud of XYZ points.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
/*
if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1){
CLOG(LWARNING) <<"Cannot read PointXYZ cloud from "<<filename;
}else{
out_cloud_xyz.write(cloud_xyz);
CLOG(LINFO) <<"PointXYZ size: "<<cloud_xyz->size();
CLOG(LINFO) <<"PointXYZ cloud loaded properly from "<<filename;
//return;
}// else
*/
// Try to read the cloud of XYZRGB points.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyzrgb (new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename, *cloud_xyzrgb) == -1){
CLOG(LWARNING) <<"Cannot read PointXYZRGB cloud from "<<filename;
}else{
out_cloud_xyzrgb.write(cloud_xyzrgb);
CLOG(LINFO) <<"PointXYZRGB cloud loaded properly from "<<filename;
//return;
}// else
// Try to read the cloud of XYZSIFT points.
pcl::PointCloud<PointXYZSIFT>::Ptr cloud_xyzsift (new pcl::PointCloud<PointXYZSIFT>);
if (pcl::io::loadPCDFile<PointXYZSIFT> (filename, *cloud_xyzsift) == -1){
CLOG(LWARNING) <<"Cannot read PointXYZSIFT cloud from "<<filename;
}else{
out_cloud_xyzsift.write(cloud_xyzsift);
CLOG(LINFO) <<"PointXYZSIFT cloud loaded properly from "<<filename;
//return;
}// else
}
void xyzrgb2xyzsift::compute() {
CLOG(LTRACE) << "xyzrgb2xyzsift::compute";
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = in_cloud_xyzrgb.read();
cloud->is_dense = true ;
cv::Mat rgbimg(cloud->height, cloud->width, CV_8UC3, cv::Scalar::all(0)) ;
for (register int row = 0; row < cloud->height; row++)
for (register int col = 0; col < cloud->width ; col++) {
pcl::PointXYZRGB& pt = cloud->at(col, row) ;
cv::Vec3b &rgbvec = rgbimg.at<cv::Vec3b>(row, col) ;
rgbvec[2] = pt.r ; //Creating image for all pixels: normal and NaN
rgbvec[1] = pt.g ;
rgbvec[0] = pt.b ;
}
std::vector<cv::KeyPoint> keypoints;
cv::Mat descriptors;
try {
//-- Step 1: Detect the keypoints.
cv::SiftFeatureDetector detector;
detector.detect(rgbimg, keypoints);
//-- Step 2: Calculate descriptors (feature vectors).
cv::SiftDescriptorExtractor extractor;
extractor.compute( rgbimg, keypoints, descriptors);
} catch (...) {
LOG(LERROR) << "sdasdas\n";
}
pcl::PointCloud<PointXYZSIFT>::Ptr cloud_xyzsift(new pcl::PointCloud<PointXYZSIFT>);
//Create output point cloud (this time - unorganized)
cloud_xyzsift->height = 1 ;
cloud_xyzsift->width = keypoints.size() ;
cloud_xyzsift->is_dense = false ;
cloud_xyzsift->points.resize(cloud_xyzsift->height * cloud_xyzsift->width) ;
//Convert SIFT keypoints/descriptors into PointXYZRGBSIFT cloud
pcl::PointCloud<PointXYZSIFT>::iterator pt_iter = cloud_xyzsift->begin();
for (register int i = 0; i < keypoints.size() ; i++) {
//std::cout << " " << keypoints[i].pt.x << " " << keypoints[i].pt.y << std::endl ;
int keyx = std::min(std::max(((unsigned int) round(keypoints[i].pt.x)), 0u), cloud->width) ;
int keyy = std::min(std::max(((unsigned int) round(keypoints[i].pt.y)), 0u), cloud->height) ;
//std::cout << keyx << " " << keyy << std::endl ;
cv::Mat desc = descriptors(cv::Range(i,i + 1), cv::Range::all()) ;
//std::cout << "descriptor " << desc << std::endl ;
//Make XYZSIFT point
PointXYZSIFT& pt_out = *pt_iter++ ;
pcl::PointXYZRGB& pt_in = cloud->at(keyx, keyy) ;
pt_out.x = pt_in.x; pt_out.y = pt_in.y; pt_out.z = pt_in.z ;
for(int j=0; j<descriptors.cols;j++){
pt_out.descriptor[j] = descriptors.row(i).at<float>(j);
}
}
out_cloud_xyzsift.write(cloud_xyzsift);
}