Skip to content

Commit

Permalink
create MLSResult::projectPoint and MLSResult::projectQueryPoint
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Sep 12, 2017
1 parent 396bbd7 commit ccc98d9
Show file tree
Hide file tree
Showing 2 changed files with 101 additions and 137 deletions.
167 changes: 64 additions & 103 deletions surface/include/pcl/surface/impl/mls.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,35 +186,8 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
{
case (NONE):
{
MLSResult::MLSProjectionResults proj;
proj.normal = mls_result.plane_normal;
proj.point = mls_result.mean;
if (polynomial_fit_)
{
if (projection_method_ == SIMPLE)
{
if (mls_result.num_neighbors >= nr_coeff_ && pcl_isfinite (mls_result.c_vec[0]))
{
proj.point += (mls_result.c_vec[0] * mls_result.plane_normal);

// Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
if (compute_normals_)
{
proj.normal = mls_result.plane_normal - mls_result.c_vec[order_ + 1] * mls_result.u_axis - mls_result.c_vec[1] * mls_result.v_axis;
proj.normal.normalize();
}
}
}
else if (projection_method_ == ORTHOGONAL)
{
double u, v, w;
mls_result.getMLSCoordinates (mls_result.query_point, u, v ,w);
proj = mls_result.projectPointOrthogonalToPolynomialSurface (u, v, w);
}
}

MLSResult::MLSProjectionResults proj = mls_result.projectQueryPoint(projection_method_, compute_normals_, nr_coeff_);
addProjectedPointNormal(index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);

break;
}

Expand All @@ -225,9 +198,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
for (float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
if (u_disp*u_disp + v_disp*v_disp < upsampling_radius_*upsampling_radius_)
{
MLSResult::MLSProjectionResults proj;
proj = mls_result.projectPointSimpleToPolynomialSurface(u_disp, v_disp);

MLSResult::MLSProjectionResults proj = mls_result.projectPointSimpleToPolynomialSurface(u_disp, v_disp);
addProjectedPointNormal(index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
}
break;
Expand All @@ -242,35 +213,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
if (num_points_to_add <= 0)
{
// Just add the current point
MLSResult::MLSProjectionResults proj;
proj.normal = mls_result.plane_normal;
proj.point = mls_result.mean;
if (polynomial_fit_)
{
if (projection_method_ == SIMPLE)
{
if (mls_result.num_neighbors >= nr_coeff_ && pcl_isfinite (mls_result.c_vec[0]))
{
// Projection onto MLS surface along Darboux normal to the height at (0,0)
proj.point += (mls_result.c_vec[0] * mls_result.plane_normal);

// Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
if (compute_normals_)
{
proj.normal = mls_result.plane_normal - mls_result.c_vec[order_ + 1] * mls_result.u_axis - mls_result.c_vec[1] * mls_result.v_axis;
proj.normal.normalize();
}
}
}
else if (projection_method_ == ORTHOGONAL)
{
double u, v, w;
mls_result.getMLSCoordinates (mls_result.query_point, u, v, w);
proj = mls_result.projectPointOrthogonalToPolynomialSurface (u, v, w);
break;
}
}

MLSResult::MLSProjectionResults proj = mls_result.projectQueryPoint(projection_method_, compute_normals_, nr_coeff_);
addProjectedPointNormal(index, proj.point, proj.normal, mls_result.curvature, projected_points, projected_points_normals, corresponding_input_indices);
}
else
Expand Down Expand Up @@ -480,24 +423,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &
continue;

Eigen::Vector3d add_point = distinct_cloud_->points[dp_i].getVector3fMap ().template cast<double> ();
double u, v, w;
mls_results_[input_index].getMLSCoordinates (add_point, u, v, w);

MLSResult::MLSProjectionResults proj;
if (polynomial_fit_ && mls_results_[input_index].num_neighbors >= 5 * nr_coeff_)
{
if (projection_method_ == SIMPLE)
proj = mls_results_[input_index].projectPointSimpleToPolynomialSurface (u, v);
else if (projection_method_ == ORTHOGONAL)
proj = mls_results_[input_index].projectPointOrthogonalToPolynomialSurface (u, v, w);
else
proj = mls_results_[input_index].projectPointToMLSPlane (u, v);
}
else
{
proj = mls_results_[input_index].projectPointToMLSPlane (u, v);
}

MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_, 5 * nr_coeff_);
addProjectedPointNormal(input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
}
}
Expand Down Expand Up @@ -534,24 +460,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &
continue;

Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();
double u, v, w;
mls_results_[input_index].getMLSCoordinates (add_point, u, v, w);

MLSResult::MLSProjectionResults proj;
if (polynomial_fit_ && mls_results_[input_index].num_neighbors >= 5 * nr_coeff_)
{
if (projection_method_ == SIMPLE)
proj = mls_results_[input_index].projectPointSimpleToPolynomialSurface (u, v);
else if (projection_method_ == ORTHOGONAL)
proj = mls_results_[input_index].projectPointOrthogonalToPolynomialSurface (u, v, w);
else
proj = mls_results_[input_index].projectPointToMLSPlane (u, v);
}
else
{
proj = mls_results_[input_index].projectPointToMLSPlane (u, v);
}

MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint(add_point, projection_method_, 5 * nr_coeff_);
addProjectedPointNormal(input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);
}
}
Expand All @@ -566,10 +475,9 @@ pcl::MLSResult::MLSResult (const Eigen::Vector3d &a_query_point,
const Eigen::VectorXd &a_c_vec,
const int a_num_neighbors,
const float a_curvature,
const int a_order,
const bool a_polynomial_fit):
const int a_order):
query_point (a_query_point), mean (a_mean), plane_normal (a_plane_normal), u_axis (a_u), v_axis (a_v), c_vec (a_c_vec), num_neighbors (a_num_neighbors),
curvature (a_curvature), order (a_order), polynomial_fit(a_polynomial_fit), valid (true)
curvature (a_curvature), order (a_order), valid (true)
{
}

