void main()
{
try {
// ビューアー
pcl::visualization::PCLVisualizer viewer( "Point Cloud Viewer" );
// 点群
pcl::PointCloud<PointType>::Ptr cloud( new pcl::PointCloud<PointType> );
// 点群の排他処理
boost::mutex mutex;
// データの更新ごとに呼ばれる関数(オブジェクト)
boost::function<void(const pcl::PointCloud<PointType>::ConstPtr&)> function =
[&cloud, &mutex]( const pcl::PointCloud<PointType>::ConstPtr &new_cloud ){
boost::mutex::scoped_lock lock( mutex );
pcl::copyPointCloud( *new_cloud, *cloud );
};
// Kinect2Grabberを開始する
pcl::Kinect2Grabber grabber;
grabber.registerCallback( function );
grabber.start();
// ビューアーが終了されるまで動作する
while ( !viewer.wasStopped() ){
// 表示を更新する
viewer.spinOnce();
// 点群がある場合
boost::mutex::scoped_try_lock lock( mutex );
if ( (cloud->size() != 0) && lock.owns_lock() ){
// 点群を間引く
voxelGridFilter( cloud );
// 平面を検出
auto inliers = segmentation( cloud );
// 平面を抽出
extractIndices( cloud, inliers );
// 点群を更新する
auto ret = viewer.updatePointCloud( cloud, "cloud" );
if ( !ret ){
// 更新がエラーになった場合は、
// 未作成なので新しい点群として追加する
viewer.addPointCloud( cloud, "cloud" );
}
}
// エスケープキーが押されたら終了する
if ( GetKeyState( VK_ESCAPE ) < 0 ){
break;
}
}
}
catch ( std::exception& ex ){
std::cout << ex.what() << std::endl;
}
}
void findTransformFromOverlapICP(const PCRGB::Ptr& target_pc, const PCRGB::Ptr& source_pc,
Eigen::Affine3d& tf_mat)
{
int icp_iters, use_rgb;
double color_weight, icp_radius;
ros::param::param<int>("~icp_iters", icp_iters, 10);
ros::param::param<double>("~color_weight", color_weight, 0.0005);
ros::param::param<double>("~icp_radius", icp_radius, 0.05);
ros::param::param<int>("~use_rgb", use_rgb, 1);
ROS_INFO("PS %d %f %f", icp_iters, color_weight, icp_radius);
pcl::IndicesPtr target_inds_list(new vector<int>()), source_inds_list(new vector<int>());
findOverlaps(target_pc, source_pc, target_inds_list, source_inds_list, icp_radius, use_rgb, color_weight);
ROS_INFO("INDS %d %d", target_inds_list->size(), source_inds_list->size());
PCRGB::Ptr target_pc_ol(new PCRGB());
PCRGB::Ptr source_pc_ol(new PCRGB());
extractIndices(target_pc, target_inds_list, target_pc_ol);
extractIndices(source_pc, source_inds_list, source_pc_ol);
ROS_INFO("PC COUNT %d %d", target_pc_ol->points.size(), source_pc_ol->points.size());
computeICPRegistration(target_pc_ol, source_pc_ol, tf_mat, icp_iters, color_weight);
}
bool removeMainPlane(const typename pcl::PointCloud<PointT>::Ptr& inputCloud,
typename pcl::PointCloud<PointT>::Ptr& cloudMainPlaneRemoved)
{
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients( new pcl::ModelCoefficients );
if ( locateMainPlane(inputCloud, inliers, coefficients) )
{
extractIndices(inputCloud,
inliers,
true, //points not in the plane will survive
cloudMainPlaneRemoved);
return true;
}
else
return false;
}
void RequestParameters::parseList(const QList<QPair<QString, QVariant >> & parameters)
{
for (const QPair<QString, QVariant> & pair : parameters)
{
if (pair.first.isEmpty())
{
continue;
}
QList<QString> indices = extractIndices(pair.first);
QVariantTree * currentParams = m_params.data();
QString index;
for (int i = 0;i < indices.size() - 1;++i)
{
index = indices[i].isEmpty() ? QString::number(currentParams->size()) : indices[i];
currentParams = ¤tParams->obtainSubtree(index);
}
index = indices.last().isEmpty() ? QString::number(currentParams->size()) : indices.last();
currentParams->insert(index, pair.second);
}
}
void BccLattice::extractTetrahedronMeshData(Vector3F * points, unsigned * indices)
{
extractPoints(points);
extractIndices(indices);
}
