template <typename PointInT, typename PointOutT> void
pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeatureFull (const float *distanceMap,
const float &bad_point,
PointCloudOut &output)
{
unsigned index = 0;
if (border_policy_ == BORDER_POLICY_IGNORE)
{
// Set all normals that we do not touch to NaN
// top and bottom borders
// That sets the output density to false!
output.is_dense = false;
unsigned border = int(normal_smoothing_size_);
PointOutT* vec1 = &output [0];
PointOutT* vec2 = vec1 + input_->width * (input_->height - border);
size_t count = border * input_->width;
for (size_t idx = 0; idx < count; ++idx)
{
vec1 [idx].getNormalVector3fMap ().setConstant (bad_point);
vec1 [idx].curvature = bad_point;
vec2 [idx].getNormalVector3fMap ().setConstant (bad_point);
vec2 [idx].curvature = bad_point;
}
// left and right borders actually columns
vec1 = &output [border * input_->width];
vec2 = vec1 + input_->width - border;
for (size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
{
for (size_t ci = 0; ci < border; ++ci)
{
vec1 [ci].getNormalVector3fMap ().setConstant (bad_point);
vec1 [ci].curvature = bad_point;
vec2 [ci].getNormalVector3fMap ().setConstant (bad_point);
vec2 [ci].curvature = bad_point;
}
}
if (use_depth_dependent_smoothing_)
{
index = border + input_->width * border;
unsigned skip = (border << 1);
for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
{
for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
{
index = ri * input_->width + ci;
const float depth = input_->points[index].z;
if (!pcl_isfinite (depth))
{
output[index].getNormalVector3fMap ().setConstant (bad_point);
output[index].curvature = bad_point;
continue;
}
float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
if (smoothing > 2.0f)
{
setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
computePointNormal (ci, ri, index, output [index]);
}
else
{
output[index].getNormalVector3fMap ().setConstant (bad_point);
output[index].curvature = bad_point;
}
}
}
}
else
{
float smoothing_constant = normal_smoothing_size_;
index = border + input_->width * border;
unsigned skip = (border << 1);
for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
{
for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
{
index = ri * input_->width + ci;
if (!pcl_isfinite (input_->points[index].z))
{
output [index].getNormalVector3fMap ().setConstant (bad_point);
output [index].curvature = bad_point;
continue;
}
float smoothing = (std::min)(distanceMap[index], smoothing_constant);
if (smoothing > 2.0f)
{
setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
computePointNormal (ci, ri, index, output [index]);
}
else
{
output [index].getNormalVector3fMap ().setConstant (bad_point);
output [index].curvature = bad_point;
}
}
}
}
}
else if (border_policy_ == BORDER_POLICY_MIRROR)
{
output.is_dense = false;
if (use_depth_dependent_smoothing_)
{
//index = 0;
//unsigned skip = 0;
//for (unsigned ri = 0; ri < input_->height; ++ri, index += skip)
for (unsigned ri = 0; ri < input_->height; ++ri)
{
//for (unsigned ci = 0; ci < input_->width; ++ci, ++index)
for (unsigned ci = 0; ci < input_->width; ++ci)
{
index = ri * input_->width + ci;
const float depth = input_->points[index].z;
if (!pcl_isfinite (depth))
{
output[index].getNormalVector3fMap ().setConstant (bad_point);
output[index].curvature = bad_point;
continue;
}
float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
if (smoothing > 2.0f)
{
setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
computePointNormalMirror (ci, ri, index, output [index]);
}
else
{
output[index].getNormalVector3fMap ().setConstant (bad_point);
output[index].curvature = bad_point;
}
}
}
}
else
{
float smoothing_constant = normal_smoothing_size_;
//index = border + input_->width * border;
//unsigned skip = (border << 1);
//for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
for (unsigned ri = 0; ri < input_->height; ++ri)
{
//for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
for (unsigned ci = 0; ci < input_->width; ++ci)
{
index = ri * input_->width + ci;
if (!pcl_isfinite (input_->points[index].z))
{
output [index].getNormalVector3fMap ().setConstant (bad_point);
output [index].curvature = bad_point;
continue;
}
float smoothing = (std::min)(distanceMap[index], smoothing_constant);
if (smoothing > 2.0f)
{
setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
computePointNormalMirror (ci, ri, index, output [index]);
}
else
{
output [index].getNormalVector3fMap ().setConstant (bad_point);
output [index].curvature = bad_point;
}
}
}
}
}
}
template <typename PointInT, typename PointOutT> void
pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
output.sensor_origin_ = input_->sensor_origin_;
output.sensor_orientation_ = input_->sensor_orientation_;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
// compute depth-change map
unsigned char * depthChangeMap = new unsigned char[input_->points.size ()];
memset (depthChangeMap, 255, input_->points.size ());
unsigned index = 0;
for (unsigned int ri = 0; ri < input_->height-1; ++ri)
{
for (unsigned int ci = 0; ci < input_->width-1; ++ci, ++index)
{
index = ri * input_->width + ci;
// const float depth = (*input_)(ci, ri ).z;
// const float depthR = (*input_)(ci + 1, ri ).z;
// const float depthD = (*input_)(ci, ri + 1).z;
const float depth = input_->points [index].z;
const float depthR = input_->points [index + 1].z;
const float depthD = input_->points [index + input_->width].z;
//const float depthDependendDepthChange = (max_depth_change_factor_ * (fabs(depth)+1.0f))/(500.0f*0.001f);
const float depthDependendDepthChange = (max_depth_change_factor_ * (fabsf (depth) + 1.0f) * 2.0f);
if (fabs (depth - depthR) > depthDependendDepthChange
|| !pcl_isfinite (depth) || !pcl_isfinite (depthR))
{
depthChangeMap[index] = 0;
depthChangeMap[index+1] = 0;
}
if (fabs (depth - depthD) > depthDependendDepthChange
|| !pcl_isfinite (depth) || !pcl_isfinite (depthD))
{
depthChangeMap[index] = 0;
depthChangeMap[index + input_->width] = 0;
}
}
}
// compute distance map
//float *distanceMap = new float[input_->points.size ()];
if (distance_map_ != NULL) delete distance_map_;
distance_map_ = new float[input_->points.size ()];
float *distanceMap = distance_map_;
for (size_t index = 0; index < input_->points.size (); ++index)
{
if (depthChangeMap[index] == 0)
distanceMap[index] = 0.0f;
else
distanceMap[index] = static_cast<float> (input_->width + input_->height);
}
// first pass
float* previous_row = distanceMap;
float* current_row = previous_row + input_->width;
for (size_t ri = 1; ri < input_->height; ++ri)
{
for (size_t ci = 1; ci < input_->width; ++ci)
{
const float upLeft = previous_row [ci - 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci-1] + 1.4f;
const float up = previous_row [ci] + 1.0f; //distanceMap[(ri-1)*input_->width + ci] + 1.0f;
const float upRight = previous_row [ci + 1] + 1.4f; //distanceMap[(ri-1)*input_->width + ci+1] + 1.4f;
const float left = current_row [ci - 1] + 1.0f; //distanceMap[ri*input_->width + ci-1] + 1.0f;
const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
const float minValue = std::min (std::min (upLeft, up), std::min (left, upRight));
if (minValue < center)
current_row [ci] = minValue; //distanceMap[ri * input_->width + ci] = minValue;
}
previous_row = current_row;
current_row += input_->width;
}
float* next_row = distanceMap + input_->width * (input_->height - 1);
current_row = next_row - input_->width;
// second pass
for (int ri = input_->height-2; ri >= 0; --ri)
{
for (int ci = input_->width-2; ci >= 0; --ci)
{
const float lowerLeft = next_row [ci - 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci-1] + 1.4f;
const float lower = next_row [ci] + 1.0f; //distanceMap[(ri+1)*input_->width + ci] + 1.0f;
const float lowerRight = next_row [ci + 1] + 1.4f; //distanceMap[(ri+1)*input_->width + ci+1] + 1.4f;
const float right = current_row [ci + 1] + 1.0f; //distanceMap[ri*input_->width + ci+1] + 1.0f;
const float center = current_row [ci]; //distanceMap[ri*input_->width + ci];
const float minValue = std::min (std::min (lowerLeft, lower), std::min (right, lowerRight));
if (minValue < center)
current_row [ci] = minValue; //distanceMap[ri*input_->width + ci] = minValue;
}
next_row = current_row;
current_row -= input_->width;
}
// Set all normals that we do not touch to NaN
// top and bottom borders
// That sets the output density to false!
output.is_dense = false;
unsigned border = int(normal_smoothing_size_);
PointOutT* vec1 = &output [0];
PointOutT* vec2 = vec1 + input_->width * (input_->height - border);
size_t count = border * input_->width;
for (size_t idx = 0; idx < count; ++idx)
{
vec1 [idx].getNormalVector4fMap ().setConstant (bad_point);
vec1 [idx].curvature = bad_point;
vec2 [idx].getNormalVector4fMap ().setConstant (bad_point);
vec2 [idx].curvature = bad_point;
}
// left and right borders actually columns
vec1 = &output [border * input_->width];
vec2 = vec1 + input_->width - border;
for (size_t ri = border; ri < input_->height - border; ++ri, vec1 += input_->width, vec2 += input_->width)
{
for (size_t ci = 0; ci < border; ++ci)
{
vec1 [ci].getNormalVector4fMap ().setConstant (bad_point);
vec1 [ci].curvature = bad_point;
vec2 [ci].getNormalVector4fMap ().setConstant (bad_point);
vec2 [ci].curvature = bad_point;
}
}
if (use_depth_dependent_smoothing_)
{
index = border + input_->width * border;
unsigned skip = (border << 1);
for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
{
for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
{
index = ri * input_->width + ci;
const float depth = input_->points[index].z;
if (!pcl_isfinite (depth))
{
output[index].getNormalVector4fMap ().setConstant (bad_point);
output[index].curvature = bad_point;
continue;
}
float smoothing = (std::min)(distanceMap[index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
if (smoothing > 2.0f)
{
setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
computePointNormal (ci, ri, index, output [index]);
}
else
{
output[index].getNormalVector4fMap ().setConstant (bad_point);
output[index].curvature = bad_point;
}
}
}
}
else
{
float smoothing_constant = normal_smoothing_size_;
index = border + input_->width * border;
unsigned skip = (border << 1);
for (unsigned ri = border; ri < input_->height - border; ++ri, index += skip)
{
for (unsigned ci = border; ci < input_->width - border; ++ci, ++index)
{
index = ri * input_->width + ci;
if (!pcl_isfinite (input_->points[index].z))
{
output [index].getNormalVector4fMap ().setConstant (bad_point);
output [index].curvature = bad_point;
continue;
}
float smoothing = (std::min)(distanceMap[index], smoothing_constant);
if (smoothing > 2.0f)
{
setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
computePointNormal (ci, ri, index, output [index]);
}
else
{
output [index].getNormalVector4fMap ().setConstant (bad_point);
output [index].curvature = bad_point;
}
}
}
}
delete[] depthChangeMap;
//delete[] distanceMap;
}
template <typename PointInT, typename PointOutT> void
pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computeFeaturePart (const float *distanceMap,
const float &bad_point,
PointCloudOut &output)
{
if (border_policy_ == BORDER_POLICY_IGNORE)
{
output.is_dense = false;
unsigned border = int(normal_smoothing_size_);
unsigned bottom = input_->height > border ? input_->height - border : 0;
unsigned right = input_->width > border ? input_->width - border : 0;
if (use_depth_dependent_smoothing_)
{
// Iterating over the entire index vector
for (std::size_t idx = 0; idx < indices_->size (); ++idx)
{
unsigned pt_index = (*indices_)[idx];
unsigned u = pt_index % input_->width;
unsigned v = pt_index / input_->width;
if (v < border || v > bottom)
{
output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
output.points[idx].curvature = bad_point;
continue;
}
if (u < border || v > right)
{
output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
output.points[idx].curvature = bad_point;
continue;
}
const float depth = input_->points[pt_index].z;
if (!pcl_isfinite (depth))
{
output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
output.points[idx].curvature = bad_point;
continue;
}
float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
if (smoothing > 2.0f)
{
setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
computePointNormal (u, v, pt_index, output [idx]);
}
else
{
output[idx].getNormalVector3fMap ().setConstant (bad_point);
output[idx].curvature = bad_point;
}
}
}
else
{
float smoothing_constant = normal_smoothing_size_;
// Iterating over the entire index vector
for (std::size_t idx = 0; idx < indices_->size (); ++idx)
{
unsigned pt_index = (*indices_)[idx];
unsigned u = pt_index % input_->width;
unsigned v = pt_index / input_->width;
if (v < border || v > bottom)
{
output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
output.points[idx].curvature = bad_point;
continue;
}
if (u < border || v > right)
{
output.points[idx].getNormalVector3fMap ().setConstant (bad_point);
output.points[idx].curvature = bad_point;
continue;
}
if (!pcl_isfinite (input_->points[pt_index].z))
{
output [idx].getNormalVector3fMap ().setConstant (bad_point);
output [idx].curvature = bad_point;
continue;
}
float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
if (smoothing > 2.0f)
{
setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
computePointNormal (u, v, pt_index, output [idx]);
}
else
{
output [pt_index].getNormalVector3fMap ().setConstant (bad_point);
output [pt_index].curvature = bad_point;
}
}
}
}// border_policy_ == BORDER_POLICY_IGNORE
else if (border_policy_ == BORDER_POLICY_MIRROR)
{
output.is_dense = false;
if (use_depth_dependent_smoothing_)
{
for (std::size_t idx = 0; idx < indices_->size (); ++idx)
{
unsigned pt_index = (*indices_)[idx];
unsigned u = pt_index % input_->width;
unsigned v = pt_index / input_->width;
const float depth = input_->points[pt_index].z;
if (!pcl_isfinite (depth))
{
output[idx].getNormalVector3fMap ().setConstant (bad_point);
output[idx].curvature = bad_point;
continue;
}
float smoothing = (std::min)(distanceMap[pt_index], normal_smoothing_size_ + static_cast<float>(depth)/10.0f);
if (smoothing > 2.0f)
{
setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
computePointNormalMirror (u, v, pt_index, output [idx]);
}
else
{
output[idx].getNormalVector3fMap ().setConstant (bad_point);
output[idx].curvature = bad_point;
}
}
}
else
{
float smoothing_constant = normal_smoothing_size_;
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
unsigned pt_index = (*indices_)[idx];
unsigned u = pt_index % input_->width;
unsigned v = pt_index / input_->width;
if (!pcl_isfinite (input_->points[pt_index].z))
{
output [idx].getNormalVector3fMap ().setConstant (bad_point);
output [idx].curvature = bad_point;
continue;
}
float smoothing = (std::min)(distanceMap[pt_index], smoothing_constant);
if (smoothing > 2.0f)
{
setRectSize (static_cast<int> (smoothing), static_cast<int> (smoothing));
computePointNormalMirror (u, v, pt_index, output [idx]);
}
else
{
output [idx].getNormalVector3fMap ().setConstant (bad_point);
output [idx].curvature = bad_point;
}
}
}
} // border_policy_ == BORDER_POLICY_MIRROR
}