Expand Down Expand Up @@ -662,7 +570,7 @@ Eigen::Vector2f pcl::MLSResult::calculatePrincipleCurvatures (const double u, co
// Then:
// k1 = H + sqrt(H^2 - K)
// k1 = H - sqrt(H^2 - K)
if (polynomial_fit && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0]))
if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0]))
{
PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v);
double Z = 1 + d.z_u * d.z_u + d.z_v * d.z_v;
Expand Down Expand Up @@ -693,7 +601,7 @@ pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectPointOrthogonalToPol

MLSProjectionResults result;
result.normal = plane_normal;
if (polynomial_fit && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0]))
if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0]))
{
PolynomialPartialDerivative d = getPolynomialPartialDerivative (gu, gv);
gw = d.z;
Expand Down Expand Up @@ -765,7 +673,7 @@ pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectPointSimpleToPolynom

result.normal = plane_normal;

if (polynomial_fit && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0]))
if (order > 1 && c_vec.size () >= (order + 1) * (order + 2) / 2 && pcl_isfinite (c_vec[0]))
{
PolynomialPartialDerivative d = getPolynomialPartialDerivative (u, v);
w = d.z;
Expand All @@ -778,6 +686,60 @@ pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectPointSimpleToPolynom
return result;
}

pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors) const
{
double u, v, w;
getMLSCoordinates (pt, u, v, w);

MLSResult::MLSProjectionResults proj;
if (order > 1 && num_neighbors >= required_neighbors && (method == SIMPLE || method == ORTHOGONAL))
{
if (method == ORTHOGONAL)
proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
else // SIMPLE
proj = projectPointSimpleToPolynomialSurface (u, v);
}
else
{
proj = projectPointToMLSPlane (u, v);
}

return proj;
}

