void transpose_test()
{
int w = 5, h = 7;
std::vector<ScalarType> s_normal(2 * w * h);
viennacl::matrix<ScalarType> normal(w, 2 * h);
viennacl::matrix<ScalarType> transp(h, 2 * w);
for (unsigned int i = 0; i < s_normal.size(); i+=2) {
s_normal[i] = i;
s_normal[i+1] = i;
}
viennacl::fast_copy(&s_normal[0], &s_normal[0] + s_normal.size(), normal);
std::cout << normal << std::endl;
viennacl::linalg::transpose(normal);
std::cout << normal << std::endl;
}
void ConvexConnectedVoxels::segmentCloud(
const pcl::PointCloud<PointT>::Ptr cloud,
const std::vector<pcl::PointIndices> &indices,
std::vector<pcl::PointCloud<PointT>::Ptr> &cloud_clusters,
std::vector<pcl::PointCloud<pcl::Normal>::Ptr> &normal_clusters,
pcl::PointCloud<pcl::PointXYZ>::Ptr centroids)
{
boost::mutex::scoped_lock lock(this->mutex_);
pcl::ExtractIndices<PointT>::Ptr eifilter(
new pcl::ExtractIndices<PointT>);
eifilter->setInputCloud(cloud);
for (int i = 0; i < indices.size(); i++) {
pcl::PointIndices::Ptr index(new pcl::PointIndices());
index->indices = indices[i].indices;
eifilter->setIndices(index);
pcl::PointCloud<PointT>::Ptr tmp_cloud(
new pcl::PointCloud<PointT>);
eifilter->filter(*tmp_cloud);
if (tmp_cloud->width > 0) {
Eigen::Vector4f centroid;
pcl::compute3DCentroid<PointT, float>(*cloud, *index, centroid);
float ct_x = static_cast<float>(centroid[0]);
float ct_y = static_cast<float>(centroid[1]);
float ct_z = static_cast<float>(centroid[2]);
if (!isnan(ct_x) && !isnan(ct_y) && !isnan(ct_z)) {
pcl::PointCloud<pcl::Normal>::Ptr s_normal(
new pcl::PointCloud<pcl::Normal>);
this->estimatePointCloudNormals(
tmp_cloud, s_normal, 40, 0.05, false);
normal_clusters.push_back(s_normal);
centroids->push_back(pcl::PointXYZ(ct_x, ct_y, ct_z));
cloud_clusters.push_back(tmp_cloud);
}
}
}
}
