void InnerModelManager::setPointCloudData(const std::string id, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud)
{
QString m = QString("setPointCloudData");
/// Aqui Marco va a mejorar el código :-) felicidad (comprobar que la nube existe)
IMVPointCloud *pcNode = imv->pointCloudsHash[QString::fromStdString(id)];
int points = cloud->size();
pcNode->points->resize(points);
pcNode->colors->resize(points);
pcNode->setPointSize(3);
int i = 0;
for(pcl::PointCloud<pcl::PointXYZRGBA>::iterator it = cloud->begin(); it != cloud->end(); it++ )
{
if (!pcl_isnan(it->x)&&!pcl_isnan(it->y)&&!pcl_isnan(it->z))
{
std::cout<<"Adding: "<<it->x<<" "<<it->y<<" "<<it->z<<endl;
pcNode->points->operator[](i) = QVecToOSGVec(QVec::vec3(it->x, it->y, it->z));
pcNode->colors->operator[](i) = osg::Vec4f(float(it->r)/255, float(it->g)/255, float(it->b)/255, 1.f);
}
i++;
}
pcNode->update();
imv->update();
}
void
PointCloud2Vector3d (pcl::PointCloud<Point>::Ptr cloud, pcl::on_nurbs::vector_vec3d &data)
{
for (unsigned i = 0; i < cloud->size (); i++)
{
Point &p = cloud->at (i);
if (!pcl_isnan (p.x) && !pcl_isnan (p.y) && !pcl_isnan (p.z))
data.push_back (Eigen::Vector3d (p.x, p.y, p.z));
}
}
void
PointCloud2Vector2d (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::on_nurbs::vector_vec2d &data)
{
for (unsigned i = 0; i < cloud->size (); i++)
{
pcl::PointXYZ &p = cloud->at (i);
if (!pcl_isnan (p.x) && !pcl_isnan (p.y))
data.push_back (Eigen::Vector2d (p.x, p.y));
}
}
void Registrator::restore_NANs___dueToPCLbugInBILATERAL( pcl::PointCloud<TYPE_Point_Sensor>::Ptr& cloud_ORGan_P_, bool shouldPrint_countYesN_NANs )
{
if (shouldPrint_countYesN_NANs) std::cout << "restore_NANs___dueToPCLbugInBILATERAL" << "\t\t" << "cloud_ORGan_P_->height = " << cloud_ORGan_P_->height << std::endl;
if (shouldPrint_countYesN_NANs) std::cout << "restore_NANs___dueToPCLbugInBILATERAL" << "\t\t" << "cloud_ORGan_P_->width = " << cloud_ORGan_P_->width << std::endl;
double minZ = +std::numeric_limits<float>::infinity();
double maxZ = -std::numeric_limits<float>::infinity();
/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
cv::Mat testNANsIMG = cv::Mat(cloud_ORGan_P_->height,cloud_ORGan_P_->width,CV_8UC1,255);
////////////////////////////////////////////////////////////////////////////////////////
int countNANs_YES = 0;
int countNANs_NOO = 0;
//////////////////////
for (int iii=0; iii<cloud_ORGan_P_->height; ++iii)
{ for (int jjj=0; jjj<cloud_ORGan_P_->width; ++jjj)
{
///////////////////////////////////////////////////
if ( pcl_isnan( (*cloud_ORGan_P_)(jjj,iii).x ) ||
pcl_isnan( (*cloud_ORGan_P_)(jjj,iii).y ) ||
pcl_isnan( (*cloud_ORGan_P_)(jjj,iii).z ) )
///////////////////////////////////////////////////
{
///////////////////////////////////////////////////////////////////////
(*cloud_ORGan_P_)(jjj,iii).x = std::numeric_limits<float>::quiet_NaN();
(*cloud_ORGan_P_)(jjj,iii).y = std::numeric_limits<float>::quiet_NaN();
(*cloud_ORGan_P_)(jjj,iii).z = std::numeric_limits<float>::quiet_NaN();
///////////////////////////////////////////////////////////////////////
countNANs_YES++;
testNANsIMG.at<uchar>(iii,jjj) = 0;
}
else
{
countNANs_NOO++;
}
if (minZ > (*cloud_ORGan_P_)(jjj,iii).z)
minZ = (*cloud_ORGan_P_)(jjj,iii).z;
if (maxZ < (*cloud_ORGan_P_)(jjj,iii).z)
maxZ = (*cloud_ORGan_P_)(jjj,iii).z;
}
}
if (shouldPrint_countYesN_NANs) std::cout << "testNANs - yes NANs" << "\t\t" << countNANs_YES << "\t\t" << "minZ="<<minZ << "\t\t" << "maxZ="<<maxZ << std::endl;
if (shouldPrint_countYesN_NANs) std::cout << "testNANs - noo NANs" << "\t\t" << countNANs_NOO << "\t\t" << "minZ="<<minZ << "\t\t" << "maxZ="<<maxZ << std::endl << std::endl;
}
void App::Visualize() {
if (! DoVisualize_) {
return;
}
TUMDataSetVisualizer viewer(CameraDescription_);
VisAdjustedGradientNorms(&viewer);
VisGradientNorms(&viewer);
VisAlpha(&viewer);
VisOriginal(&viewer);
VisSVDensity(&viewer);
VisZ(&viewer);
VisDistToNN(&viewer);
if (DoShowNormals_) {
Builder_.CalcNormals();
NormalCloud::ConstPtr normals = Builder_.Normals;
viewer.addPointCloudNormals<PointType, NormalType>(Input_, normals);
viewer.EasyAdd(Input_, "input", [this, &normals] (int i) {
return pcl_isnan(normals->at(i).normal_x)
? Color({0, 0, 255})
: Color({255, 0, 0});
});
}
if (DoShowGradients_) {
NormalCloud::ConstPtr grads = Builder_.Gradients;
viewer.addPointCloudNormals<PointType, NormalType>(Input_, grads, 100, 0.002);
}
viewer.Run(SaveScreenshotPath_);
}
// Get the voxel which contains this point
cpu_tsdf::OctreeNode*
cpu_tsdf::Octree::getContainingVoxel (float x, float y, float z, float min_size)
{
if (pcl_isnan (z) || std::fabs (x) > size_x_/2 || std::fabs (y) > size_y_/2 || std::fabs (z) > size_z_/2)
return (NULL);
return (root_->getContainingVoxel (x, y, z, min_size));
}
void TrainingSetGenerator::GenerateUsingNormals(const PointCloud & input, const NormalCloud & normals, float gap) {
Grid2Num.Resize(input.height, input.width);
Pixel2Num.resize(input.height * input.width);
int indexNoNan = 0;
for (int y = 0; y < input.height; ++y) {
for (int x = 0; x < input.width; ++x) {
PointType point = input.at(x, y);
NormalType normal = normals.at(x, y);
int & num = Pixel2Num[y * input.width + x];
if (pointHasNan(point) || pcl_isnan(normal.normal_x)) {
num = -1;
continue;
}
auto nv3f = normal.getNormalVector3fMap();
pcl::flipNormalTowardsViewpoint(point, 0, 0, 0, nv3f[0], nv3f[1], nv3f[2]);
nv3f *= -1;
nv3f /= nv3f.norm();
nv3f *= Step_ * gap;
num = AddPoint(x, y, point, +1);
PointType shifted(point);
shifted.getVector3fMap() += nv3f;
AddPoint(x, y, shifted, -1);
indexNoNan++;
}
}
}
bool
reprojectPoint (const pcl::PointXYZRGBA &pt, int &u, int &v)
{
u = (pt.x * focal_length_x_ / pt.z) + principal_point_x_;
v = (pt.y * focal_length_y_ / pt.z) + principal_point_y_;
return (!pcl_isnan (pt.z) && pt.z > 0 && u >= 0 && u < width_ && v >= 0 && v < height_);
}
bool FilterLight::doLightFilter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &outcloud){
//(lcm_t *lcm, LightFilter *lightfilter,PointCloudPtr &cloud,PointCloudPtr &cloud_light_filtered){
int filter_nan=1; // @param: to fiter out the nan points or not
// Duplicate the PC to do light filtering: might be better to just collect the indices and use them
outcloud->width = incloud->points.size();
outcloud->height = 1;
outcloud->is_dense = false;
outcloud->points.resize (outcloud->width * outcloud->height);
int Nlight=0;
int decimate_image[] = {1,1};
if (verbose_text > 0){
std::cout << "Input to pcd_lightfilter: w: "<< incloud->width << " h " << incloud->height << "\n";
}
for(int j3x=0; j3x <incloud->points.size(); j3x++ ) { //l2r state->width 640
if (incloud->points[j3x].x > max_range ){ // remove all points 5.5+ m away
}else if (incloud->points[j3x].x < 0){ // remove all points behind camera (null ranges)
//}else if (v > 473){ // remove a black edge on the bottom of the point cloud:
}else{
bool nan_point =false;
if (filter_nan==1){
if (pcl_isnan (incloud->points[j3x].x)){
nan_point=true;
}else if(pcl_isnan (incloud->points[j3x].y)){
nan_point=true;
}else if(pcl_isnan (incloud->points[j3x].z)){
nan_point=true;
}
}
if (!nan_point){
outcloud->points[Nlight].x =incloud->points[j3x].x;
outcloud->points[Nlight].y =incloud->points[j3x].y;
outcloud->points[Nlight].z =incloud->points[j3x].z;
outcloud->points[Nlight].rgba =incloud->points[j3x].rgba;
Nlight++;
}
}
}
outcloud->points.resize (Nlight);
outcloud->width = (Nlight);
}
template<typename PointInT> std::size_t
pcl::nurbs::NurbsFitter<PointInT>::PCL2Eigen (PointCloudPtr &pcl_cloud, const std::vector<int> &indices, vector_vec3 &on_cloud)
{
std::size_t numPoints (0);
for (unsigned i = 0; i < indices.size (); i++)
{
const PointInT &pt = pcl_cloud->at (indices[i]);
if (!pcl_isnan (pt.x) && !pcl_isnan (pt.y) && !pcl_isnan (pt.z))
{
on_cloud.push_back (vec3 (pt.x, pt.y, pt.z));
++numPoints;
}
}
return (numPoints);
}
template <typename PointT, typename NormalT> bool
cpu_tsdf::TSDFVolumeOctree::integrateCloud (
const pcl::PointCloud<PointT> &cloud,
const pcl::PointCloud<NormalT> &normals,
const Eigen::Affine3d &trans)
{
Eigen::Affine3f trans_inv = trans.inverse ().cast<float> ();
// First, sample a few points and force their containing voxels to split
int px_step = 1;
int nsplit = 0;
for (size_t u = 0; u < cloud.width; u += px_step)
{
for (size_t v = 0; v < cloud.height; v += px_step)
{
const PointT &pt_surface_orig = cloud (u, v);
if (pcl_isnan (pt_surface_orig.z))
continue;
// Look at surroundings
int nstep = 0;
Eigen::Vector3f ray = pt_surface_orig.getVector3fMap ().normalized ();
for (int perm = 0; perm < num_random_splits_; perm++)
{
// Get containing voxels
PointT pt_trans;
float scale = (float)rand () / (float)RAND_MAX * 0.03;
Eigen::Vector3f noise = Eigen::Vector3f::Random ().normalized () * scale;;
if (perm == 0) noise *= 0;
pt_trans.getVector3fMap () = trans.cast<float> () * (pt_surface_orig.getVector3fMap ()+ noise);
OctreeNode* voxel = octree_->getContainingVoxel (pt_trans.x, pt_trans.y, pt_trans.z);
if (voxel != NULL)
{
while (voxel->getMinSize () > xsize_ / xres_)
{
nsplit++;
voxel->split ();
voxel = voxel->getContainingVoxel (pt_trans.x, pt_trans.y, pt_trans.z);
}
}
}
}
}
// Do Frustum Culling to get rid of unseen voxels
std::vector<cpu_tsdf::OctreeNode::Ptr> voxels_culled;
getFrustumCulledVoxels(trans, voxels_culled);
#pragma omp parallel for
for (size_t i = 0; i < voxels_culled.size (); i++)
{
updateVoxel (voxels_culled[i], cloud, normals, trans_inv);
}
// Cloud is no longer empty
is_empty_ = false;
return (true);
}
template <typename PointInT, typename PointNT, typename PointLabelT, typename PointOutT> void
cob_3d_features::OrganizedCurvatureEstimation<PointInT,PointNT,PointLabelT,PointOutT>::computeFeature (PointCloudOut &output)
{
if (labels_->points.size() != input_->size())
{
labels_->points.resize(input_->size());
labels_->height = input_->height;
labels_->width = input_->width;
}
if (output.points.size() != input_->size())
{
output.points.resize(input_->size());
output.height = input_->height;
output.width = input_->width;
}
std::vector<int> nn_indices;
std::vector<float> nn_distances;
for (std::vector<int>::iterator it=indices_->begin(); it != indices_->end(); ++it)
{
if (pcl_isnan(surface_->points[*it].z) ||
pcl_isnan(normals_->points[*it].normal[2]))
{
labels_->points[*it].label = I_NAN;
}
else if (this->searchForNeighborsInRange(*it, nn_indices, nn_distances) != -1)
{
computePointCurvatures(*normals_, *it, nn_indices, nn_distances,
output.points[*it].principal_curvature[0],
output.points[*it].principal_curvature[1],
output.points[*it].principal_curvature[2],
output.points[*it].pc1,
output.points[*it].pc2,
labels_->points[*it].label);
}
else
{
labels_->points[*it].label = I_NAN;
}
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::GSS3DEstimation<PointInT, PointNT, PointOutT>::calculateGeometricScaleSpace ()
{
normal_maps_.resize (scales_.size ());
// TODO Do we need the original normal_map? It's noisy anyway ...
// The normal map at the largest scale is the given one
// normal_maps_[0] = PointCloudNPtr ( new PointCloudN (*normals_));
// Compute the following scale spaces by convolving with a geodesic Gaussian kernel
float sigma, sigma_squared, dist, gauss_coeff, normalization_factor;
PointNT result, aux;
for (size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
{
printf ("Computing for scale %d\n", scales_[scale_i]);
PointCloudNPtr normal_map (new PointCloudN ());
normal_map->header = normals_->header;
normal_map->width = normals_->width;
normal_map->height = normals_->height;
normal_map->resize (normals_->size ());
sigma = scales_[scale_i];
sigma_squared = sigma * sigma;
// For all points on the depth image
for (int x = 0; x < int (normal_map->width); ++x)
for (int y = 0; y < int (normal_map->height); ++y)
{
result.normal_x = result.normal_y = result.normal_z = 0.0f;
// For all points in the Gaussian window
for (int win_x = -window_size_/2 * sigma; win_x <= window_size_/2 * sigma; ++win_x)
for (int win_y = -window_size_/2 * sigma; win_y <= window_size_/2 * sigma; ++win_y)
// This virtually borders the image with 0-normals
if ( (x + win_x >= 0) && (x + win_x < int (normal_map->width)) && (y + win_y >= 0) && (y + win_y < int (normal_map->height)))
{
dist = computeGeodesicDistance (x, y, x+win_x, y+win_y);
if (dist == -1.0f) continue;
gauss_coeff = 1 / (2 * M_PI * sigma_squared) * exp ((-1) * dist*dist / (2 * sigma_squared));
aux = normals_->at (x + win_x, y + win_y);
if (pcl_isnan (aux.normal_x)) continue;
result.normal_x += aux.normal_x * gauss_coeff; result.normal_y += aux.normal_y * gauss_coeff; result.normal_z += aux.normal_z * gauss_coeff;
}
// Normalize the resulting normal
normalization_factor = sqrt (result.normal_x*result.normal_x + result.normal_y*result.normal_y + result.normal_z*result.normal_z);
result.normal_x /= normalization_factor; result.normal_y /= normalization_factor; result.normal_z /= normalization_factor;
(*normal_map) (x, y) = result;
}
normal_maps_[scale_i] = normal_map;
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::GSS3DEstimation<PointInT, PointNT, PointOutT>::extractEdges ()
{
printf ("Extracting edges ...\n");
laplacians_ = boost::shared_ptr<Array3D> (new Array3D (boost::extents [scales_.size ()][normals_->width][normals_->height]));
for (size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
{
PointCloudInPtr edges (new PointCloudIn ());
// Border the image
for (int x = 0; x < int (normals_->width); ++x)
(*laplacians_)[scale_i][x][0] = (*laplacians_)[scale_i][x][normals_->height - 1] = 0.0f;
for (int y = 0; y < int (normals_->height); ++y)
(*laplacians_)[scale_i][0][y] = (*laplacians_)[scale_i][normals_->width - 1][y] = 0.0f;
// Look for zero crossings in the Laplacian of the normal map at this scale
for (int x = 1; x < int (normals_->width); ++x)
for (int y = 1; y < int (normals_->height); ++y)
{
(*laplacians_)[scale_i][x][y] = (*dd_horiz_normal_maps_)[scale_i][x][y] + (*dd_vert_normal_maps_)[scale_i][x][y];
/// TODO also need to threshold on the first derivative magnitude
if ( pcl_isnan ((*laplacians_)[scale_i][x][y]) || pcl_isnan ((*laplacians_)[scale_i][x-1][y]) || pcl_isnan ((*laplacians_)[scale_i][x][y-1]))
continue;
if ( ((*laplacians_)[scale_i][x-1][y] * (*laplacians_)[scale_i][x][y] < 0.0f ||
(*laplacians_)[scale_i][x][y-1] * (*laplacians_)[scale_i][x][y] < 0.0f) &&
(*d_horiz_normal_maps_)[scale_i][x][y] > 50 && (*d_vert_normal_maps_)[scale_i][x][y] > 50)
{
// This point is on an edge
printf ("%f %f\n", (*laplacians_)[scale_i][x-1][y], (*d_horiz_normal_maps_)[scale_i][x][y]);
edges->push_back (surface_->at(x, y));
}
}
edges_.push_back (edges);
}
}
template <typename PointInT> double
PointCloudCoherence<PointInT>::calcPointCoherence (PointInT &source, PointInT &target)
{
double val = 0.0;
for (size_t i = 0; i < point_coherences_.size (); i++)
{
PointCoherencePtr coherence = point_coherences_[i];
double d = log(coherence->compute (source, target));
//double d = coherence->compute (source, target);
if (! pcl_isnan(d))
val += d;
else
PCL_WARN ("nan!\n");
}
return val;
}
void
pcl::DenseCrf::addPairwiseNormals (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > &coord,
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &normals,
float sx, float sy, float sz,
float snx, float sny, float snz,
float w)
{
std::cout << coord.size () << std::endl;
std::cout << normals.size () << std::endl;
// create feature vector
std::vector<float> feature;
// reserve space for the six-dimensional Gaussian kernel
feature.resize (N_ * 6);
// fill the feature vector
for (size_t i = 0; i < coord.size (); i++)
{
if (pcl_isnan (normals[i].x ()))
{
if (i > 0)
{
normals[i].x () = normals[i-1].x ();
normals[i].y () = normals[i-1].y ();
normals[i].z () = normals[i-1].z ();
}
//std::cout << "NaN" << std::endl;
}
feature[i * 6 ] = static_cast<float> (coord[i].x ()) / sx;
feature[i * 6 + 1] = static_cast<float> (coord[i].y ()) / sy;
feature[i * 6 + 2] = static_cast<float> (coord[i].z ()) / sz;
feature[i * 6 + 3] = static_cast<float> (normals[i].x ()) / snx;
feature[i * 6 + 4] = static_cast<float> (normals[i].y ()) / sny;
feature[i * 6 + 5] = static_cast<float> (normals[i].z ()) / snz;
}
// add kernel
std::cout << "TEEEEST" << std::endl;
addPairwiseEnergy (feature, 6, w);
std::cout << "TEEEEST2" << std::endl;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr TSDFVolumeOctree::renderColoredView (const Eigen::Affine3d& trans, int downsampleBy) const {
pcl::PointCloud<pcl::PointNormal>::Ptr grayscale = renderView (trans, downsampleBy);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr colored (new pcl::PointCloud<pcl::PointXYZRGBNormal> (grayscale->width, grayscale->height));
colored->is_dense = false;
#pragma omp parallel for
for (size_t i = 0; i < colored->size(); ++i) {
pcl::PointXYZRGBNormal& pt = colored->at(i);
pt.getVector3fMap() = grayscale->at(i).getVector3fMap();
pt.getNormalVector3fMap() = grayscale->at(i).getNormalVector3fMap();
if (pcl_isnan (pt.z))
continue;
Eigen::Vector3f v_t = trans.cast<float> () * pt.getVector3fMap();
const OctreeNode* voxel = octree_->getContainingVoxel (v_t (0), v_t (1), v_t (2));
if (!voxel)
continue;
voxel->getRGB (pt.r, pt.g, pt.b);
}
return (colored);
}
pcl::CorrespondencesPtr Recognizer::flann_matcher(pcl::PointCloud<DescriptorType>::Ptr input_descriptors, pcl::PointCloud<DescriptorType>::Ptr target_descriptors, float match_thresh) {
pcl::KdTreeFLANN<DescriptorType> match_search;
match_search.setInputCloud(input_descriptors);
for(size_t i = 0; i < target_descriptors->size (); ++i) {
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
for (int j = 0; j < 33; j++) { // for each bin
if(pcl_isnan(target_descriptors->at(i).histogram[j]) || !pcl_isfinite(target_descriptors->at(i).histogram[j]))
continue;
}
int found_neighs = match_search.nearestKSearch(target_descriptors->at(i), 1, neigh_indices, neigh_sqr_dists);
if(found_neighs == 1 && neigh_sqr_dists[0] < match_thresh) { // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
(this->corrs)->push_back(corr);
}
}
return this->corrs;
}
template <typename T> void
pcl::visualization::ImageViewer::addRGBImage (const pcl::PointCloud<T> &cloud,
const std::string &layer_id,
double opacity)
{
if (data_size_ < cloud.width * cloud.height)
{
data_size_ = cloud.width * cloud.height * 3;
data_.reset (new unsigned char[data_size_]);
}
for (size_t i = 0; i < cloud.points.size (); ++i)
{
memcpy (&data_[i * 3], reinterpret_cast<const unsigned char*> (&cloud.points[i].rgba), sizeof (unsigned char) * 3);
/// Convert from BGR to RGB
unsigned char aux = data_[i*3];
data_[i*3] = data_[i*3+2];
data_[i*3+2] = aux;
for (int j = 0; j < 3; ++j)
if (pcl_isnan (data_[i * 3 + j]))
data_[i * 3 + j] = 0;
}
return (addRGBImage (data_.get (), cloud.width, cloud.height, layer_id, opacity));
}
template <typename PointInT> double
pcl::tracking::NormalCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target)
{
Eigen::Vector4f n = source.getNormalVector4fMap ();
Eigen::Vector4f n_dash = target.getNormalVector4fMap ();
if ( n.norm () <= 1e-5 || n_dash.norm () <= 1e-5 )
{
PCL_ERROR("norm might be ZERO!\n");
std::cout << "source: " << source << std::endl;
std::cout << "target: " << target << std::endl;
exit (1);
return 0.0;
}
else
{
n.normalize ();
n_dash.normalize ();
double theta = pcl::getAngle3D (n, n_dash);
if (!pcl_isnan (theta))
return 1.0 / (1.0 + weight_ * theta * theta);
else
return 0.0;
}
}
int
main (int argc, char** argv)
{
namespace bpo = boost::program_options;
namespace bfs = boost::filesystem;
bpo::options_description opts_desc("Allowed options");
bpo::positional_options_description p;
opts_desc.add_options()
("help,h", "produce help message")
("in", bpo::value<std::string> ()->required (), "Input dir")
("out", bpo::value<std::string> ()->required (), "Output dir")
("volume-size", bpo::value<float> (), "Volume size")
("cell-size", bpo::value<float> (), "Cell size")
("num-frames", bpo::value<size_t> (), "Partially integrate the sequence: only the first N clouds used")
("visualize", "Visualize")
("verbose", "Verbose")
("color", "Store color in addition to depth in the TSDF")
("flatten", "Flatten mesh vertices")
("cleanup", "Clean up mesh")
("invert", "Transforms are inverted (world -> camera)")
("world", "Clouds are given in the world frame")
("organized", "Clouds are already organized")
("width", bpo::value<int> (), "Image width")
("height", bpo::value<int> (), "Image height")
("zero-nans", "Nans are represented as (0,0,0)")
("num-random-splits", bpo::value<int> (), "Number of random points to sample around each surface reading. Leave empty unless you know what you're doing.")
("fx", bpo::value<float> (), "Focal length x")
("fy", bpo::value<float> (), "Focal length y")
("cx", bpo::value<float> (), "Center pixel x")
("cy", bpo::value<float> (), "Center pixel y")
("save-ascii", "Save ply file as ASCII rather than binary")
("cloud-units", bpo::value<float> (), "Units of the data, in meters")
("pose-units", bpo::value<float> (), "Units of the poses, in meters")
("max-sensor-dist", bpo::value<float> (), "Maximum distance data can be from the sensor")
("min-sensor-dist", bpo::value<float> (), "Minimum distance data can be from the sensor")
("trunc-dist-pos", bpo::value<float> (), "Positive truncation distance")
("trunc-dist-neg", bpo::value<float> (), "Negative truncation distance")
("min-weight", bpo::value<float> (), "Minimum weight to render")
("cloud-only", "Save aggregate cloud rather than actually running TSDF")
;
bpo::variables_map opts;
bpo::store(bpo::parse_command_line(argc, argv, opts_desc, bpo::command_line_style::unix_style ^ bpo::command_line_style::allow_short), opts);
bool badargs = false;
try { bpo::notify(opts); }
catch(...) { badargs = true; }
if(opts.count("help") || badargs) {
cout << "Usage: " << bfs::basename(argv[0]) << " --in [in_dir] --out [out_dir] [OPTS]" << endl;
cout << "Integrates multiple clouds and returns a mesh. Assumes clouds are PCD files and poses are ascii (.txt) or binary float (.transform) files with the same prefix, specifying the pose of the camera in the world frame. Can customize many parameters, but if you don't know what they do, the defaults are strongly recommended." << endl;
cout << endl;
cout << opts_desc << endl;
return (1);
}
// Visualize?
bool visualize = opts.count ("visualize");
bool verbose = opts.count ("verbose");
bool flatten = opts.count ("flatten");
bool cleanup = opts.count ("cleanup");
bool invert = opts.count ("invert");
bool organized = opts.count ("organized");
bool world_frame = opts.count ("world");
bool zero_nans = opts.count ("zero-nans");
bool save_ascii = opts.count ("save-ascii");
float cloud_units = 1.;
if (opts.count ("cloud-units"))
cloud_units = opts["cloud-units"].as<float> ();
float pose_units = 1.;
if (opts.count ("pose-units"))
pose_units = opts["pose-units"].as<float> ();
int num_random_splits = 1;
if (opts.count ("num-random-splits"))
num_random_splits = opts["num-random-splits"].as<int> ();
float max_sensor_dist = 3.0;
if (opts.count ("max-sensor-dist"))
max_sensor_dist = opts["max-sensor-dist"].as<float> ();
float min_sensor_dist = 0;
if (opts.count ("min-sensor-dist"))
min_sensor_dist = opts["min-sensor-dist"].as<float> ();
float min_weight = 0;
if (opts.count ("min-weight"))
min_weight = opts["min-weight"].as<float> ();
float trunc_dist_pos = 0.03;
if (opts.count ("trunc-dist-pos"))
trunc_dist_pos = opts["trunc-dist-pos"].as<float> ();
float trunc_dist_neg = 0.03;
if (opts.count ("trunc-dist-neg"))
trunc_dist_neg = opts["trunc-dist-neg"].as<float> ();
bool binary_poses = false;
if (opts.count ("width"))
width_ = opts["width"].as<int> ();
if (opts.count ("height"))
height_ = opts["height"].as<int> ();
focal_length_x_ = 525. * width_ / 640.;
focal_length_y_ = 525. * height_ / 480.;
principal_point_x_ = static_cast<float> (width_)/2. - 0.5;
principal_point_y_ = static_cast<float> (height_)/2. - 0.5;
if (opts.count ("fx"))
focal_length_x_ = opts["fx"].as<float> ();
if (opts.count ("fy"))
focal_length_y_ = opts["fy"].as<float> ();
if (opts.count ("cx"))
principal_point_x_ = opts["cx"].as<float> ();
if (opts.count ("cy"))
principal_point_y_ = opts["cy"].as<float> ();
bool cloud_only = opts.count ("cloud-only");
bool integrate_color = opts.count("color");
pcl::console::TicToc tt;
tt.tic ();
// Scrape files
std::vector<std::string> pcd_files;
std::vector<std::string> pose_files;
std::vector<std::string> pose_files_unordered;
bool found_pose_file = false;
std::string pose_extension = "";
std::string dir = opts["in"].as<std::string> ();
std::string out_dir = opts["out"].as<std::string> ();
boost::filesystem::directory_iterator end_itr;
for (boost::filesystem::directory_iterator itr (dir); itr != end_itr; ++itr)
{
std::string extension = boost::filesystem::extension (itr->path ());
std::string pathname = itr->path ().string ();
// Look for PCD files
if (extension == ".PCD" || extension == ".pcd")
{
pcd_files.push_back (pathname);
}
else if (extension == ".TRANSFORM" || extension == ".transform")
{
if (found_pose_file && extension != pose_extension)
{
PCL_ERROR ("Files with extension %s and %s were found in this folder! Please choose a consistent extension.\n", extension.c_str (), pose_extension.c_str ());
return (1);
}
else if (!found_pose_file)
{
found_pose_file = true;
binary_poses = true;
pose_extension = extension;
}
pose_files_unordered.push_back (pathname);
}
else if (extension == ".TXT" || extension == ".txt")
{
if (found_pose_file && extension != pose_extension)
{
PCL_ERROR ("Files with extension %s and %s were found in this folder! Please choose a consistent extension.\n", extension.c_str (), pose_extension.c_str ());
return (1);
}
else if (!found_pose_file)
{
found_pose_file = true;
binary_poses = false;
pose_extension = extension;
}
pose_files_unordered.push_back (pathname);
}
}
// Sort PCDS
std::sort (pcd_files.begin (), pcd_files.end ());
std::sort (pose_files_unordered.begin (), pose_files_unordered.end ());
std::string pcd_prefix = getSharedPrefix(pcd_files);
std::string pose_prefix = getSharedPrefix(pose_files_unordered);
PCL_INFO ("Found PCD files with prefix: %s, poses with prefix: %s poses\n", pcd_prefix.c_str (), pose_prefix.c_str ());
// For each PCD, get the pose file
for (size_t i = 0; i < pcd_files.size (); i++)
{
const std::string pcd_path = pcd_files[i];
std::string suffix = boost::filesystem::basename (boost::filesystem::path (pcd_path.substr (pcd_prefix.length())));
std::string pose_path = pose_prefix+suffix+pose_extension;
// Check if .transform file exists
if (boost::filesystem::exists (pose_path))
{
pose_files.push_back (pose_path);
}
else
{
PCL_ERROR ("Could not find matching transform file for %s\n", pcd_path.c_str ());
return 1;
}
}
std::sort (pose_files.begin (), pose_files.end ());
PCL_INFO ("Reading in %s pose files\n",
binary_poses ? "binary" : "ascii");
std::vector<Eigen::Affine3d> poses (pose_files.size ());
for (size_t i = 0; i < pose_files.size (); i++)
{
ifstream f (pose_files[i].c_str ());
float v;
Eigen::Matrix4d mat;
mat (3,0) = 0; mat (3,1) = 0; mat (3,2) = 0; mat (3,3) = 1;
for (int y = 0; y < 3; y++)
{
for (int x = 0; x < 4; x++)
{
if (binary_poses)
f.read ((char*)&v, sizeof (float));
else
f >> v;
mat (y,x) = static_cast<double> (v);
}
}
f.close ();
poses[i] = mat;
if (invert)
poses[i] = poses[i].inverse ();
// Update units
poses[i].matrix ().topRightCorner <3, 1> () *= pose_units;
if (verbose)
{
std::cout << "Pose[" << i << "]" << std::endl
<< poses[i].matrix () << std::endl;
}
}
PCL_INFO ("Done!\n");
// Begin Integration
float tsdf_size = 12.;
if (opts.count ("volume-size"))
tsdf_size = opts["volume-size"].as<float> ();
float cell_size = 0.006;
if (opts.count ("cell-size"))
cell_size = opts["cell-size"].as<float> ();
int tsdf_res;
int desired_res = tsdf_size / cell_size;
// Snap to nearest power of 2;
int n = 1;
while (desired_res > n)
{
n *= 2;
}
tsdf_res = n;
// Initialize
cpu_tsdf::TSDFVolumeOctree::Ptr tsdf;
if (!cloud_only)
{
tsdf.reset (new cpu_tsdf::TSDFVolumeOctree);
tsdf->setGridSize (tsdf_size, tsdf_size, tsdf_size);
PCL_INFO("Setting resolution: %d with grid size %f\n", tsdf_res, tsdf_size);
tsdf->setResolution (tsdf_res, tsdf_res, tsdf_res);
tsdf->setImageSize (width_, height_);
tsdf->setCameraIntrinsics (focal_length_x_, focal_length_y_, principal_point_x_, principal_point_y_);
tsdf->setNumRandomSplts (num_random_splits);
tsdf->setSensorDistanceBounds (min_sensor_dist, max_sensor_dist);
tsdf->setIntegrateColor (integrate_color);
tsdf->setDepthTruncationLimits (trunc_dist_pos, trunc_dist_neg);
tsdf->reset ();
}
// Load data
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr map (new pcl::PointCloud<pcl::PointXYZRGBA>);
// Set up visualization
pcl::visualization::PCLVisualizer::Ptr vis;
if (visualize)
{
vis.reset (new pcl::visualization::PCLVisualizer);
vis->addCoordinateSystem ();
}
// Initialize aggregate cloud if we are just doing that instead
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr aggregate;
if (cloud_only)
aggregate.reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
size_t num_frames = pcd_files.size ();
if (opts.count ("num-frames"))
{
size_t user_selected_num_frames = opts["num-frames"].as<size_t> ();
if (user_selected_num_frames <= num_frames)
{
num_frames = user_selected_num_frames;
}
else
{
PCL_WARN("Warning: Manually input --num-frames=%zu, but the sequence only has %zu clouds. Ignoring user specification.\n", user_selected_num_frames, num_frames);
}
}
for (size_t i = 0; i < num_frames; i++)
{
PCL_INFO ("On frame %d / %d\n", i+1, num_frames);
if (poses.size () <= i)
{
PCL_WARN ("Warning: no matching pose file found for cloud %s.\n"
"Defaulting to identity, but unless the camera never moved, this will yield a very poor mesh!\n",
pcd_files[i].c_str ());
pose_files.push_back ("not_found");
poses.push_back (Eigen::Affine3d::Identity ());
}
else
{
PCL_INFO ("Cloud: %s, pose: %s\n",
pcd_files[i].c_str (), pose_files[i].c_str ());
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::io::loadPCDFile (pcd_files[i], *cloud);
if (cloud_units != 1)
{
for (size_t i = 0; i < cloud->size (); i++)
{
pcl::PointXYZRGBA &pt = cloud->at (i);
pt.x *= cloud_units;
pt.y *= cloud_units;
pt.z *= cloud_units;
}
}
// Remove nans
if (zero_nans)
{
for (size_t j = 0; j < cloud->size (); j++)
{
if (cloud->at (j).x == 0 && cloud->at (j).y == 0 && cloud->at (j).z == 0)
cloud->at (j).x = cloud->at (j).y = cloud->at (j).z = std::numeric_limits<float>::quiet_NaN ();
}
}
// Transform
if (world_frame)
pcl::transformPointCloud (*cloud, *cloud, poses[i].inverse ());
// Make organized
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_organized (new pcl::PointCloud<pcl::PointXYZRGBA> (width_, height_));
if (organized)
{
if (cloud->height != height_ || cloud->width != width_)
{
PCL_ERROR ("Error: cloud %d has size %d x %d, but TSDF is initialized for %d x %d pointclouds\n", i+1, cloud->width, cloud->height, width_, height_);
return (1);
}
pcl::copyPointCloud (*cloud, *cloud_organized);
}
else
{
size_t nonnan_original = 0;
size_t nonnan_new = 0;
float min_x = std::numeric_limits<float>::infinity ();
float min_y = std::numeric_limits<float>::infinity ();
float min_z = std::numeric_limits<float>::infinity ();
float max_x = -std::numeric_limits<float>::infinity ();
float max_y = -std::numeric_limits<float>::infinity ();
float max_z = -std::numeric_limits<float>::infinity ();
for (size_t j = 0; j < cloud_organized->size (); j++)
cloud_organized->at (j).z = std::numeric_limits<float>::quiet_NaN ();
for (size_t j = 0; j < cloud->size (); j++)
{
const pcl::PointXYZRGBA &pt = cloud->at (j);
if (verbose && !pcl_isnan (pt.z))
nonnan_original++;
int u, v;
if (reprojectPoint (pt, u, v))
{
pcl::PointXYZRGBA &pt_old = (*cloud_organized) (u, v);
if (pcl_isnan (pt_old.z) || (pt_old.z > pt.z))
{
if (verbose)
{
if (pcl_isnan (pt_old.z))
nonnan_new++;
if (pt.x < min_x) min_x = pt.x;
if (pt.y < min_y) min_y = pt.y;
if (pt.z < min_z) min_z = pt.z;
if (pt.x > max_x) max_x = pt.x;
if (pt.y > max_y) max_y = pt.y;
if (pt.z > max_z) max_z = pt.z;
}
pt_old = pt;
}
}
}
if (verbose)
{
PCL_INFO ("Reprojection yielded %d valid points, of initial %d\n", nonnan_new, nonnan_original);
PCL_INFO ("Cloud bounds: [%f, %f], [%f, %f], [%f, %f]\n", min_x, max_x, min_y, max_y, min_z, max_x);
}
}
if (visualize) // Just for visualization purposes
{
vis->removeAllPointClouds ();
pcl::PointCloud<pcl::PointXYZRGBA> cloud_trans;
pcl::transformPointCloud (*cloud_organized, cloud_trans, poses[i]);
*map += cloud_trans;
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGBA> map_handler (map, 255, 0, 0);
vis->addPointCloud (map, map_handler, "map");
PCL_INFO ("Map\n");
vis->spin ();
}
//Integrate
Eigen::Affine3d pose_rel_to_first_frame = poses[0].inverse () * poses[i];
if (cloud_only) // Only if we're just dumping out the cloud
{
pcl::PointCloud<pcl::PointXYZRGBA> cloud_unorganized;
for (size_t i = 0; i < cloud_organized->size (); i++)
{
if (!pcl_isnan (cloud_organized->at (i).z))
cloud_unorganized.push_back (cloud_organized->at (i));
}
pcl::transformPointCloud (cloud_unorganized, cloud_unorganized, pose_rel_to_first_frame);
*aggregate += cloud_unorganized;
// Filter so it doesn't get too big
if (i % 20 == 0 || i == num_frames - 1)
{
pcl::VoxelGrid<pcl::PointXYZRGBA> vg;
vg.setLeafSize (0.01, 0.01, 0.01);
vg.setInputCloud (aggregate);
vg.filter (cloud_unorganized);
*aggregate = cloud_unorganized;
}
}
else
{
tsdf->integrateCloud (*cloud_organized, pcl::PointCloud<pcl::Normal> (), pose_rel_to_first_frame);
}
}
// Save
boost::filesystem::create_directory (out_dir);
// If we're just saving the cloud, no need to mesh
if (cloud_only)
{
pcl::io::savePCDFileBinaryCompressed (out_dir + "/cloud.pcd", *aggregate);
}
else
{
cpu_tsdf::MarchingCubesTSDFOctree mc;
mc.setMinWeight (min_weight);
mc.setInputTSDF (tsdf);
if (integrate_color)
{
mc.setColorByRGB (true);
}
pcl::PolygonMesh::Ptr mesh (new pcl::PolygonMesh);
mc.reconstruct (*mesh);
if (flatten)
flattenVertices (*mesh);
if (cleanup)
cleanupMesh (*mesh);
if (visualize)
{
vis->removeAllPointClouds ();
vis->addPolygonMesh (*mesh);
vis->spin ();
}
PCL_INFO ("Entire pipeline took %f ms\n", tt.toc ());
if (save_ascii)
pcl::io::savePLYFile (out_dir + "/mesh.ply", *mesh);
else
pcl::io::savePLYFileBinary (out_dir + "/mesh.ply", *mesh);
PCL_INFO ("Saved to %s/mesh.ply\n", out_dir.c_str ());
}
}
pcl::PointCloud<pcl::PointNormal>::Ptr TSDFVolumeOctree::renderView (const Eigen::Affine3d& trans, int downsampleBy) const {
int new_width = image_width_ / downsampleBy;
int new_height = image_height_ / downsampleBy;
double new_fx = focal_length_x_ / downsampleBy;
double new_fy = focal_length_y_ / downsampleBy;
double new_cx = principal_point_x_ / downsampleBy;
double new_cy = principal_point_y_ / downsampleBy;
pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal> (new_width, new_height));
cloud->is_dense = false;
float min_step = max_dist_neg_ * 3/4.;
#pragma omp parallel for
for (size_t i = 0; i < cloud->size (); ++i) {
size_t x = i % new_width;
size_t y = i / new_width;
//Raytrace
pcl::PointNormal& pt = cloud->operator() (x,y);
bool found_crossing = false;
Eigen::Vector3f du ( (x - new_cx)/new_fx,
(y - new_cy)/new_fy,
1);
du.normalize ();
//Apply transform -- rotate the ray, start at the offset
du = trans.rotation ().cast<float> () * du;
Eigen::Vector3f p = trans.translation ().cast<float> ();
//Preallocate
int x_i, y_i, z_i;
float d;
float w;
float last_w = 0;
float last_d = 0;
float t = min_sensor_dist_;
p += t*du;
float step = min_step;
//Check if ray intersects cube
//Initially we haven't ever hit a voxel
bool hit_voxel = false;
int niter = 0;
while (t < max_sensor_dist_) {
const OctreeNode* voxel = octree_->getContainingVoxel (p (0), p(1), p(2));
if (voxel) {
hit_voxel = true;
voxel->getData (d, w);
if (((d < 0 && last_d > 0) || (d > 0 && last_d < 0)) && last_w && w) {
found_crossing = true;
float old_t = t - step;
step = (zsize_/zres_)/2;
float new_d;
float new_w;
float last_new_d = d;
float last_new_w = w;
while (t >= old_t) {
t -= step;
p -= step*du;
voxel = octree_->getContainingVoxel (p (0), p(1), p(2));
if (!voxel)
break;
voxel->getData (new_d, new_w);
if ((last_d > 0 && new_d > 0) || (last_d < 0 && new_d < 0)) {
last_d = new_d;
last_w = new_w;
d = last_new_d;
w = last_new_w;
t += step;
p += step*du;
break;
}
last_new_d = d;
last_new_w = w;
}
break;
}
last_d = d;
last_w = w;
//Update step
step = std::max (voxel->getMinSize () / 4., fabs (d)*max_dist_neg_);
}
else {
if (hit_voxel)
break;
}
t += step;
p += step*du;
niter++;
}
if (!found_crossing) {
pt.x = pt.y = pt.z =std::numeric_limits<float>::quiet_NaN();
}
else {
//Get optimal t
bool has_data = true;
float tcurr = t;
float tprev = t-step;
last_d = getTSDFValue (trans.translation ().cast<float> () + (tprev) * du,
&has_data);
d = getTSDFValue (trans.translation ().cast<float> () + tcurr * du, &has_data);
if (!has_data || pcl_isnan (d) || pcl_isnan (last_d)) {
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
}
float t_star = t + step * (-1 + fabs (last_d / (last_d -d)));
pt.getVector3fMap () = trans.translation().cast<float> () + tcurr * du;
//Get normals by looking at adjacent voxels
const OctreeNode* voxel = octree_->getContainingVoxel (pt.x, pt.y, pt.z);
if (!voxel) {
pt.normal_x = pt.normal_y = pt.normal_z = std::numeric_limits<float>::quiet_NaN();
continue;
}
float xsize, ysize, zsize;
voxel->getSize (xsize, ysize, zsize);
bool valid = true;
float d_xm = getTSDFValue (pt.x-xsize, pt.y, pt.z, &valid);
float d_xp = getTSDFValue (pt.x+xsize, pt.y, pt.z, &valid);
float d_ym = getTSDFValue (pt.x, pt.y-ysize, pt.z, &valid);
float d_yp = getTSDFValue (pt.x, pt.y+ysize, pt.z, &valid);
float d_zm = getTSDFValue (pt.x, pt.y, pt.z-zsize, &valid);
float d_zp = getTSDFValue (pt.x, pt.y, pt.z+zsize, &valid);
if (!valid) {
pt.normal_x = pt.normal_y = pt.normal_z = std::numeric_limits<float>::quiet_NaN();
continue;
}
Eigen::Vector3f dF;
dF (0) = (d_xp - d_xm)*max_dist_neg_/ (2*xsize);
dF (1) = (d_yp - d_ym)*max_dist_neg_/ (2*ysize);
dF (2) = (d_zp - d_zm)*max_dist_neg_/ (2*zsize);
dF.normalize();
pt.normal_x = dF(0);
pt.normal_y = dF(1);
pt.normal_z = dF(2);
}
}
pcl::transformPointCloudWithNormals (*cloud, *cloud, trans.inverse());
return (cloud);
}
bool FilterCloudToLaser::doCloud2Laser(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &outcloud,bot_core_planar_lidar_t &outlidar){
//(lcm_t *lcm, LightFilter *lightfilter,PointCloudPtr &cloud,PointCloudPtr &cloud_light_filtered){
int filter_nan=1; // @param: to filter out the nan points or not
// Duplicate the PC to do light filtering: might be better to just collect the indices and use them
outcloud->width = incloud->points.size();
outcloud->height = 1;
outcloud->is_dense = false;
outcloud->points.resize (outcloud->width * outcloud->height);
//bot_core_planar_lidar_t outlidar;
outlidar.utime = 0;
outlidar.nranges = nranges;
outlidar.rad0 = rad0;
outlidar.radstep = radstep;
outlidar.nintensities =0;
outlidar.intensities =NULL;
int Npts_out=0;
for(int j3x=0; j3x <incloud->points.size(); j3x++ ) { //l2r state->width 640
if (incloud->points[j3x].x < 0){ // remove all points behind camera (null ranges)
//continue;
//}else if (v > 473){ // remove a black edge on the bottom of the point cloud:
}else if (incloud->points[j3x].z > max_height){
//continue;
}else if (incloud->points[j3x].z < min_height){
//continue;
}else{
bool nan_point =false;
if (filter_nan==1){
if (pcl_isnan (incloud->points[j3x].x)){
nan_point=true;
}else if(pcl_isnan (incloud->points[j3x].y)){
nan_point=true;
}else if(pcl_isnan (incloud->points[j3x].z)){
nan_point=true;
}
}
if (!nan_point){
double x =incloud->points[j3x].x;
double y =incloud->points[j3x].y;
double z =incloud->points[j3x].z;
outcloud->points[Npts_out].x =x;
outcloud->points[Npts_out].y =y;
outcloud->points[Npts_out].z =z;
outcloud->points[Npts_out].rgba =incloud->points[j3x].rgba;
Npts_out++;
double angle = atan2(y, x);
int index = (angle - rad0) / radstep;
double range_sq = y*y+x*x;
if (ranges[index] * ranges[index] > range_sq){
ranges[index] = sqrt(range_sq);
}
}
}
}
outcloud->points.resize (Npts_out);
outcloud->width = (Npts_out);
outlidar.ranges = ranges;
//bot_core_planar_lidar_t_publish(publish_lcm,"KINECT_LIDAR",&outlidar);
//free(ranges);
}
static bool is_invalid (double value) { return pcl_isnan (value); };
static bool is_invalid (float value) { return pcl_isnan (value); };
template <typename PointInT, typename PointNT, typename PointLabelT, typename PointOutT> void
cob_3d_features::OrganizedCurvatureEstimation<PointInT,PointNT,PointLabelT,PointOutT>::computePointCurvatures (
const NormalCloudIn &normals,
const int index,
const std::vector<int> &indices,
const std::vector<float> &sqr_distances,
float &pcx, float &pcy, float &pcz, float &pc1, float &pc2,
int &label_out)
{
Eigen::Vector3f n_idx(normals.points[index].normal);
Eigen::Matrix3f M = Eigen::Matrix3f::Identity() - n_idx * n_idx.transpose(); // projection matrix
std::vector<Eigen::Vector3f> normals_projected;
normals_projected.reserve(n_points_);
Eigen::Vector3f centroid = Eigen::Vector3f::Zero();
Eigen::Vector3f p_idx = surface_->points[index].getVector3fMap();
float angle = 0.0; // to discriminate convex and concave surface
int prob_concave = 0, prob_convex = 0;
for (std::vector<int>::const_iterator it = indices.begin(); it != indices.end(); ++it)
{
PointNT const* n_i = &(normals.points[*it]);
if ( pcl_isnan(n_i->normal[2]) ) continue;
normals_projected.push_back( M * Eigen::Vector3f(n_i->normal) );
centroid += normals_projected.back();
if ( (surface_->points[*it].getVector3fMap() - p_idx).normalized().dot(n_idx) > 0.0) ++prob_concave;
else ++prob_convex;
}
if (normals_projected.size() <=1) return;
float num_p_inv = 1.0f / normals_projected.size();
centroid *= num_p_inv;
Eigen::Matrix3f cov = Eigen::Matrix3f::Zero();
{
std::vector<Eigen::Vector3f>::iterator n_it = normals_projected.begin();
std::vector<float>::const_iterator d_it = sqr_distances.begin();
for (; n_it != normals_projected.end(); ++n_it, ++d_it)
{
Eigen::Vector3f demean = *n_it - centroid;
//cov += 1.0f / sqrt(*d_it) * demean * demean.transpose();
cov += demean * demean.transpose();
}
}
Eigen::Vector3f eigenvalues;
Eigen::Matrix3f eigenvectors;
pcl::eigen33(cov, eigenvectors, eigenvalues);
pcx = eigenvectors (0,2);
pcy = eigenvectors (1,2);
pcz = eigenvectors (2,2);
if (prob_concave < prob_convex) // if convex, make eigenvalues negative
num_p_inv *= surface_->points[index].z * (-1);
//num_p_inv *= 1.0 / (10.0*centroid.norm()) * surface_->points[index].z * (-1);
else
num_p_inv *= surface_->points[index].z;
pc1 = eigenvalues (2) * num_p_inv;
pc2 = eigenvalues (1) * num_p_inv;
//normals_->points[index].curvature = curvatures_->points[index].pc1;
if (std::abs(pc1) >= edge_curvature_threshold_ && label_out == 0)
label_out = I_EDGE;
}
template<typename PointT, typename PointLT> void
pcl::OrganizedEdgeDetection<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
{
const unsigned invalid_label = std::numeric_limits<unsigned>::max ();
pcl::Label invalid_pt;
invalid_pt.label = std::numeric_limits<unsigned>::max ();
labels.points.resize (input_->points.size (), invalid_pt);
labels.width = input_->width;
labels.height = input_->height;
unsigned int clust_id = 0;
std::cout << "width: " << labels.width << std::endl;
std::cout << "height: " << labels.height << std::endl;
std::cout << "[done] OrganizedEdgeDetection::compute ()" << std::endl;
// fill lookup table for next points to visit
const int num_of_ngbr = 8;
Neighbor directions [num_of_ngbr] = {Neighbor(-1, 0, -1),
Neighbor(-1, -1, -labels.width - 1),
Neighbor( 0, -1, -labels.width ),
Neighbor( 1, -1, -labels.width + 1),
Neighbor( 1, 0, 1),
Neighbor( 1, 1, labels.width + 1),
Neighbor( 0, 1, labels.width ),
Neighbor(-1, 1, labels.width - 1)};
for (int row = 1; row < int(input_->height) - 1; row++)
{
for (int col = 1; col < int(input_->width) - 1; col++)
{
int curr_idx = row*int(input_->width) + col;
if (!pcl_isfinite (input_->points[curr_idx].z))
continue;
float curr_depth = fabs (input_->points[curr_idx].z);
// Calculate depth distances between current point and neighboring points
std::vector<float> nghr_dist;
nghr_dist.resize (8);
bool found_invalid_neighbor = false;
for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
{
int nghr_idx = curr_idx + directions[d_idx].d_index;
assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
if (!pcl_isfinite (input_->points[nghr_idx].z))
{
found_invalid_neighbor = true;
break;
}
nghr_dist[d_idx] = curr_depth - fabs (input_->points[nghr_idx].z);
}
if (!found_invalid_neighbor)
{
// Every neighboring points are valid
std::vector<float>::iterator min_itr = std::min_element (nghr_dist.begin (), nghr_dist.end ());
std::vector<float>::iterator max_itr = std::max_element (nghr_dist.begin (), nghr_dist.end ());
float nghr_dist_min = *min_itr;
float nghr_dist_max = *max_itr;
float dist_dominant = fabs (nghr_dist_min) > fabs (nghr_dist_max) ? nghr_dist_min : nghr_dist_max;
if (fabs (dist_dominant) > th_depth_discon_*fabs (curr_depth))
{
// Found a depth discontinuity
if (dist_dominant > 0.f)
labels[curr_idx].label = EDGELABEL_OCCLUDED;
else
labels[curr_idx].label = EDGELABEL_OCCLUDING;
}
}
else
{
// Some neighboring points are not valid (nan points)
// Search for corresponding point across invalid points
// Search direction is determined by nan point locations with respect to current point
int dx = 0;
int dy = 0;
int num_of_invalid_pt = 0;
for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
{
int nghr_idx = curr_idx + directions[d_idx].d_index;
assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
if (!pcl_isfinite (input_->points[nghr_idx].z))
{
dx += directions[d_idx].d_x;
dy += directions[d_idx].d_y;
num_of_invalid_pt++;
}
}
// Search directions
assert (num_of_invalid_pt > 0);
float f_dx = static_cast<float> (dx) / static_cast<float> (num_of_invalid_pt);
float f_dy = static_cast<float> (dy) / static_cast<float> (num_of_invalid_pt);
// Search for corresponding point across invalid points
float corr_depth = std::numeric_limits<float>::quiet_NaN ();
for (int s_idx = 1; s_idx < max_search_neighbors_; s_idx++)
{
int s_row = row + static_cast<int> (std::floor (f_dy*static_cast<float> (s_idx)));
int s_col = col + static_cast<int> (std::floor (f_dx*static_cast<float> (s_idx)));
if (s_row < 0 || s_row >= int(input_->height) || s_col < 0 || s_col >= int(input_->width))
break;
if (pcl_isfinite (input_->points[s_row*int(input_->width)+s_col].z))
{
corr_depth = fabs (input_->points[s_row*int(input_->width)+s_col].z);
break;
}
}
if (!pcl_isnan (corr_depth))
{
// Found a corresponding point
float dist = curr_depth - corr_depth;
if (fabs (dist) > th_depth_discon_*fabs (curr_depth))
{
// Found a depth discontinuity
if (dist > 0.f)
labels[curr_idx].label = EDGELABEL_OCCLUDED;
else
labels[curr_idx].label = EDGELABEL_OCCLUDING;
}
}
else
{
// Not found a corresponding point, just nan boundary edge
labels[curr_idx].label = EDGELABEL_NAN_BOUNDARY;
}
}
}
}
// Assign label indices
label_indices.resize (3);
for (unsigned idx = 0; idx < input_->points.size (); idx++)
{
if (labels[idx].label != invalid_label)
{
label_indices[labels[idx].label].indices.push_back (idx);
}
}
}
template <typename PointT> void
pcl::PassThrough<PointT>::applyFilter (PointCloud &output)
{
// Has the input dataset been set already?
if (!input_)
{
PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
// Check if we're going to keep the organized structure of the cloud or not
if (keep_organized_)
{
if (filter_field_name_.empty ())
{
// Silly - if no filtering is actually done, and we want to keep the data organized,
// just copy everything. Any optimizations that can be done here???
output = *input_;
return;
}
output.width = input_->width;
output.height = input_->height;
// Check what the user value is: if !finite, set is_dense to false, true otherwise
if (!pcl_isfinite (user_filter_value_))
output.is_dense = false;
else
output.is_dense = input_->is_dense;
}
else
{
output.height = 1; // filtering breaks the organized structure
// Because we're doing explit checks for isfinite, is_dense = true
output.is_dense = true;
}
output.points.resize (input_->points.size ());
removed_indices_->resize (input_->points.size ());
int nr_p = 0;
int nr_removed_p = 0;
// If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
if (!filter_field_name_.empty ())
{
// Get the distance field index
std::vector<sensor_msgs::PointField> fields;
int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_, fields);
if (distance_idx == -1)
{
PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
output.width = output.height = 0;
output.points.clear ();
return;
}
if (keep_organized_)
{
for (size_t cp = 0; cp < input_->points.size (); ++cp)
{
if (pcl_isnan (input_->points[cp].x) ||
pcl_isnan (input_->points[cp].y) ||
pcl_isnan (input_->points[cp].z))
{
output.points[cp].x = output.points[cp].y = output.points[cp].z = user_filter_value_;
continue;
}
// Copy the point
pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (input_->points[cp], output.points[cp]));
// Filter it. Get the distance value
uint8_t* pt_data = (uint8_t*)&input_->points[cp];
float distance_value = 0;
memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
if (filter_limit_negative_)
{
// Use a threshold for cutting out points which inside the interval
if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
{
output.points[cp].x = output.points[cp].y = output.points[cp].z = user_filter_value_;
continue;
}
else
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p]=cp;
nr_removed_p++;
}
}
}
else
{
// Use a threshold for cutting out points which are too close/far away
if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
{
output.points[cp].x = output.points[cp].y = output.points[cp].z = user_filter_value_;
continue;
}
else
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
}
}
}
nr_p = input_->points.size ();
}
// Remove filtered points
else
{
// Go over all points
for (size_t cp = 0; cp < input_->points.size (); ++cp)
{
// Check if the point is invalid
if (!pcl_isfinite (input_->points[cp].x) || !pcl_isfinite (input_->points[cp].y) || !pcl_isfinite (input_->points[cp].z))
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
// Get the distance value
uint8_t* pt_data = (uint8_t*)&input_->points[cp];
float distance_value = 0;
memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
if (filter_limit_negative_)
{
// Use a threshold for cutting out points which inside the interval
if (distance_value < filter_limit_max_ && distance_value > filter_limit_min_)
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
}
else
{
// Use a threshold for cutting out points which are too close/far away
if (distance_value > filter_limit_max_ || distance_value < filter_limit_min_)
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
}
pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (input_->points[cp], output.points[nr_p]));
nr_p++;
}
output.width = nr_p;
} // !keep_organized_
}
// No distance filtering, process all data. No need to check for is_organized here as we did it above
else
{
// First pass: go over all points and insert them into the right leaf
for (size_t cp = 0; cp < input_->points.size (); ++cp)
{
// Check if the point is invalid
if (!pcl_isfinite (input_->points[cp].x) || !pcl_isfinite (input_->points[cp].y) || !pcl_isfinite (input_->points[cp].z))
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (input_->points[cp], output.points[nr_p]));
nr_p++;
}
output.width = nr_p;
}
output.points.resize (output.width * output.height);
removed_indices_->resize(nr_removed_p);
}
// Returns 0 iff no measurement taken, 1 if a valid measurement was found, -1 if observed to be empty
template <typename PointT, typename NormalT> int
cpu_tsdf::TSDFVolumeOctree::updateVoxel (
const cpu_tsdf::OctreeNode::Ptr &voxel,
const pcl::PointCloud<PointT> &cloud,
const pcl::PointCloud<NormalT> &normals,
const Eigen::Affine3f &trans_inv)
{
// assert (!voxel->hasChildren ());
if (voxel->hasChildren ())
{
std::vector<OctreeNode::Ptr>& children = voxel->getChildren ();
std::vector<bool> is_empty (children.size ());
//#pragma omp parallel for
for (size_t i = 0; i < children.size (); i++)
{
is_empty[i] = updateVoxel (children[i], cloud, normals, trans_inv) < 0;
}
bool all_are_empty = true;
for (size_t i = 0; i < is_empty.size (); i++)
all_are_empty &= is_empty[i];
if (all_are_empty)
{
children.clear (); // Remove empty voxels
}
else
{
return (1); // Say I am not empty
}
}
pcl::PointXYZ v_g_orig;
voxel->getCenter (v_g_orig.x, v_g_orig.y, v_g_orig.z);
pcl::PointXYZ v_g = pcl::transformPoint (v_g_orig, trans_inv);
if (v_g.z < min_sensor_dist_ || v_g.z > max_sensor_dist_)
return (0); // No observation
int u, v;
if (!reprojectPoint (v_g, u, v))
return (0); // No observation
const PointT &pt = cloud (u,v);
if (pcl_isnan (pt.z))
return (0); // No observation
// if (pt.z >= max_sensor_dist_) // We want to let empty points be empty, even at noisy readings
// return (0);
float d;
float w;
voxel->getData (d, w);
float d_new = (pt.z - v_g.z);
// Check if we can split
if (fabs (d_new) < 3*voxel->getMaxSize ()/4.)// || (fabs (d_new) < max_dist_))
{
float xsize, ysize, zsize;
voxel->getSize (xsize, ysize, zsize);
if (xsize > xsize_ / xres_ && ysize > ysize_ / yres_ && zsize > zsize_ / zres_)
{
std::vector<OctreeNode::Ptr>& children = voxel->split ();
std::vector<bool> is_empty (children.size ());
//#pragma omp parallel for
for (size_t i = 0; i < children.size (); i++)
{
// TODO Make the weight and distance match the parent's?
// children[i]->setData (d, w);
is_empty[i] = updateVoxel (children[i], cloud, normals, trans_inv) < 0;
}
bool all_are_empty = true;
for (size_t i = 0; i < is_empty.size (); i++)
all_are_empty &= is_empty[i];
if (all_are_empty)
{
children.clear (); // Remove empty voxel
}
else
{
return (1); // Say I am not empty
}
}
}
if (d_new > max_dist_pos_)
{
d_new = max_dist_pos_;
}
else if (d_new < -max_dist_neg_)
{
return (0); // No observation, effectively
}
// Normalize s.t. -1 = unobserved
d_new /= max_dist_neg_;
float w_new = 1;
if (weight_by_depth_)
w_new *= (1 - std::min(pt.z / 10., 1.)); // Scales s.t. at 10m it's worthless
if (weight_by_variance_ && voxel->nsample_ > 5)
w_new *= std::exp (logNormal (d_new, voxel->d_, voxel->getVariance ()));
if (integrate_color_)
voxel->addObservation (d_new, w_new, max_weight_, pt.r, pt.g, pt.b);
else
voxel->addObservation (d_new, w_new, max_weight_);
if (voxel->d_ < -0.99)
return (0);
else if (voxel->d_ < 0.99* max_dist_pos_ / max_dist_neg_) //if I am not empty
return (1); // Say so
else
return (-1); // Otherwise say I'm empty
}
template <typename PointT> int
pcl::PCDWriter::writeASCII (const std::string &file_name,
const pcl::PointCloud<PointT> &cloud,
const std::vector<int> &indices,
const int precision)
{
if (cloud.points.empty () || indices.empty ())
{
throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Input point cloud has no data or empty indices given!");
return (-1);
}
if (cloud.width * cloud.height != cloud.points.size ())
{
throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Number of points different than width * height!");
return (-1);
}
std::ofstream fs;
fs.open (file_name.c_str ()); // Open file
if (!fs.is_open () || fs.fail ())
{
throw pcl::IOException ("[pcl::PCDWriter::writeASCII] Could not open file for writing!");
return (-1);
}
fs.precision (precision);
fs.imbue (std::locale::classic ());
std::vector<sensor_msgs::PointField> fields;
pcl::getFields (cloud, fields);
// Write the header information
fs << generateHeader<PointT> (cloud, static_cast<int> (indices.size ())) << "DATA ascii\n";
std::ostringstream stream;
stream.precision (precision);
stream.imbue (std::locale::classic ());
// Iterate through the points
for (size_t i = 0; i < indices.size (); ++i)
{
for (size_t d = 0; d < fields.size (); ++d)
{
// Ignore invalid padded dimensions that are inherited from binary data
if (fields[d].name == "_")
continue;
int count = fields[d].count;
if (count == 0)
count = 1; // we simply cannot tolerate 0 counts (coming from older converter code)
for (int c = 0; c < count; ++c)
{
switch (fields[d].datatype)
{
case sensor_msgs::PointField::INT8:
{
int8_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (int8_t), sizeof (int8_t));
//if (pcl_isnan (value))
// stream << "nan";
//else
stream << boost::numeric_cast<uint32_t>(value);
break;
}
case sensor_msgs::PointField::UINT8:
{
uint8_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (uint8_t), sizeof (uint8_t));
//if (pcl_isnan (value))
// stream << "nan";
//else
stream << boost::numeric_cast<uint32_t>(value);
break;
}
case sensor_msgs::PointField::INT16:
{
int16_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (int16_t), sizeof (int16_t));
//if (pcl_isnan (value))
// stream << "nan";
//else
stream << boost::numeric_cast<int16_t>(value);
break;
}
case sensor_msgs::PointField::UINT16:
{
uint16_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (uint16_t), sizeof (uint16_t));
//if (pcl_isnan (value))
// stream << "nan";
//else
stream << boost::numeric_cast<uint16_t>(value);
break;
}
case sensor_msgs::PointField::INT32:
{
int32_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (int32_t), sizeof (int32_t));
//if (pcl_isnan (value))
// stream << "nan";
//else
stream << boost::numeric_cast<int32_t>(value);
break;
}
case sensor_msgs::PointField::UINT32:
{
uint32_t value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (uint32_t), sizeof (uint32_t));
//if (pcl_isnan (value))
// stream << "nan";
//else
stream << boost::numeric_cast<uint32_t>(value);
break;
}
case sensor_msgs::PointField::FLOAT32:
{
float value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (float), sizeof (float));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<float>(value);
break;
}
case sensor_msgs::PointField::FLOAT64:
{
double value;
memcpy (&value, reinterpret_cast<const char*> (&cloud.points[indices[i]]) + fields[d].offset + c * sizeof (double), sizeof (double));
if (pcl_isnan (value))
stream << "nan";
else
stream << boost::numeric_cast<double>(value);
break;
}
default:
PCL_WARN ("[pcl::PCDWriter::writeASCII] Incorrect field data type specified (%d)!\n", fields[d].datatype);
break;
}
if (d < fields.size () - 1 || c < static_cast<int> (fields[d].count - 1))
stream << " ";
}
}
// Copy the stream, trim it, and write it to disk
std::string result = stream.str ();
boost::trim (result);
stream.str ("");
fs << result << "\n";
}
fs.close (); // Close file
return (0);
}