pcl::MLSResult::MLSProjectionResults pcl::MLSResult::projectQueryPoint(ProjectionMethod method, bool compute_normal, int required_neighbors) const
{
MLSResult::MLSProjectionResults proj;
if (order > 1 && num_neighbors >= required_neighbors && pcl_isfinite (c_vec[0]) && (method == SIMPLE || method == ORTHOGONAL))
{
if (method == ORTHOGONAL)
{
double u, v, w;
getMLSCoordinates (query_point, u, v, w);
proj = projectPointOrthogonalToPolynomialSurface (u, v, w);
}
else // SIMPLE
{
// Projection onto MLS surface along Darboux normal to the height at (0,0)
proj.point += (c_vec[0] * plane_normal);

// Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
if (compute_normal)
{
proj.normal = plane_normal - c_vec[order + 1] * u_axis - c_vec[1] * v_axis;
proj.normal.normalize();
}
}
}
else
{
proj.normal = plane_normal;
proj.point = mean;
}

return proj;
}

template <typename PointT>
void pcl::computeMLSSurface (const pcl::PointCloud<PointT> &cloud,
int index,
Expand Down Expand Up @@ -826,7 +788,6 @@ void pcl::computeMLSSurface (const pcl::PointCloud<PointT> &cloud,
// Perform polynomial fit to update point and normal
////////////////////////////////////////////////////
mls_result.num_neighbors = static_cast<int> (nn_indices.size ());
mls_result.polynomial_fit = polynomial_fit;
if (polynomial_fit)
{
mls_result.order = polynomial_order;
Expand Down
71 changes: 37 additions & 34 deletions surface/include/pcl/surface/mls.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ namespace pcl
/** \brief Data structure used to store the results of the MLS fitting */
struct MLSResult
{
enum ProjectionMethod {SIMPLE, ORTHOGONAL};

/** \brief Data structure used to store the MLS polynomial partial derivatives */
struct PolynomialPartialDerivative
Expand Down Expand Up @@ -91,8 +92,7 @@ namespace pcl
const Eigen::VectorXd &a_c_vec,
const int a_num_neighbors,
const float a_curvature,
const int a_order,
const bool a_polynomial_fit);
const int a_order);

/** \brief Given a point calculate it's 3D location in the MLS frame.
* \param[in] pt The point
Expand Down Expand Up @@ -131,31 +131,36 @@ namespace pcl
*/
Eigen::Vector2f calculatePrincipleCurvatures (const double u, const double v) const;

/** \brief Project a point orthogonal to the polynomial surface.
* \param[in] u The u-coordinate of the point in local MLS frame.
* \param[in] v The v-coordinate of the point in local MLS frame.
* \param[in] w The w-coordinate of the point in local MLS frame.
* \return The MLSProjectionResults for the input data.
* \note If the MLSResults does not contain polynomial data it projects the point onto the mls plane.
* \note If the optimization diverges it performs a simple projection on to the polynomial surface.
* \note This was implemented based on this https://math.stackexchange.com/questions/1497093/shortest-distance-between-point-and-surface
*/
MLSProjectionResults projectPointOrthogonalToPolynomialSurface (const double u, const double v, const double w) const;

/** \brief Project a point onto the MLS plane.
* \param[in] u The u-coordinate of the point in local MLS frame.
* \param[in] v The v-coordinate of the point in local MLS frame.
* \return The MLSProjectionResults for the input data.
*/
MLSProjectionResults projectPointToMLSPlane (const double u, const double v) const;

/** \brief Project a point along the MLS plane normal to the polynomial surface.
* \param[in] u The u-coordinate of the point in local MLS frame.
* \param[in] v The v-coordinate of the point in local MLS frame.
* \return The MLSProjectionResults for the input data.
* \note If the MLSResults does not contain polynomial data it projects the point onto the mls plane.
*/
MLSProjectionResults projectPointSimpleToPolynomialSurface (const double u, const double v) const;
/** \brief Project a point orthogonal to the polynomial surface.
* \param[in] u The u-coordinate of the point in local MLS frame.
* \param[in] v The v-coordinate of the point in local MLS frame.
* \param[in] w The w-coordinate of the point in local MLS frame.
* \return The MLSProjectionResults for the input data.
* \note If the MLSResults does not contain polynomial data it projects the point onto the mls plane.
* \note If the optimization diverges it performs a simple projection on to the polynomial surface.
* \note This was implemented based on this https://math.stackexchange.com/questions/1497093/shortest-distance-between-point-and-surface
*/
MLSProjectionResults projectPointOrthogonalToPolynomialSurface (const double u, const double v, const double w) const;

/** \brief Project a point onto the MLS plane.
* \param[in] u The u-coordinate of the point in local MLS frame.
* \param[in] v The v-coordinate of the point in local MLS frame.
* \return The MLSProjectionResults for the input data.
*/
MLSProjectionResults projectPointToMLSPlane (const double u, const double v) const;

/** \brief Project a point along the MLS plane normal to the polynomial surface.
* \param[in] u The u-coordinate of the point in local MLS frame.
* \param[in] v The v-coordinate of the point in local MLS frame.
* \return The MLSProjectionResults for the input data.
* \note If the MLSResults does not contain polynomial data it projects the point onto the mls plane.
*/
MLSProjectionResults projectPointSimpleToPolynomialSurface (const double u, const double v) const;

MLSProjectionResults projectPoint(const Eigen::Vector3d &pt, ProjectionMethod method, int required_neighbors = 0) const;

MLSProjectionResults projectQueryPoint(ProjectionMethod method, bool compute_normal, int required_neighbors = 0) const;


Eigen::Vector3d query_point; /**< \brief The query point about which the mls surface was generated */
Eigen::Vector3d mean; /**< \brief The mean point of all the neighbors. */
Expand All @@ -165,8 +170,7 @@ namespace pcl
Eigen::VectorXd c_vec; /**< \brief The polynomial coefficients Example: z = c_vec[0] + c_vec[1]*v + c_vec[2]*v^2 + c_vec[3]*u + c_vec[4]*u*v + c_vec[5]*u^2 */
int num_neighbors; /**< \brief The number of neighbors used to create the mls surface. */
float curvature; /**< \brief The curvature at the query point. */
int order; /**< \brief The order of the polynomial used if polynomial_fit = true */
bool polynomial_fit; /**< \brief If True, a polynomial surface was fitted to the neighbors as the mls surface */
int order; /**< \brief The order of the polynomial. If order > 1 then use polynomial fit */
bool valid; /**< \brief If True, the mls results data is valid, otherwise False. */

};
Expand Down Expand Up @@ -237,7 +241,6 @@ namespace pcl
typedef boost::function<int (int, double, std::vector<int> &, std::vector<float> &)> SearchMethod;

enum UpsamplingMethod {NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION};
enum ProjectionMethod {SIMPLE, ORTHOGONAL};

/** \brief Empty constructor. */
MovingLeastSquares () : CloudSurfaceProcessing<PointInT, PointOutT> (),
Expand All @@ -256,7 +259,7 @@ namespace pcl
desired_num_points_in_radius_ (0),
cache_mls_results_ (true),
mls_results_ (),
projection_method_ (SIMPLE),
projection_method_ (MLSResult::SIMPLE),
voxel_size_ (1.0),
dilation_iteration_num_ (0),
nr_coeff_ (),
Expand Down Expand Up @@ -452,11 +455,11 @@ namespace pcl
* * ORTHONORMAL - Project the point to the closest point on the polynonomial surface.
*/
inline void
setProjectionMethod (ProjectionMethod method) { projection_method_ = method; }
setProjectionMethod (MLSResult::ProjectionMethod method) { projection_method_ = method; }


/** \brief Get the current projection method being used. */
inline ProjectionMethod
inline MLSResult::ProjectionMethod
getProjectionMethod () const { return projection_method_; }

/** \brief Get the MLSResults for input cloud
Expand Down Expand Up @@ -534,7 +537,7 @@ namespace pcl
std::vector<MLSResult> mls_results_;

/** \brief Parameter that specifies the projection method to be used. */
ProjectionMethod projection_method_;
MLSResult::ProjectionMethod projection_method_;


/** \brief A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling
Expand Down

0 comments on commit ccc98d9

Please sign in to comment.