double
computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud)
{
double res = 0.0;
int n_points = 0;
int nres;
std::vector<int> indices (2);
std::vector<float> sqr_distances (2);
pcl::search::KdTree<PointType> tree;
tree.setInputCloud (cloud);
for (size_t i = 0; i < cloud->size (); ++i)
{
if (! pcl_isfinite ((*cloud)[i].x))
{
continue;
}
//Considering the second neighbor since the first is the point itself.
nres = tree.nearestKSearch (i, 2, indices, sqr_distances);
if (nres == 2)
{
res += sqrt (sqr_distances[1]);
++n_points;
}
}
if (n_points != 0)
{
res /= n_points;
}
return res;
}
void
computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud)
{
double res = 0.0;
int n_points = 0;
int nres;
std::vector<int> indices (2);
std::vector<float> sqr_distances (2);
pcl::search::KdTree<PointType> tree;
tree.setInputCloud (cloud);
for (size_t i = 0; i < cloud->size (); ++i)
{
if (! pcl_isfinite ((*cloud)[i].x))
{
continue;
}
//Considering the second neighbor since the first is the point itself.
nres = tree.nearestKSearch (i, 2, indices, sqr_distances);
if (nres == 2)
{
res += sqrt (sqr_distances[1]);
++n_points;
}
}
if (n_points != 0)
{
res /= n_points;
}
// Set up resolution invariance
//
if (use_cloud_resolution_)
{
float resolution = static_cast<float>(res);
if (resolution != 0.0f)
{
model_ss_ *= resolution;
scene_ss_ *= resolution;
rf_rad_ *= resolution;
descr_rad_ *= resolution;
cg_size_ *= resolution;
}
std::cout << "Model resolution: " << resolution << std::endl;
std::cout << "Model sampling size: " << model_ss_ << std::endl;
std::cout << "Scene sampling size: " << scene_ss_ << std::endl;
std::cout << "LRF support radius: " << rf_rad_ << std::endl;
std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl;
std::cout << "Clustering bin size: " << cg_size_ << std::endl << std::endl;
}
}
double Registrator::computeCloudResolution(const pcl::PointCloud<TYPE_Point_Sensor>::ConstPtr &cloud)
{
double res = 0.0;
int n_points = 0;
int nres;
pcl::search::KdTree<TYPE_Point_Sensor> tree;
tree.setInputCloud (cloud);
for (int iii=0; iii<cloud->size(); ++iii)
{
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
if ( !pcl_isfinite( (*cloud)[iii].x ) || /////////
!pcl_isfinite( (*cloud)[iii].y ) || /////////
!pcl_isfinite( (*cloud)[iii].z ) ) continue;
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
//std::cout << "computeCloudResolution - TYPE_Point_Sensor" << "\n"
// << (*cloud)[iii].x << "\t\t"
// << (*cloud)[iii].y << "\t\t"
// << (*cloud)[iii].z << "\t\t"
// << std::endl;
std::vector<int> indices( 2 ); // dummy
std::vector<float> sqr_distances( 2 );
//Considering the second neighbor since the first is the point itself !!!
nres = tree.nearestKSearch(iii, 2, indices, sqr_distances);
if (nres == 2)
{
res += sqrt (sqr_distances[1]);
++n_points;
}
}
//////////////////////////////////////
if (n_points != 0) res /= n_points;
//////////////////////////////////////
return res;
}
//int MeshOn2D(const std::vector<ModelT> &model_set, const std::vector<poseT> &poses, cv::Mat &map2d, float fx = FOCAL_X, float fy = FOCAL_Y);
float segAcc(const std::vector<ModelT> &model_set, const std::vector<poseT> &poses, pcl::PointCloud<PointT>::Ptr cloud)
{
pcl::PointCloud<PointT>::Ptr link_cloud(new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr node_cloud(new pcl::PointCloud<PointT>());
for( int i = 0 ; i < model_set.size() ; i++ )
{
uint32_t cur_label;
pcl::PointCloud<PointT>::Ptr cur_cloud(new pcl::PointCloud<PointT>());
if( model_set[i].model_label == "link" )
{
cur_label = 1;
cur_cloud = link_cloud;
}
else if( model_set[i].model_label == "node" )
{
cur_label = 2;
cur_cloud = node_cloud;
}
pcl::copyPointCloud(*model_set[i].model_cloud, *cur_cloud);
for( pcl::PointCloud<PointT>::iterator it = cur_cloud->begin() ; it < cur_cloud->end() ; it++ )
it->rgba = cur_label;
}
Eigen::Quaternionf calibrate_rot(Eigen::AngleAxisf(M_PI/2, Eigen::Vector3f (1, 0, 0)));
pcl::PointCloud<PointT>::Ptr all_cloud(new pcl::PointCloud<PointT>());
for(std::vector<poseT>::const_iterator it = poses.begin() ; it < poses.end() ; it++)
{
for( int i = 0 ; i < model_set.size() ; i++ )
{
if(model_set[i].model_label == it->model_name )
{
pcl::PointCloud<PointT>::Ptr cur_cloud(new pcl::PointCloud<PointT>());
if( it->model_name == "link" )
pcl::copyPointCloud(*link_cloud, *cur_cloud);
else if( it->model_name == "node" )
pcl::copyPointCloud(*node_cloud, *cur_cloud);
pcl::transformPointCloud(*cur_cloud, *cur_cloud, it->shift, it->rotation*calibrate_rot);
all_cloud->insert(all_cloud->end(), cur_cloud->begin(), cur_cloud->end());
}
}
}
pcl::search::KdTree<PointT> tree;
tree.setInputCloud (all_cloud);
uint8_t tmp_color = 255;
uint32_t red = tmp_color << 16;
uint32_t blue = tmp_color;
int pos_count = 0;
float T = 0.02*0.02;
for ( pcl::PointCloud<PointT>::iterator it = cloud->begin() ; it < cloud->end() ; it++ )
{
std::vector<int> indices (1);
std::vector<float> sqr_distances (1);
int nres = tree.nearestKSearch(*it, 1, indices, sqr_distances);
if( it->rgba > 255 )
it->rgba = 1;
else if( it->rgba > 0 )
it->rgba = 2;
else
it->rgba = 0;
if ( nres == 1 && sqr_distances[0] < T )
{
if(it->rgba == all_cloud->at(indices[0]).rgba)
pos_count++;
}
else if( it->rgba == 0 || sqr_distances[0] > T )
pos_count++;
if( nres == 1 && sqr_distances[0] < T )
{
if( all_cloud->at(indices[0]).rgba == 1 )
it->rgba = red;
else if( all_cloud->at(indices[0]).rgba == 2 )
it->rgba = blue;
}
else
it->rgba = 0;
}
return (pos_count +0.0) / cloud->size();
}
void PhotonLTEIntegrator::_ConstructIrradiancePhotonMap()
{
// Caustic map can be null.
ASSERT(mp_photon_maps->GetDirectMap() && mp_photon_maps->GetIndirectMap());
mp_irradiance_map.reset((KDTree<IrradiancePhoton>*)NULL);
const std::vector<Photon> &direct_photons = mp_photon_maps->GetDirectMap()->GetAllPoints();
const std::vector<Photon> &indirect_photons = mp_photon_maps->GetIndirectMap()->GetAllPoints();
//The method selects 10% of indirect photons as positions for irradiance photons.
std::vector<IrradiancePhoton> irradiance_photons;
irradiance_photons.reserve(indirect_photons.size() / 10);
for(size_t i=0;i<indirect_photons.size();i+=10)
irradiance_photons.push_back( IrradiancePhoton(indirect_photons[i].m_point, Spectrum_f(), Spectrum_f(), indirect_photons[i].m_normal) );
if (irradiance_photons.empty())
return;
// Estimate lookup radius so that the corresponding area will contain required number of photons (in average).
double direct_photon_area = direct_photons.empty() ? 0.0 : m_scene_total_area / direct_photons.size();
double max_direct_lookup_dist = sqrt(direct_photon_area*LOOKUP_PHOTONS_NUM_FOR_IRRADIANCE*INV_PI);
double indirect_photon_area = indirect_photons.empty() ? 0.0 : m_scene_total_area / indirect_photons.size();
double max_indirect_lookup_dist = sqrt(indirect_photon_area*LOOKUP_PHOTONS_NUM_FOR_IRRADIANCE*INV_PI);
// Not sure what's a better way to estimate caustic lookup radius.
double max_caustic_lookup_dist = std::max(max_direct_lookup_dist, max_indirect_lookup_dist);
// Compute irradiance value for each of the irradiance photons.
// We do that in multiple threads since all photons can be processed independently.
IrradiancePhotonProcess process(this, irradiance_photons, max_caustic_lookup_dist, max_direct_lookup_dist, max_indirect_lookup_dist);
tbb::parallel_for(tbb::blocked_range<size_t>(0,irradiance_photons.size()), process);
mp_irradiance_map.reset( new KDTree<IrradiancePhoton>(irradiance_photons) );
/*
Estimate maximum lookup distance for the irradiance photons.
We do this by finding the set of the most distant indirect photons to the set of irradiance photons.
The set contains 0.1% of all of the indirect photons. After we have the distances we take the smallest one from this set and use it as the maximum lookup distance.
That way we can say that for 99.9% of all points in the scene (reachable by indirect light) the nearest irradiance photon can be found within this distance.
*/
// Process only every 11th indirect photon to speed things up. It's important that this step is relatively prime with the increment step for irradiance photons above.
size_t step = 11;
// Compute min distance for every indirect photon in parallel.
std::vector<double> sqr_distances((indirect_photons.size()+step-1)/step, 0.f);
tbb::parallel_for((size_t)0, sqr_distances.size(), [&](size_t i)
{
Point3D_d photon_position = Convert<double>(indirect_photons[i*step].m_point);
Vector3D_d photon_normal = indirect_photons[i*step].m_normal.ToVector3D<double>();
IrradiancePhotonFilter filter(photon_position, photon_normal, MAX_NORMAL_DEVIATION_COS);
const IrradiancePhoton *p_irradiance_photon = mp_irradiance_map->GetNearestPoint(photon_position, filter);
if (p_irradiance_photon == NULL)
sqr_distances[i] = 0.0; // by setting it to 0 we prevent this value from affecting the final result
else
sqr_distances[i] = Vector3D_d(photon_position - Convert<double>(p_irradiance_photon->m_point)).LengthSqr();
});
// Now get the 1/1000th most distant photon to get the 0.1% percentile
size_t index = sqr_distances.size()<=1 ? 0 : std::max(sqr_distances.size()/1000, (size_t)1);
std::nth_element(sqr_distances.begin(), sqr_distances.begin()+index, sqr_distances.end(), std::greater<double>());
ASSERT(sqr_distances[index]>=0.0);
if (sqr_distances[index] == 0.0)
m_max_irradiance_lookup_dist = DBL_INF;
else
m_max_irradiance_lookup_dist = sqrt(sqr_distances[index]);
}
