template <typename PointT, typename PointNT> bool
pcl::SurfelSmoothing<PointT, PointNT>::initCompute ()
{
if (!PCLBase<PointT>::initCompute ())
return false;
if (!normals_)
{
PCL_ERROR ("SurfelSmoothing: normal cloud not set\n");
return false;
}
if (input_->points.size () != normals_->points.size ())
{
PCL_ERROR ("SurfelSmoothing: number of input points different from the number of given normals\n");
return false;
}
// Initialize the spatial locator
if (!tree_)
{
if (input_->isOrganized ())
tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
else
tree_.reset (new pcl::search::KdTree<PointT> (false));
}
// create internal copies of the input - these will be modified
interm_cloud_ = PointCloudInPtr (new PointCloudIn (*input_));
interm_normals_ = NormalCloudPtr (new NormalCloud (*normals_));
return (true);
}
template <typename PointT, typename PointNT> bool
pcl::SurfelSmoothing<PointT, PointNT>::initCompute ()
{
if (!PCLBase<PointT>::initCompute ())
return false;
if (!normals_)
{
PCL_ERROR ("SurfelSmoothing: normal cloud not set\n");
return false;
}
if (input_->points.size () != normals_->points.size ())
{
PCL_ERROR ("SurfelSmoothing: number of input points different from the number of given normals\n");
return false;
}
if (!tree_)
{
PCL_ERROR ("SurfelSmoothing: kd-tree not set\n");
return false;
}
// create internal copies of the input - these will be modified
interm_cloud_ = PointCloudInPtr (new PointCloudIn (*input_));
interm_normals_ = NormalCloudPtr (new NormalCloud (*normals_));
return true;
}
template <typename PointT, typename PointNT> void
pcl::SurfelSmoothing<PointT, PointNT>::smoothCloudIteration (PointCloudInPtr &output_positions,
NormalCloudPtr &output_normals)
{
PCL_INFO ("SurfelSmoothing: cloud smoothing iteration starting ...\n");
tree_->setInputCloud (interm_cloud_);
output_positions = PointCloudInPtr (new PointCloudIn);
output_positions->points.resize (interm_cloud_->points.size ());
output_normals = NormalCloudPtr (new NormalCloud);
output_normals->points.resize (interm_cloud_->points.size ());
std::vector<int> nn_indices;
std::vector<float> nn_distances;
std::vector<float> diffs (interm_cloud_->points.size ());
Eigen::Vector4f total_residual = Eigen::Vector4f::Zero ();
for (size_t i = 0; i < interm_cloud_->points.size (); ++i)
{
Eigen::Vector4f smoothed_point = Eigen::Vector4f::Zero ();
Eigen::Vector4f smoothed_normal = Eigen::Vector4f::Zero ();
// get neighbors
tree_->radiusSearch (i, scale_, nn_indices, nn_distances);
float theta_normalization_factor = 0.0;
Eigen::Vector4f e_residual = Eigen::Vector4f::Zero ();
for (std::vector<int>::iterator nn_index_it = nn_indices.begin (); nn_index_it != nn_indices.end (); ++nn_index_it)
{
float dist = pcl::squaredEuclideanDistance (interm_cloud_->points[i], interm_cloud_->points[*nn_index_it]);
float theta_i = exp ( (-1) * dist / scale_squared_);
theta_normalization_factor += theta_i;
smoothed_normal += theta_i * interm_normals_->points[*nn_index_it].getNormalVector4fMap ();
e_residual += theta_i * (interm_cloud_->points[i].getVector4fMap () - interm_cloud_->points[*nn_index_it].getVector4fMap ());
}
smoothed_normal /= theta_normalization_factor;
e_residual /= theta_normalization_factor;
smoothed_point = interm_cloud_->points[i].getVector4fMap () - e_residual.dot (smoothed_normal) * smoothed_normal;
/// smoothed_point = interm_cloud_->points[point_i].getVector3fMap () - e_residual;
total_residual += e_residual;
output_positions->points[i].getVector4fMap () = smoothed_point;
output_normals->points[i].getNormalVector4fMap () = smoothed_normal;
// Calculate difference
diffs[i] = smoothed_normal.dot (smoothed_point - interm_cloud_->points[i].getVector4fMap ());
}
std::cerr << "Total residual after an iteration: " << total_residual << std::endl;
PCL_INFO("SurfelSmoothing done iteration\n");
}
template <typename PointT, typename PointNT> float
pcl::SurfelSmoothing<PointT, PointNT>::smoothCloudIteration (PointCloudInPtr &output_positions,
NormalCloudPtr &output_normals)
{
// PCL_INFO ("SurfelSmoothing: cloud smoothing iteration starting ...\n");
output_positions = PointCloudInPtr (new PointCloudIn);
output_positions->points.resize (interm_cloud_->points.size ());
output_normals = NormalCloudPtr (new NormalCloud);
output_normals->points.resize (interm_cloud_->points.size ());
std::vector<int> nn_indices;
std::vector<float> nn_distances;
std::vector<float> diffs (interm_cloud_->points.size ());
float total_residual = 0.0f;
for (size_t i = 0; i < interm_cloud_->points.size (); ++i)
{
Eigen::Vector4f smoothed_point = Eigen::Vector4f::Zero ();
Eigen::Vector4f smoothed_normal = Eigen::Vector4f::Zero ();
// get neighbors
// @todo using 5x the scale for searching instead of all the points to avoid O(N^2)
tree_->radiusSearch (interm_cloud_->points[i], 5*scale_, nn_indices, nn_distances);
float theta_normalization_factor = 0.0;
std::vector<float> theta (nn_indices.size ());
for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
{
float dist = pcl::squaredEuclideanDistance (interm_cloud_->points[i], input_->points[nn_indices[nn_index_i]]);//interm_cloud_->points[nn_indices[nn_index_i]]);
float theta_i = exp ( (-1) * dist / scale_squared_);
theta_normalization_factor += theta_i;
smoothed_normal += theta_i * interm_normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap ();
theta[nn_index_i] = theta_i;
}
smoothed_normal /= theta_normalization_factor;
smoothed_normal(3) = 0.0f;
smoothed_normal.normalize ();
// find minimum along the normal
float e_residual;
smoothed_point = interm_cloud_->points[i].getVector4fMap ();
while (1)
{
e_residual = 0.0f;
smoothed_point(3) = 0.0f;
for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i)
{
Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();//interm_cloud_->points[nn_indices[nn_index_i]].getVector4fMap ();
neighbor(3) = 0.0f;
float dot_product = smoothed_normal.dot (neighbor - smoothed_point);
e_residual += theta[nn_index_i] * dot_product;// * dot_product;
}
e_residual /= theta_normalization_factor;
if (e_residual < 1e-5) break;
smoothed_point = smoothed_point + e_residual * smoothed_normal;
}
total_residual += e_residual;
output_positions->points[i].getVector4fMap () = smoothed_point;
output_normals->points[i].getNormalVector4fMap () = normals_->points[i].getNormalVector4fMap ();//smoothed_normal;
}
// std::cerr << "Total residual after iteration: " << total_residual << std::endl;
// PCL_INFO("SurfelSmoothing done iteration\n");
return total_residual;
}
