PcdGtAnnotator()
{
f_ = 525.f;
source_.reset(new v4r::ModelOnlySource<pcl::PointXYZRGBNormal, pcl::PointXYZRGB>);
models_dir_ = "";
gt_dir_ = "";
threshold_ = 0.01f;
first_view_only_ = false;
reconstructed_scene_.reset(new pcl::PointCloud<PointT>);
}
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const
{
normals.reset (new pcl::PointCloud<Normal>);
normals->clear ();
normals->resize (leaves_.size ());
typename SupervoxelHelper::const_iterator leaf_itr;
typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin ();
for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr)
{
const VoxelData& leaf_data = (*leaf_itr)->getData ();
leaf_data.getNormal (*normal_itr);
}
}
void
NMBasedCloudIntegration<PointT>::compute (typename pcl::PointCloud<PointT>::Ptr & output)
{
if(input_clouds_.empty())
{
LOG(ERROR) << "No input clouds set for cloud integration!";
return;
}
big_cloud_info_.clear();
collectInfo();
if(param_.reason_about_points_)
reasonAboutPts();
pcl::octree::OctreePointCloudPointVector<PointT> octree( param_.octree_resolution_ );
typename pcl::PointCloud<PointT>::Ptr big_cloud ( new pcl::PointCloud<PointT>());
big_cloud->width = big_cloud_info_.size();
big_cloud->height = 1;
big_cloud->points.resize( big_cloud_info_.size() );
for(size_t i=0; i < big_cloud_info_.size(); i++)
big_cloud->points[i] = big_cloud_info_[i].pt;
octree.setInputCloud( big_cloud );
octree.addPointsFromInputCloud();
typename pcl::octree::OctreePointCloudPointVector<PointT>::LeafNodeIterator leaf_it;
const typename pcl::octree::OctreePointCloudPointVector<PointT>::LeafNodeIterator it2_end = octree.leaf_end();
size_t kept = 0;
size_t total_used = 0;
std::vector<PointInfo> filtered_cloud_info ( big_cloud_info_.size() );
for (leaf_it = octree.leaf_begin(); leaf_it != it2_end; ++leaf_it)
{
pcl::octree::OctreeContainerPointIndices& container = leaf_it.getLeafContainer();
// add points from leaf node to indexVector
std::vector<int> indexVector;
container.getPointIndices (indexVector);
if(indexVector.empty())
continue;
std::vector<PointInfo> voxel_pts ( indexVector.size() );
for(size_t k=0; k < indexVector.size(); k++)
voxel_pts[k] = big_cloud_info_ [indexVector[k]];
PointInfo p;
size_t num_good_pts = 0;
if(param_.average_)
{
for(const PointInfo &pt_tmp : voxel_pts)
{
if (pt_tmp.distance_to_depth_discontinuity > param_.min_px_distance_to_depth_discontinuity_)
{
p.moving_average( pt_tmp );
num_good_pts++;
}
}
if( num_good_pts < param_.min_points_per_voxel_ )
continue;
total_used += num_good_pts;
}
else // take only point with min weight
{
for(const PointInfo &pt_tmp : voxel_pts)
{
if ( pt_tmp.distance_to_depth_discontinuity > param_.min_px_distance_to_depth_discontinuity_)
{
num_good_pts++;
if ( pt_tmp.weight < p.weight || num_good_pts == 1)
p = pt_tmp;
}
}
if( num_good_pts < param_.min_points_per_voxel_ )
continue;
total_used++;
}
filtered_cloud_info[kept++] = p;
}
LOG(INFO) << "Number of points in final noise model based integrated cloud: " << kept << " used: " << total_used << std::endl;
if(!output)
output.reset(new pcl::PointCloud<PointT>);
if(!output_normals_)
output_normals_.reset( new pcl::PointCloud<pcl::Normal>);
filtered_cloud_info.resize(kept);
output->points.resize(kept);
output_normals_->points.resize(kept);
output->width = output_normals_->width = kept;
output->height = output_normals_->height = 1;
output->is_dense = output_normals_->is_dense = true;
PointT na;
na.x = na.y = na.z = std::numeric_limits<float>::quiet_NaN();
input_clouds_used_.resize( input_clouds_.size() );
for(size_t i=0; i<input_clouds_used_.size(); i++) {
input_clouds_used_[i].reset( new pcl::PointCloud<PointT> );
input_clouds_used_[i]->points.resize( input_clouds_[i]->points.size(), na);
input_clouds_used_[i]->width = input_clouds_[i]->width;
input_clouds_used_[i]->height = input_clouds_[i]->height;
}
for(size_t i=0; i<filtered_cloud_info.size(); i++)
{
output->points[i] = filtered_cloud_info[i].pt;
output_normals_->points[i] = filtered_cloud_info[i].normal;
int origin = filtered_cloud_info[i].origin;
input_clouds_used_[origin]->points[filtered_cloud_info[i].pt_idx] = filtered_cloud_info[i].pt;
}
cleanUp();
}
View()
{
cloud_.reset(new pcl::PointCloud<PointT>());
}
static bool colorize(const drc::map_image_t& iDepthMap,
const bot_core::image_t& iImage,
typename pcl::PointCloud<T>::Ptr& oCloud) {
int w(iDepthMap.width), h(iDepthMap.height);
if ((w != iImage.width) || (h != iImage.height)) {
return false;
}
// TODO: for now, reject compressed depth maps
if (iDepthMap.compression != drc::map_image_t::COMPRESSION_NONE) {
return false;
}
// TODO: for now, only accept rgb3 images
if (iImage.pixelformat != PIXEL_FORMAT_RGB) {
return false;
}
if (oCloud == NULL) {
oCloud.reset(new pcl::PointCloud<T>());
}
oCloud->points.clear();
Eigen::Matrix4d xform;
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
xform(i,j) = iDepthMap.transform[i][j];
}
}
xform = xform.inverse();
for (int i = 0; i < h; ++i) {
for (int j = 0; j < w; ++j) {
double z;
if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_FLOAT32) {
z = ((float*)(&iDepthMap.data[0] + i*iDepthMap.row_bytes))[j];
if (z < -1e10) {
continue;
}
}
else if (iDepthMap.format == drc::map_image_t::FORMAT_GRAY_UINT8) {
uint8_t val = iDepthMap.data[i*iDepthMap.row_bytes + j];
if (val == 0) {
continue;
}
z = val;
}
else {
continue;
}
Eigen::Vector4d pt = xform*Eigen::Vector4d(j,i,z,1);
T newPoint;
newPoint.x = pt(0)/pt(3);
newPoint.y = pt(1)/pt(3);
newPoint.z = pt(2)/pt(3);
int imageIndex = i*iImage.row_stride + 3*j;
newPoint.r = iImage.data[imageIndex+0];
newPoint.g = iImage.data[imageIndex+1];
newPoint.b = iImage.data[imageIndex+2];
oCloud->points.push_back(newPoint);
}
}
oCloud->width = oCloud->points.size();
oCloud->height = 1;
oCloud->is_dense = false;
return true;
}
