int main(int argc, char** argv)
{
Options opts;
if (options(argc, argv, opts))
return 1;
// Load stuff
PCL_INFO("Loading src_pd: %s\n", opts.src_pcd_file.c_str());
PointCloud::Ptr src_cloud(new PointCloud());
if (pcl::io::loadPCDFile(opts.src_pcd_file, *src_cloud) < 0)
{
std::cout << "Error loading input point cloud " << opts.src_pcd_file << std::endl;
return -1;
}
// Do downsampling
PointCloud::Ptr out_cloud(new PointCloud());
pcl::VoxelGrid<PointT> voxel_grid;
voxel_grid.setInputCloud(src_cloud);
// FIXME Should be an argument
voxel_grid.setLeafSize(opts.leaf_size, opts.leaf_size, opts.leaf_size);
voxel_grid.filter(*out_cloud);
// Save downsampled cloud
pcl::io::savePCDFileBinary(opts.out_pcd_file, *out_cloud);
PCL_INFO("Downsampled from %d to %d points\n", src_cloud->size(), out_cloud->size());
return 0;
}
void Segmentation::computeHeight()
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr gr(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr out_cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
gr = _ground.getPlaneCloud();
pcl::transformPointCloud(*gr, *out_cloud, transformXY);
Eigen::Vector4f centroid;
pcl::compute3DCentroid (*gr, centroid);
for(size_t i = 0; i < _planes.size();i++)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr plane(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr plane_out(new pcl::PointCloud<pcl::PointXYZRGBA>);
plane = _planes[i].getPlaneCloud();
pcl::transformPointCloud(*plane, *plane_out, transformXY);
Eigen::Vector4f centroidPlane;
pcl::compute3DCentroid (*plane, centroidPlane);
Eigen::Vector4f height = centroidPlane - centroid;
_planes[i].setHeight(height);
Eigen::Vector4f height_out = _planes[i].getHeight();
_planes[i].setCentroid(centroidPlane);
//std::cout << "centroid:" << centroidPlane[0] << " " << centroidPlane[1] << " " << centroidPlane[2] << " " <<centroidPlane[3] << " \n";
//std::cout << "height:" << height[0] << " " << height[1] << " " << height[2] << " " <<height[3] << " \n";
// pcl::PointXYZ c;
}
}
PCLCloud::Ptr loadSensorMessage(const QString &filename,
Eigen::Vector4f &origin,
Eigen::Quaternionf &orientation)
{
PCLCloud::Ptr out_cloud(new PCLCloud);
//Load the given file
if (pcl::io::loadPCDFile(filename.toStdString(), *out_cloud, origin, orientation) < 0)
{
//loading failed
out_cloud.reset();
}
return out_cloud;
}
int main(int argc, char** argv)
{
Options opts;
if (options(argc, argv, opts))
return 1;
// Load stuff
PCL_INFO("Loading src_pd: %s\n", opts.src_pcd_file.c_str());
pcl::PointCloud<pcl::PointXYZI>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZI>());
if (pcl::io::loadPCDFile(opts.src_pcd_file, *src_cloud) < 0)
{
std::cout << "Error loading input point cloud " << opts.src_pcd_file << std::endl;
return -1;
}
// Do normal estimation
PointCloudWithNormals::Ptr out_cloud(new PointCloudWithNormals);
pcl::NormalEstimation<pcl::PointXYZI, pcl::PointXYZINormal> norm_est;
pcl::search::KdTree<pcl::PointXYZI>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZI>());
norm_est.setSearchMethod(tree);
norm_est.setKSearch(opts.k_search);
norm_est.setInputCloud(src_cloud);
// Only outputs the normals
norm_est.compute (*out_cloud);
// This copies over the XYZ coordinates
pcl::copyPointCloud (*src_cloud, *out_cloud);
// Save cloud with normals
pcl::io::savePCDFileBinary(opts.out_pcd_file, *out_cloud);
return 0;
}
void WorldDownloadManager::extractMeshWorker(kinfu_msgs::KinfuTsdfRequestConstPtr req)
{
ROS_INFO("kinfu: Extract Mesh Worker started.");
// prepare with same header
kinfu_msgs::KinfuTsdfResponsePtr resp(new kinfu_msgs::KinfuTsdfResponse());
resp->tsdf_header = req->tsdf_header;
resp->reference_frame_id = m_reference_frame_name;
ROS_INFO("kinfu: Locking kinfu...");
if (!lockKinfu())
return; // shutting down or synchronization error
ROS_INFO("kinfu: Locked.");
pcl::PointCloud<pcl::PointXYZI>::Ptr kinfu_cloud = req->request_reset ?
m_kinfu->extractWorldAndReset() : m_kinfu->extractWorld();
unlockKinfu();
Eigen::Affine3f transformation = m_reverse_initial_transformation;
std::vector<Mesh::Ptr> meshes;
ROS_INFO("kinfu: Marching cubes...");
if (!marchingCubes(kinfu_cloud,meshes))
return; // ERROR
kinfu_cloud->clear(); // save some memory
std::vector<PointCloud::Ptr> clouds;
std::vector<TrianglesPtr> clouds_triangles;
ROS_INFO("kinfu: Divide triangles and points...");
for (uint i = 0; i < meshes.size(); i++)
{
clouds.push_back(PointCloud::Ptr(new PointCloud()));
clouds_triangles.push_back(TrianglesPtr(new Triangles()));
separateMesh(meshes[i],clouds[i],clouds_triangles[i]);
}
meshes.clear(); // save some memory
TrianglesPtr triangles(new Triangles());
PointCloud::Ptr cloud(new PointCloud());
ROS_INFO("kinfu: Merging points...");
mergePointCloudsAndMesh(clouds,cloud,&clouds_triangles,&(*triangles));
// if needed, transform
if (req->request_transformation)
{
ROS_INFO("kinfu: A custom transformation will be applied.");
Eigen::Affine3f tr = toEigenAffine(req->transformation_linear,req->transformation_translation);
transformation = tr * transformation;
}
ROS_INFO("kinfu: Applying transformation...");
pcl::transformPointCloud(*cloud,*cloud,transformation);
// if needed, crop
if (req->request_bounding_box)
{
ROS_INFO("kinfu: Cropping...");
TrianglesPtr out_triangles(new Triangles());
PointCloud::Ptr out_cloud(new PointCloud());
cropMesh(req->bounding_box_min,req->bounding_box_max,cloud,triangles,out_cloud,out_triangles);
cloud = out_cloud;
triangles = out_triangles;
}
ROS_INFO("kinfu: Publishing...");
pclPointCloudToMessage(cloud,resp->point_cloud);
resp->triangles.swap(*triangles);
m_pub.publish(resp);
ROS_INFO("kinfu: Extract Mesh Worker complete.");
}
void WorldDownloadManager::extractKnownWorker(kinfu_msgs::KinfuTsdfRequestConstPtr req)
{
ROS_INFO("kinfu: Extract Known Worker started.");
// prepare with same header
kinfu_msgs::KinfuTsdfResponsePtr resp(new kinfu_msgs::KinfuTsdfResponse());
resp->tsdf_header = req->tsdf_header;
resp->reference_frame_id = m_reference_frame_name;
ROS_INFO("kinfu: Locking kinfu...");
if (!lockKinfu())
return; // shutting down or synchronization error
ROS_INFO("kinfu: Locked.");
m_kinfu->syncKnownPoints();
if (req->request_reset)
m_kinfu->reset();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = m_cube_listener->GetOccupiedVoxelCenters();
unlockKinfu();
Eigen::Affine3f transformation = m_reverse_initial_transformation;
if (req->request_subsample)
{
ROS_INFO("kinfu: Subsampling to voxel size %f",float(req->subsample_voxel_size));
pcl::BitmaskOctree<3> octree;
octree.SetOccupiedVoxelsCenters(*cloud,req->subsample_voxel_size);
cloud->clear();
octree.GetOccupiedVoxelsCenters(*cloud,req->subsample_voxel_size);
}
// if needed, transform
if (req->request_transformation)
{
ROS_INFO("kinfu: A custom transformation will be applied.");
Eigen::Affine3f tr = toEigenAffine(req->transformation_linear,req->transformation_translation);
transformation = tr * transformation;
}
ROS_INFO("kinfu: Applying transformation...");
pcl::transformPointCloud(*cloud,*cloud,transformation);
// if needed, crop
if (req->request_bounding_box)
{
ROS_INFO("kinfu: Cropping...");
PointCloud::Ptr out_cloud(new PointCloud());
TrianglesPtr empty(new Triangles());
cropMesh(req->bounding_box_min,req->bounding_box_max,cloud,empty,out_cloud,empty);
cloud = out_cloud;
}
ROS_INFO("kinfu: Publishing...");
pclPointCloudToMessage(cloud,resp->point_cloud);
m_pub.publish(resp);
ROS_INFO("kinfu: Extract Known Worker complete.");
}
int ExtractSIFT::compute()
{
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
PCLCloud::Ptr sm_cloud (new PCLCloud);
std::vector<std::string> req_fields;
req_fields.resize(2);
req_fields[0] = "xyz"; // always needed
switch (m_mode)
{
case RGB:
req_fields[1] = "rgb";
break;
case SCALAR_FIELD:
req_fields[1] = m_field_to_use;
break;
}
cc2smReader converter;
converter.setInputCloud(cloud);
converter.getAsSM(req_fields, *sm_cloud);
//Now change the name of the field to use to a standard name, only if in OTHER_FIELD mode
if (m_mode == SCALAR_FIELD)
{
int field_index = pcl::getFieldIndex(*sm_cloud, m_field_to_use_no_space);
sm_cloud->fields.at(field_index).name = "intensity"; //we always use intensity as name... even if it is curvature or another field.
}
//initialize all possible clouds
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_i (new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud (new pcl::PointCloud<pcl::PointXYZ>);
//Now do the actual computation
if (m_mode == SCALAR_FIELD)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_i);
estimateSIFT<pcl::PointXYZI, pcl::PointXYZ>(cloud_i, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
else if (m_mode == RGB)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_rgb);
estimateSIFT<pcl::PointXYZRGB, pcl::PointXYZ>(cloud_rgb, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
PCLCloud::Ptr out_cloud_sm (new PCLCloud);
TO_PCL_CLOUD(*out_cloud, *out_cloud_sm);
if ( (out_cloud_sm->height * out_cloud_sm->width) == 0)
{
//cloud is empty
return -53;
}
ccPointCloud* out_cloud_cc = sm2ccConverter(out_cloud_sm).getCCloud();
if (!out_cloud_cc)
{
//conversion failed (not enough memory?)
return -1;
}
std::stringstream name;
if (m_mode == RGB)
name << "SIFT Keypoints_" << m_nr_octaves << "_" << "rgb" << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
else
name << "SIFT Keypoints_" << m_nr_octaves << "_" << m_field_to_use_no_space << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
out_cloud_cc->setName(name.str().c_str());
out_cloud_cc->setDisplay(cloud->getDisplay());
if (cloud->getParent())
cloud->getParent()->addChild(out_cloud_cc);
emit newEntity(out_cloud_cc);
return 1;
}
int SavePCD::compute()
{
ccPointCloud * cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
//search for a sensor as child
size_t n_childs = cloud->getChildrenNumber();
ccSensor * sensor(0);
for (size_t i = 0; i < n_childs; ++i)
{
ccHObject * child = cloud->getChild(i);
//try to cast to a ccSensor
if (!child->isKindOf(CC_TYPES::SENSOR))
continue;
sensor = ccHObjectCaster::ToSensor(child);
}
PCLCloud::Ptr out_cloud (new PCLCloud);
cc2smReader* converter = new cc2smReader();
converter->setInputCloud(cloud);
int result = converter->getAsSM(*out_cloud);
delete converter;
converter=0;
Eigen::Vector4f pos;
Eigen::Quaternionf ori;
if(!sensor)
{
//we append to the cloud null sensor informations
pos = Eigen::Vector4f::Zero ();
ori = Eigen::Quaternionf::Identity ();
}
else
{
//we get out valid sensor informations
ccGLMatrix mat = sensor->getRigidTransformation();
CCVector3 trans = mat.getTranslationAsVec3D();
pos(0) = trans[0];
pos(1) = trans[1];
pos(2) = trans[2];
//also the rotation
Eigen::Matrix3f eigrot;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
eigrot(i,j) = mat.getColumn(j)[i];
// now translate to a quaternion notation
ori = Eigen::Quaternionf(eigrot);
}
if (result != 1)
{
return -31;
}
if (pcl::io::savePCDFile( m_filename.toStdString(), *out_cloud, pos, ori, true) < 0)
{
return -32;
}
return 1;
}
int ExtractSIFT::compute()
{
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
std::list<std::string> req_fields;
try
{
req_fields.push_back("xyz"); // always needed
switch (m_mode)
{
case RGB:
req_fields.push_back("rgb");
break;
case SCALAR_FIELD:
req_fields.push_back(qPrintable(m_field_to_use)); //DGM: warning, toStdString doesn't preserve "local" characters
break;
default:
assert(false);
break;
}
}
catch (const std::bad_alloc&)
{
//not enough memory
return -1;
}
PCLCloud::Ptr sm_cloud = cc2smReader(cloud).getAsSM(req_fields);
if (!sm_cloud)
return -1;
//Now change the name of the field to use to a standard name, only if in OTHER_FIELD mode
if (m_mode == SCALAR_FIELD)
{
int field_index = pcl::getFieldIndex(*sm_cloud, m_field_to_use_no_space);
sm_cloud->fields.at(field_index).name = "intensity"; //we always use intensity as name... even if it is curvature or another field.
}
//initialize all possible clouds
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_i (new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud (new pcl::PointCloud<pcl::PointXYZ>);
//Now do the actual computation
if (m_mode == SCALAR_FIELD)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_i);
estimateSIFT<pcl::PointXYZI, pcl::PointXYZ>(cloud_i, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
else if (m_mode == RGB)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_rgb);
estimateSIFT<pcl::PointXYZRGB, pcl::PointXYZ>(cloud_rgb, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
PCLCloud::Ptr out_cloud_sm (new PCLCloud);
TO_PCL_CLOUD(*out_cloud, *out_cloud_sm);
if ( out_cloud_sm->height * out_cloud_sm->width == 0)
{
//cloud is empty
return -53;
}
ccPointCloud* out_cloud_cc = sm2ccConverter(out_cloud_sm).getCloud();
if (!out_cloud_cc)
{
//conversion failed (not enough memory?)
return -1;
}
std::stringstream name;
if (m_mode == RGB)
name << "SIFT Keypoints_" << m_nr_octaves << "_" << "rgb" << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
else
name << "SIFT Keypoints_" << m_nr_octaves << "_" << m_field_to_use_no_space << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
out_cloud_cc->setName(name.str().c_str());
out_cloud_cc->setDisplay(cloud->getDisplay());
//copy global shift & scale
out_cloud_cc->setGlobalScale(cloud->getGlobalScale());
out_cloud_cc->setGlobalShift(cloud->getGlobalShift());
if (cloud->getParent())
cloud->getParent()->addChild(out_cloud_cc);
emit newEntity(out_cloud_cc);
return 1;
}
int main(int argc, char** argv)
{
Options opts;
if (options(argc, argv, opts))
return 1;
// Load stuff
Eigen::MatrixXf ldr_data;
readLDRFile(opts.src_ldr_file, ldr_data);
std::cout << "rows: " << ldr_data.rows() << " cols: " << ldr_data.cols() << std::endl;
std::cout << ldr_data.block<10, 6>(0, 0) << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>());
ldr_to_cloud(ldr_data, *src_cloud);
std::cout << "Cloud size: " << src_cloud->size() << std::endl;
/*
PCL_INFO("Loading src_pcd: %s\n", opts.src_pcd_file.c_str());
pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>());
if (pcl::io::loadPCDFile(opts.src_pcd_file, *src_cloud) < 0)
{
std::cout << "Error loading input point cloud " << opts.src_pcd_file << std::endl;
return -1;
}
*/
// Do upsampling
pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls_upsampling;
// Set up KD-tree
// TODO Weight intensity
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
// Set parameters
// FIXME PARAMS
mls_upsampling.setInputCloud(src_cloud);
mls_upsampling.setComputeNormals (true);
mls_upsampling.setPolynomialFit (true);
mls_upsampling.setSearchMethod (tree);
mls_upsampling.setSearchRadius (0.25);
//mls_upsampling.setUpsamplingMethod (pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::VOXEL_GRID_DILATION);
//mls_upsampling.setUpsamplingMethod (pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::SAMPLE_LOCAL_PLANE);
mls_upsampling.setUpsamplingMethod (pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::NONE);
mls_upsampling.setUpsamplingRadius (0.25);
mls_upsampling.setUpsamplingStepSize (0.125);
//mls_upsampling.setDilationVoxelSize(0.25);
//mls_upsampling.setDilationIterations(1);
mls_upsampling.setPointDensity(1);
NormalCloud::Ptr out_cloud(new NormalCloud());
std::cout << "Running upsampling" << std::endl;
mls_upsampling.process(*out_cloud);
std::cout << "Finished upsampling" << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>());
pcl::copyPointCloud(*out_cloud, *out_cloud_xyz);
// TODO For each point in upsampled cloud, assign it the
// time, laser num, intensity of the closet point in
// the original cloud
// TODO Will have to read in original ldr file to do this
//pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2;
Eigen::MatrixXf out_matrix(out_cloud_xyz->size(), 6);
for (int k = 0; k < out_cloud_xyz->size(); k++)
{
pcl::PointXYZ pt = out_cloud_xyz->at(k);
// FIXME Fill in with intensity, laser
out_matrix.row(k) << (float)pt.x, (float)pt.y, (float)pt.z, 0.0f, 0.0f, 0.0f;
}
// Save upsampled cloud
if (opts.out_pcd_file != "")
pcl::io::savePCDFileBinary(opts.out_pcd_file, *out_cloud);
// Save ldr file
if (opts.out_ldr_file != "")
writeLDRFile(opts.out_ldr_file, out_matrix);
PCL_INFO("Upsampled from %d to %d points\n", src_cloud->size(), out_cloud->size());
return 0;
}
int
main(int argc, char **argv)
{
if (argc < 3)
{
std::cerr << "Needs two pcd input files." << std::endl;
return (-1);
}
pcl::PointCloud<pcl::RGB>::Ptr left_cloud (new pcl::PointCloud<pcl::RGB>);
pcl::PointCloud<pcl::RGB>::Ptr right_cloud (new pcl::PointCloud<pcl::RGB>);
//Read pcd files
pcl::PCDReader pcd;
if (pcd.read (argv[1], *left_cloud) == -1)
return (-1);
if (pcd.read (argv[2], *right_cloud) == -1)
return (-1);
if ( !left_cloud->isOrganized() || !right_cloud->isOrganized() || left_cloud->width != right_cloud -> width || left_cloud->height != left_cloud->height)
{
std::cout << "Wrong stereo pair; please check input pcds .." << std::endl;
return 0;
}
pcl::AdaptiveCostSOStereoMatching stereo;
stereo.setMaxDisparity(60);
stereo.setXOffset(0);
stereo.setRadius(5);
stereo.setSmoothWeak(20);
stereo.setSmoothStrong(100);
stereo.setGammaC(25);
stereo.setGammaS(10);
stereo.setRatioFilter(20);
stereo.setPeakFilter(0);
stereo.setLeftRightCheck(true);
stereo.setLeftRightCheckThreshold(1);
stereo.setPreProcessing(true);
stereo.compute(*left_cloud, *right_cloud);
stereo.medianFilter(4);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud( new pcl::PointCloud<pcl::PointXYZRGB> );
stereo.getPointCloud(318.112200, 224.334900, 368.534700, 0.8387445, out_cloud, left_cloud);
pcl::PointCloud<pcl::RGB>::Ptr vmap( new pcl::PointCloud<pcl::RGB> );
stereo.getVisualMap(vmap);
pcl::visualization::ImageViewer iv ("My viewer");
iv.addRGBImage<pcl::RGB> (vmap);
//iv.addRGBImage<pcl::RGB> (left_cloud);
//iv.spin (); // press 'q' to exit
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
//viewer->addPointCloud<pcl::PointXYZRGB> (out_cloud, "stereo");
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> intensity(out_cloud);
viewer->addPointCloud<pcl::PointXYZRGB> (out_cloud, intensity, "stereo");
//viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "stereo");
viewer->initCameraParameters ();
//viewer->spin();
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
iv.spinOnce (100); // press 'q' to exit
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
