void QNode::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg){
// Convert ROS message (PointCloud2) to PCL point cloud (PointCloud(PointXYZ))
pcl::fromROSMsg(*cloud_msg, cloud);
if(rgb_enabled){
pcl::fromROSMsg(*cloud_msg, cloudRGB);
}
// Cloud conversion and visualization
pcl::PointCloud<pcl::PointXYZ>::Ptr tmpCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *tmpCloud);
picture_flag = true;
Q_EMIT setPointCloud(tmpCloud);
}
void CLASS_GEN(Algorithm)::executeAlgorithm(CContextDataStore *store){
// get input files
auto pInputImages = store->getData<CInputDataSet>();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pPointCloud = loadPointCoudFromPcd(mSettings->value("PcSrcFile").toString().toStdString());
pcl::PolygonMesh::Ptr pMesh = loadMeshFromPly(mSettings->value("MeshSrcFile").toString().toStdString());
pcl::PolygonMesh::Ptr pTexMesh = loadMeshFromPly(mSettings->value("TexMeshSrcFile").toString().toStdString());
auto fusionData = new CDataFusion;
fusionData->setPointCloud(loadPointCoudFromPcd(mSettings->value("PcSrcFile").toString().toStdString()));
fusionData->setPolygonMesh(loadMeshFromPly(mSettings->value("MeshSrcFile").toString().toStdString()));
// fusionData->setTextureMesh(loadMeshFromPly(mSettings->value("TexMeshSrcFile").toString().toStdString()));
store->appendData<CDataFusion>(std::shared_ptr<CDataFusion>(fusionData), true);
}
//Retrieve the transform between the lens and the base-link at capturing time
void BagLoader::sendWithTransformation(pointcloud_type* cloud)
{
ScopedTimer s(__FUNCTION__);
ParameterServer* ps = ParameterServer::instance();
std::string fixed_frame = ps->get<std::string>("fixed_frame_name");
std::string depth_frame_id = cloud->header.frame_id;
ros::Time depth_time = cloud->header.stamp;
tf::StampedTransform map2points;
try{
tflistener_->waitForTransform(fixed_frame, depth_frame_id, depth_time, ros::Duration(0.005));
tflistener_->lookupTransform( fixed_frame, depth_frame_id, depth_time,map2points);
//printTransform("Current Transform", map2points);
QMatrix4x4 transform_to_map = g2o2QMatrix(tf2G2O(map2points));
Q_EMIT setPointCloud(cloud, transform_to_map);
}
catch (tf::TransformException ex){
ROS_ERROR("%s - No transformation available",ex.what());
}
}
void TerrainClassifierNode::loadTestPointCloud(const std::string& path)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
ROS_INFO("Loading point cloud from %s...", path.c_str());
if (pcl::io::loadPCDFile(path, *point_cloud))
{
ROS_ERROR("FAILED!");
return;
}
else
ROS_INFO("Done!");;
sensor_msgs::PointCloud2 point_cloud_msg;
pcl::toROSMsg(*point_cloud, point_cloud_msg);
point_cloud_msg.header.frame_id = terrain_classifier.getFrameId();
point_cloud_msg.header.stamp = ros::Time::now();
ROS_INFO("Generating terrain model...");
setPointCloud(point_cloud_msg);
ROS_INFO("Done!");
}
void MultiClientWindow::on_pushButton_Clean_clicked()
{
pcl::RadiusOutlierRemoval<PointT> outrem;
// build the filter
outrem.setRadiusSearch(0.1);
outrem.setMinNeighborsInRadius(5);
outrem.setNegative(false);
for (int i = 1; i < 5; i++)
{
PointCloudT::Ptr PC_input(new PointCloudT);
PC_input = getPointCloud(i);
PointCloudT::Ptr PC_output(new PointCloudT);
pcl::copyPointCloud(*PC_input, *PC_output);
outrem.setInputCloud(PC_input);
outrem.setKeepOrganized(PC_input->isOrganized());
// apply filter
outrem.filter (*PC_output);
// update PC
setPointCloud(i, PC_output);
ui->widget->replacePC(PC_output);
}
}
