linear_least_squares_normal.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef PCL_FEATURES_LINEAR_LEAST_SQUARES_NORMAL_HPP_
#define PCL_FEATURES_LINEAR_LEAST_SQUARES_NORMAL_HPP_
#define EIGEN_II_METHOD 1
#include <pcl/features/linear_least_squares_normal.h>
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT>
pcl::LinearLeastSquaresNormalEstimation<PointInT, PointOutT>::~LinearLeastSquaresNormalEstimation ()
{
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::LinearLeastSquaresNormalEstimation<PointInT, PointOutT>::computePointNormal (
const int pos_x, const int pos_y, PointOutT &normal)
{
const float bad_point = std::numeric_limits<float>::quiet_NaN ();
const int width = input_->width;
const int height = input_->height;
const int x = pos_x;
const int y = pos_y;
const int index = y * width + x;
const float px = (*input_)[index].x;
const float py = (*input_)[index].y;
const float pz = (*input_)[index].z;
if (std::isnan (px))
{
normal.normal_x = bad_point;
normal.normal_y = bad_point;
normal.normal_z = bad_point;
normal.curvature = bad_point;
return;
}
float smoothingSize = normal_smoothing_size_;
if (use_depth_dependent_smoothing_) smoothingSize = smoothingSize*(pz+0.5f);
const int smoothingSizeInt = static_cast<int> (smoothingSize);
float matA0 = 0.0f;
float matA1 = 0.0f;
float matA3 = 0.0f;
float vecb0 = 0.0f;
float vecb1 = 0.0f;
for (int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt)
{
for (int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt)
{
if (u < 0 || u >= width || v < 0 || v >= height) continue;
const int index2 = v * width + u;
const float qx = (*input_)[index2].x;
const float qy = (*input_)[index2].y;
const float qz = (*input_)[index2].z;
if (std::isnan (qx)) continue;
const float delta = qz - pz;
const float i = qx - px;
const float j = qy - py;
float depthChangeThreshold = pz*pz * 0.05f * max_depth_change_factor_;
if (use_depth_dependent_smoothing_) depthChangeThreshold *= pz;
const float f = std::fabs (delta) > depthChangeThreshold ? 0 : 1;
matA0 += f * i * i;
matA1 += f * i * j;
matA3 += f * j * j;
vecb0 += f * i * delta;
vecb1 += f * j * delta;
}
}
const float det = matA0 * matA3 - matA1 * matA1;
const float ddx = matA3 * vecb0 - matA1 * vecb1;
const float ddy = -matA1 * vecb0 + matA0 * vecb1;
const float nx = ddx;
const float ny = ddy;
const float nz = -det * pz;
const float length = nx * nx + ny * ny + nz * nz;
if (length <= 0.01f)
{
normal.normal_x = bad_point;
normal.normal_y = bad_point;
normal.normal_z = bad_point;
normal.curvature = bad_point;
}
else
{
const float normInv = 1.0f / std::sqrt (length);
normal.normal_x = -nx * normInv;
normal.normal_y = -ny * normInv;
normal.normal_z = -nz * normInv;
normal.curvature = bad_point;
}
return;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::LinearLeastSquaresNormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
const float bad_point = std::numeric_limits<float>::quiet_NaN ();
const int width = input_->width;
const int height = input_->height;
// we compute the normals as follows:
// ----------------------------------
//
// for the depth-gradient you can make the following first-order Taylor approximation:
// D(x + dx) - D(x) = dx^T \Delta D + h.o.t.
//
// build linear system by stacking up equation for 8 neighbor points:
// Y = X \Delta D
//
// => \Delta D = (X^T X)^{-1} X^T Y
// => \Delta D = (A)^{-1} b
//const float smoothingSize = 30.0f;
for (int y = 0; y < height; ++y)
{
for (int x = 0; x < width; ++x)
{
const int index = y * width + x;
const float px = (*input_)[index].x;
const float py = (*input_)[index].y;
const float pz = (*input_)[index].z;
if (std::isnan(px)) continue;
//float depthDependentSmoothingSize = smoothingSize + pz / 10.0f;
float smoothingSize = normal_smoothing_size_;
//if (use_depth_dependent_smoothing_) smoothingSize *= pz;
//if (use_depth_dependent_smoothing_) smoothingSize += pz*5;
//if (smoothingSize < 1.0f) smoothingSize += 1.0f;
const int smoothingSizeInt = static_cast<int>(smoothingSize);
float matA0 = 0.0f;
float matA1 = 0.0f;
float matA3 = 0.0f;
float vecb0 = 0.0f;
float vecb1 = 0.0f;
for (int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt)
{
for (int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt)
{
if (u < 0 || u >= width || v < 0 || v >= height) continue;
const int index2 = v * width + u;
const float qx = (*input_)[index2].x;
const float qy = (*input_)[index2].y;
const float qz = (*input_)[index2].z;
if (std::isnan(qx)) continue;
const float delta = qz - pz;
const float i = qx - px;
const float j = qy - py;
const float depthDependendDepthChange = (max_depth_change_factor_ * (std::abs (pz) + 1.0f) * 2.0f);
const float f = std::fabs(delta) > depthDependendDepthChange ? 0 : 1;
//float f = std::abs(delta) > (pz * 0.05f - 0.3f) ? 0 : 1;
//const float f = std::abs(delta) > (pz*pz * 0.05f * max_depth_change_factor_) ? 0 : 1;
//float f = Math.Abs(delta) > (depth * Math.Log(depth + 1.0) * 0.02f - 0.2f) ? 0 : 1;
matA0 += f * i * i;
matA1 += f * i * j;
matA3 += f * j * j;
vecb0 += f * i * delta;
vecb1 += f * j * delta;
}
}
const float det = matA0 * matA3 - matA1 * matA1;
const float ddx = matA3 * vecb0 - matA1 * vecb1;
const float ddy = -matA1 * vecb0 + matA0 * vecb1;
const float nx = ddx;
const float ny = ddy;
const float nz = -det * pz;
const float length = nx * nx + ny * ny + nz * nz;
if (length <= 0.0f)
{
output[index].normal_x = bad_point;
output[index].normal_y = bad_point;
output[index].normal_z = bad_point;
output[index].curvature = bad_point;
}
else
{
const float normInv = 1.0f / std::sqrt (length);
output[index].normal_x = nx * normInv;
output[index].normal_y = ny * normInv;
output[index].normal_z = nz * normInv;
output[index].curvature = bad_point;
}
}
}
}
#define PCL_INSTANTIATE_LinearLeastSquaresNormalEstimation(T,NT) template class PCL_EXPORTS pcl::LinearLeastSquaresNormalEstimation<T,NT>;
//#define LinearLeastSquaresNormalEstimation(T,NT) template class PCL_EXPORTS pcl::LinearLeastSquaresNormalEstimation<T,NT>;
#endif
pcl::LinearLeastSquaresNormalEstimation::computePointNormal
void computePointNormal(const int pos_x, const int pos_y, PointOutT &normal)
Computes the normal at the specified position.
Definition: linear_least_squares_normal.hpp:53
pcl::LinearLeastSquaresNormalEstimation::~LinearLeastSquaresNormalEstimation
~LinearLeastSquaresNormalEstimation()
Destructor.
Definition: linear_least_squares_normal.hpp:47
pcl::LinearLeastSquaresNormalEstimation::computeFeature
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud.
Definition: linear_least_squares_normal.hpp:155
pcl::PointCloud< PointOutT >