void
MultiviewRecognizerWithChangeDetection<PointT>::reconstructionFiltering(typename pcl::PointCloud<PointT>::Ptr observation,
pcl::PointCloud<pcl::Normal>::Ptr observation_normals, const Eigen::Matrix4f &absolute_pose, size_t view_id) {
CloudPtr observation_transformed(new Cloud);
pcl::transformPointCloud(*observation, *observation_transformed, absolute_pose);
CloudPtr cloud_tmp(new Cloud);
std::vector<int> indices;
v4r::ChangeDetector<PointT>::difference(observation_transformed, removed_points_cumulated_history_[view_id],
cloud_tmp, indices, param_.tolerance_for_cloud_diff_);
/* Visualization of changes removal for reconstruction:
Cloud rec_changes;
rec_changes += *cloud_transformed;
v4r::VisualResultsStorage::copyCloudColored(*removed_points_cumulated_history_[view_id], rec_changes, 255, 0, 0);
v4r::VisualResultsStorage::copyCloudColored(*cloud_tmp, rec_changes, 200, 0, 200);
stringstream ss;
ss << view_id;
visResStore.savePcd("reconstruction_changes_" + ss.str(), rec_changes);*/
std::vector<bool> preserved_mask(observation->size(), false);
for (std::vector<int>::iterator i = indices.begin(); i < indices.end(); i++) {
preserved_mask[*i] = true;
}
for (size_t j = 0; j < preserved_mask.size(); j++) {
if (!preserved_mask[j]) {
setNan(observation->at(j));
setNan(observation_normals->at(j));
}
}
PCL_INFO("Points by change detection removed: %d\n", observation->size() - indices.size());
}
pcl::IndicesPtr radiusFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float radiusSearch,
int minNeighborsInRadius)
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
KdTreePtr tree (new KdTree(false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud, indices);
for(unsigned int i=0; i<indices->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
}
output->resize(oi);
return output;
}
else
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
tree->setInputCloud(cloud);
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
if(k > minNeighborsInRadius)
{
output->at(oi++) = i;
}
}
output->resize(oi);
return output;
}
}
void projectCloudOnXYPlane(
typename pcl::PointCloud<PointT>::Ptr & cloud)
{
for(unsigned int i=0; i<cloud->size(); ++i)
{
cloud->at(i).z = 0;
}
}
void OctreeVoxelGrid::generateVoxelCloudImpl(const sensor_msgs::PointCloud2ConstPtr& input_msg)
{
typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
typename pcl::PointCloud<PointT>::Ptr cloud_voxeled (new pcl::PointCloud<PointT> ());
pcl::fromROSMsg(*input_msg, *cloud);
// generate octree
typename pcl::octree::OctreePointCloud<PointT> octree(resolution_);
// add point cloud to octree
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
// get points where grid is occupied
typename pcl::octree::OctreePointCloud<PointT>::AlignedPointTVector point_vec;
octree.getOccupiedVoxelCenters(point_vec);
// put points into point cloud
cloud_voxeled->width = point_vec.size();
cloud_voxeled->height = 1;
for (int i = 0; i < point_vec.size(); i++) {
cloud_voxeled->push_back(point_vec[i]);
}
// publish point cloud
sensor_msgs::PointCloud2 output_msg;
toROSMsg(*cloud_voxeled, output_msg);
output_msg.header = input_msg->header;
pub_cloud_.publish(output_msg);
// publish marker
if (publish_marker_flag_) {
visualization_msgs::Marker marker_msg;
marker_msg.type = visualization_msgs::Marker::CUBE_LIST;
marker_msg.scale.x = resolution_;
marker_msg.scale.y = resolution_;
marker_msg.scale.z = resolution_;
marker_msg.header = input_msg->header;
marker_msg.pose.orientation.w = 1.0;
if (marker_color_ == "flat") {
marker_msg.color = jsk_topic_tools::colorCategory20(0);
marker_msg.color.a = marker_color_alpha_;
}
// compute min and max
Eigen::Vector4f minpt, maxpt;
pcl::getMinMax3D<PointT>(*cloud_voxeled, minpt, maxpt);
PointT p;
for (size_t i = 0; i < cloud_voxeled->size(); i++) {
p = cloud_voxeled->at(i);
geometry_msgs::Point point_ros;
point_ros.x = p.x;
point_ros.y = p.y;
point_ros.z = p.z;
marker_msg.points.push_back(point_ros);
if (marker_color_ == "flat") {
marker_msg.colors.push_back(jsk_topic_tools::colorCategory20(0));
}
else if (marker_color_ == "z") {
marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.z - minpt[2]) / (maxpt[2] - minpt[2])));
}
else if (marker_color_ == "x") {
marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.x - minpt[0]) / (maxpt[0] - minpt[0])));
}
else if (marker_color_ == "y") {
marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.y - minpt[1]) / (maxpt[1] - minpt[1])));
}
marker_msg.colors[marker_msg.colors.size() - 1].a = marker_color_alpha_;
}
pub_marker_.publish(marker_msg);
}
}