void
PCLOpenCVConverter<PointT>::computeOrganizedCloud()
{
CHECK( !cloud_->isOrganized() && cam_ && cam_->getWidth()>0 && cam_->getHeight()>0 );
ZBufferingParameter zBufParams;
zBufParams.do_smoothing_ = true;
zBufParams.smoothing_radius_ = 2;
ZBuffering<PointT> zbuf (cam_, zBufParams);
typename pcl::PointCloud<PointT>::Ptr organized_cloud (new pcl::PointCloud<PointT>);
zbuf.renderPointCloud( *cloud_, *organized_cloud);
for(PointT &p:organized_cloud->points)
{
if (!pcl::isFinite(p))
{
p.r = background_color_(0);
p.g = background_color_(1);
p.b = background_color_(2);
}
}
index_map_ = zbuf.getIndexMap();
if(!indices_.empty())
{
size_t width = cam_->getWidth();
size_t height = cam_->getHeight();
boost::dynamic_bitset<> fg_mask = createMaskFromIndices(indices_, cloud_->points.size());
std::vector<int> new_indices(indices_.size());
size_t kept=0;
for(size_t u=0; u<width; u++)
{
for(size_t v=0; v<height; v++)
{
int orig_idx = index_map_(v,u);
if(orig_idx >= 0 && fg_mask[orig_idx] )
{
new_indices[kept++] = v*width + u;
}
}
}
new_indices.resize(kept);
indices_.swap(new_indices);
}
cloud_ = organized_cloud;
}
void
SIFTLocalEstimation<PointT>::compute (std::vector<std::vector<float> > &signatures)
{
CHECK( cloud_ && cloud_->isOrganized() );
cv::Mat colorImage = ConvertPCLCloud2Image(*cloud_);
Eigen::Matrix2Xf keypoints2d;
compute(colorImage, keypoints2d, signatures);
keypoint_indices_.resize(keypoints2d.cols());
std::vector<bool> obj_mask;
if(indices_.empty())
obj_mask.resize(cloud_->width * cloud_->height, true);
else
obj_mask = createMaskFromIndices(indices_, cloud_->width * cloud_->height);
size_t kept = 0;
for(int i=0; i < keypoints2d.cols(); i++)
{
int u = std::max<int>(0, std::min<int>((int)cloud_->width -1, keypoints2d(0,i) ) );
int v = std::max<int>(0, std::min<int>((int)cloud_->height -1, keypoints2d(1,i) ) );
int idx = v * cloud_->width + u;
if(!obj_mask[idx]) // keypoint does not belong to given object mask
continue;
if( pcl::isFinite(cloud_->points[idx]) && cloud_->points[idx].z < max_distance_)
{
signatures[kept] = signatures[i];
keypoint_indices_[kept] = idx;
kept++;
}
}
signatures.resize(kept);
keypoint_indices_.resize(kept);
indices_.clear();
keypoints_.reset( new pcl::PointCloud<PointT>);
pcl::copyPointCloud(*cloud_, keypoint_indices_, *keypoints_);
processed_ = cloud_;
}
cv::Mat
PCLOpenCVConverter<PointT>::fillMatrix( std::vector<MatType> (PCLOpenCVConverter<PointT>::*pf)(int v, int u) )
{
CHECK( !cloud_->points.empty() );
if(!cloud_->isOrganized())
computeOrganizedCloud();
boost::dynamic_bitset<> fg_mask;
if ( !indices_.empty() )
fg_mask = createMaskFromIndices(indices_, cloud_->points.size());
else
{
fg_mask = boost::dynamic_bitset<> (cloud_->points.size());
fg_mask.set();
}
MatType* p = (MatType*)output_matrix_.data;
int nChannels = output_matrix_.channels();
for (size_t v = 0; v < cloud_->height; v++)
{
for (size_t u = 0; u < cloud_->width; u++)
{
if( !remove_background_ || fg_mask[v*cloud_->width + u] )
{
std::vector<MatType> val = (this->*pf)(v,u);
for(size_t c=0; c<val.size(); c++)
*p++ = val[c];
}
else
p += nChannels;
}
}
indices_.empty() ? roi_ = cv::Rect(cv::Point(0,0), cv::Point(cloud_->width, cloud_->height) ) : roi_ = computeROIfromIndices();
return output_matrix_;
}
bool
LocalRecognitionPipeline<PointT>::initialize (bool force_retrain)
{
if(!estimator_)
throw std::runtime_error("Keypoint extractor with feature estimator is not set!");
descr_name_ = estimator_->getFeatureDescriptorName();
std::vector<ModelTPtr> models = source_->getModels();
std::cout << "Models size:" << models.size () << std::endl;
if (force_retrain)
{
for (size_t i = 0; i < models.size (); i++)
source_->removeDescDirectory (*models[i], models_dir_, descr_name_);
}
for (ModelTPtr &m : models)
{
const std::string dir = models_dir_ + "/" + m->class_ + "/" + m->id_ + "/" + descr_name_;
if (!io::existsFolder(dir))
{
std::cout << "Model " << m->class_ << " " << m->id_ << " not trained. Training " << estimator_->getFeatureDescriptorName() << " on " << m->view_filenames_.size () << " views..." << std::endl;
if(!source_->getLoadIntoMemory())
source_->loadInMemorySpecificModel(*m);
for (size_t v = 0; v < m->view_filenames_.size(); v++)
{
std::vector<std::vector<float> > all_signatures;
std::vector<std::vector<float> > object_signatures;
typename pcl::PointCloud<PointT> all_keypoints;
typename pcl::PointCloud<PointT> object_keypoints;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
computeNormals<PointT>(m->views_[v], normals, param_.normal_computation_method_);
std::vector<int> all_kp_indices, obj_kp_indices;
estimator_->setNormals(normals);
typename pcl::PointCloud<PointT> foo;
bool success = estimator_->estimate (*m->views_[v], foo, all_keypoints, all_signatures);
(void) success;
estimator_->getKeypointIndices(all_kp_indices);
// remove signatures and keypoints which do not belong to object
std::vector<bool> obj_mask = createMaskFromIndices(m->indices_[v].indices, m->views_[v]->points.size());
obj_kp_indices.resize( all_kp_indices.size() );
object_signatures.resize( all_kp_indices.size() ) ;
size_t kept=0;
for (size_t kp_id = 0; kp_id < all_kp_indices.size(); kp_id++)
{
int idx = all_kp_indices[kp_id];
if ( obj_mask[idx] )
{
obj_kp_indices[kept] = idx;
object_signatures[kept] = all_signatures[kp_id];
kept++;
}
}
object_signatures.resize( kept );
obj_kp_indices.resize( kept );
pcl::copyPointCloud( *m->views_[v], obj_kp_indices, object_keypoints);
if (object_keypoints.points.size()) //save descriptors and keypoints to disk
{
io::createDirIfNotExist(dir);
std::string descriptor_basename (m->view_filenames_[v]);
boost::replace_last(descriptor_basename, source_->getViewPrefix(), "/descriptors_");
boost::replace_last(descriptor_basename, ".pcd", ".desc");
std::ofstream f(dir + descriptor_basename, std::ofstream::binary );
int rows = object_signatures.size();
int cols = object_signatures[0].size();
f.write((const char*)(&rows), sizeof(rows));
f.write((const char*)(&cols), sizeof(cols));
for(size_t sig_id=0; sig_id<object_signatures.size(); sig_id++)
f.write(reinterpret_cast<const char*>(&object_signatures[sig_id][0]), sizeof(object_signatures[sig_id][0]) * object_signatures[sig_id].size());
f.close();
std::string keypoint_basename (m->view_filenames_[v]);
boost::replace_last(keypoint_basename, source_->getViewPrefix(), "/keypoints_");
pcl::io::savePCDFileBinary (dir + keypoint_basename, object_keypoints);
std::string kp_normals_basename (m->view_filenames_[v]);
boost::replace_last(kp_normals_basename, source_->getViewPrefix(), "/keypoint_normals_");
pcl::PointCloud<pcl::Normal>::Ptr normals_keypoints(new pcl::PointCloud<pcl::Normal>);
pcl::copyPointCloud(*normals, obj_kp_indices, *normals_keypoints);
pcl::io::savePCDFileBinary (dir + kp_normals_basename, *normals_keypoints);
}
}
if(!source_->getLoadIntoMemory())
m->views_.clear();
}
else
{
std::cout << "Model " << m->class_ << " " << m->id_ << " already trained using " << estimator_->getFeatureDescriptorName() << "." << std::endl;
m->views_.clear(); //there is no need to keep the views in memory once the model has been trained
}
}
loadFeaturesAndCreateFLANN ();
if(param_.icp_iterations_ > 0 && param_.icp_type_ == 1)
source_->createVoxelGridAndDistanceTransform(param_.voxel_size_icp_);
return true;
}
