int main (int argc, char** argv)
{
// Setup points only
typedef pcl::PointCloud<pcl::PointXYZ> CloudPointType;
CloudPointType::Ptr cloudPoints(new CloudPointType);
CloudPointType::PointType p;
p.x = 1; p.y = 2; p.z = 3;
cloudPoints->push_back(p);
// Setup normals only
typedef pcl::PointCloud<pcl::Normal> CloudNormalType;
CloudNormalType::Ptr cloudNormals(new CloudNormalType);
CloudNormalType::PointType n;
n.normal_x = 4; n.normal_y = 5; n.normal_z = 6;
cloudNormals->push_back(n);
// Concatenate
typedef pcl::PointCloud<pcl::PointNormal> CloudPointNormalType;
CloudPointNormalType::Ptr cloudPointNormals(new CloudPointNormalType);
pcl::concatenateFields (*cloudPoints, *cloudNormals, *cloudPointNormals);
CloudPointNormalType::PointType p_retrieved = cloudPointNormals->points[0];
std::cout << p_retrieved.x << " " << p_retrieved.y << " " << p_retrieved.z << std::endl;
std::cout << p_retrieved.normal_x << " " << p_retrieved.normal_y << " " << p_retrieved.normal_z << std::endl;
return 0;
}
int main(int argc, char** argv)
{
// Object for storing a pointcloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
// Read a PCD file from disk
if(pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1],*cloud)!=0)
{
return -1;
}
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> normalEstimation;
normalEstimation.setInputCloud(cloud);
normalEstimation.setRadiusSearch(0.03);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
normalEstimation.setSearchMethod(kdtree);
normalEstimation.compute(*normals);
// The triangulation object requires the points and normals to be stored in the same structure
pcl::concatenateFields(*cloud,*normals,*cloudNormals);
// Tree object for searches in this new object
pcl::search::KdTree<pcl::PointNormal>::Ptr kdtree2(new pcl::search::KdTree<pcl::PointNormal>);
kdtree2->setInputCloud(cloudNormals);
// Triangulation object
pcl::GreedyProjectionTriangulation<pcl::PointNormal> triangulation;
// Output object containing the mesh
pcl::PolygonMesh triangles;
// Maximum distance between connected points (maximum edge length)
triangulation.setSearchRadius(30);
// Maximum acceptable distance for a point to be considered,
// relative to the distance of the nearest point
triangulation.setMu(2.5);
// Set the number of neighbors searched for
triangulation.setMaximumNearestNeighbors(200);
// Points will not be connected to the current points
// if normals deviate more than the specified angle.
triangulation.setMaximumSurfaceAngle(M_PI / 4); // 45 degrees
// If false the direction of normals will not be taken into account
// when computing the angle between them
triangulation.setNormalConsistency(false);
// Minimum and maximum angle there can be in a triangle
// The first in not guaranteed, the second is
triangulation.setMinimumAngle(M_PI / 18); // 10 degrees
triangulation.setMaximumAngle(2*M_PI/ 3); // 120 degrees
// Triangulate the cloud
triangulation.setInputCloud(cloudNormals);
triangulation.setSearchMethod(kdtree2);
triangulation.reconstruct(triangles);
// Save to disk
pcl::io::saveVTKFile("mesh.vtk",triangles);
return 0;
}
void ObjectDescription::extractFPFHDescriptors(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudPtr (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampledCloudPtr (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB> objectPtCloudSampled;
*cloudPtr = *cloud;
pcl::VoxelGrid<pcl::PointXYZRGB> grid;
grid.setInputCloud (cloudPtr);
grid.setLeafSize (0.002, 0.002, 0.002);
grid.filter (objectPtCloudSampled);
*sampledCloudPtr = objectPtCloudSampled;
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
ne.setInputCloud (sampledCloudPtr);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr normalsTree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
ne.setSearchMethod (normalsTree);
pcl::PointCloud<pcl::Normal>::Ptr cloudNormals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.03);
ne.compute (*cloudNormals);
pcl::FPFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> fpfh;
fpfh.setInputCloud (sampledCloudPtr);
fpfh.setInputNormals (cloudNormals);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr fpfhTree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
fpfh.setSearchMethod (fpfhTree);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr ptFeatHistograms (new pcl::PointCloud<pcl::FPFHSignature33> ());
fpfh.setRadiusSearch (0.05);
fpfh.compute (*ptFeatHistograms);
fpfhDescriptors = cv::Mat(ptFeatHistograms->size(), FPFH_LEN, CV_32F);
for (size_t i = 0; i < ptFeatHistograms->size(); i++) {
pcl::FPFHSignature33 feat = ptFeatHistograms->points[i];
float sum = 0.0;
for(int j = 0; j < FPFH_LEN; j++ ) {
fpfhDescriptors.at<float>(i, j) = feat.histogram[j];
sum += feat.histogram[j];
}
for(int j = 0; j < FPFH_LEN; j++ ) {
fpfhDescriptors.at<float>(i, j) /= sum;
}
}
}
void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
{
UASSERT(dataPtr!=0);
SensorData & data = *dataPtr;
if(_colorOnly && !data.depthRaw().empty())
{
data.setDepthOrRightRaw(cv::Mat());
}
if(_distortionModel && !data.depthRaw().empty())
{
UTimer timer;
if(_distortionModel->getWidth() == data.depthRaw().cols &&
_distortionModel->getHeight() == data.depthRaw().rows )
{
cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
_distortionModel->undistort(depth);
data.setDepthOrRightRaw(depth);
}
else
{
UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
_distortionModel->getWidth(), _distortionModel->getHeight(),
data.depthRaw().cols, data.depthRaw().rows);
}
if(info) info->timeUndistortDepth = timer.ticks();
}
if(_bilateralFiltering && !data.depthRaw().empty())
{
UTimer timer;
data.setDepthOrRightRaw(util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR));
if(info) info->timeBilateralFiltering = timer.ticks();
}
if(_imageDecimation>1 && !data.imageRaw().empty())
{
UDEBUG("");
UTimer timer;
if(!data.depthRaw().empty() &&
!(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
{
UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
"Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
}
else
{
data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation));
data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation));
std::vector<CameraModel> models = data.cameraModels();
for(unsigned int i=0; i<models.size(); ++i)
{
if(models[i].isValidForProjection())
{
models[i] = models[i].scaled(1.0/double(_imageDecimation));
}
}
data.setCameraModels(models);
StereoCameraModel stereoModel = data.stereoCameraModel();
if(stereoModel.isValidForProjection())
{
stereoModel.scale(1.0/double(_imageDecimation));
data.setStereoCameraModel(stereoModel);
}
}
if(info) info->timeImageDecimation = timer.ticks();
}
if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size() == 1)
{
UDEBUG("");
UTimer timer;
cv::Mat tmpRgb;
cv::flip(data.imageRaw(), tmpRgb, 1);
data.setImageRaw(tmpRgb);
UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
if(data.cameraModels().size() && data.cameraModels()[0].cx())
{
CameraModel tmpModel(
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy(),
float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].localTransform());
data.setCameraModel(tmpModel);
}
if(!data.depthRaw().empty())
{
cv::Mat tmpDepth;
cv::flip(data.depthRaw(), tmpDepth, 1);
data.setDepthOrRightRaw(tmpDepth);
}
if(info) info->timeMirroring = timer.ticks();
}
if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
{
UDEBUG("");
UTimer timer;
cv::Mat depth = util2d::depthFromDisparity(
_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
// set Tx for stereo bundle adjustment (when used)
CameraModel model = CameraModel(
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().left().fy(),
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
data.stereoCameraModel().localTransform(),
-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
data.setCameraModel(model);
data.setDepthOrRightRaw(depth);
data.setStereoCameraModel(StereoCameraModel());
if(info) info->timeDisparity = timer.ticks();
}
if(_scanFromDepth &&
data.cameraModels().size() &&
data.cameraModels().at(0).isValidForProjection() &&
!data.depthRaw().empty())
{
UDEBUG("");
if(data.laserScanRaw().empty())
{
UASSERT(_scanDecimation >= 1);
UTimer timer;
pcl::IndicesPtr validIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
data,
_scanDecimation,
_scanMaxDepth,
_scanMinDepth,
validIndices.get());
float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
cv::Mat scan;
const Transform & baseToScan = data.cameraModels()[0].localTransform();
if(validIndices->size())
{
if(_scanVoxelSize>0.0f)
{
cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
float ratio = float(cloud->size()) / float(validIndices->size());
maxPoints = ratio * maxPoints;
}
else if(!cloud->is_dense)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
cloud = denseCloud;
}
if(cloud->size())
{
if(_scanNormalsK>0)
{
Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
}
}
}
data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
if(info) info->timeScanFromDepth = timer.ticks();
}
else
{
UWARN("Option to create laser scan from depth image is enabled, but "
"there is already a laser scan in the captured sensor data. Scan from "
"depth will not be created.");
}
}
}
