/* * Software License Agreement (BSD License) * * 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. * * * */ #pragma once //#include //#include "v4r/TomGine/tgTomGine.h" //#include "v4r/NurbsConvertion/NurbsConvertion.h" // remove multiple #defines from xlib and OpenMesh #undef True #undef False #undef None #undef Status #include #include #include #include #include #include //#include "v4r/NurbsConvertion/DataLoading.h" namespace pcl { namespace on_nurbs { class SequentialFitter { public: struct Parameter { int order; int refinement; int iterationsQuad; int iterationsBoundary; int iterationsAdjust; int iterationsInterior; double forceBoundary; double forceBoundaryInside; double forceInterior; double stiffnessBoundary; double stiffnessInterior; int resolution; Parameter (int order = 3, int refinement = 1, int iterationsQuad = 5, int iterationsBoundary = 5, int iterationsAdjust = 5, int iterationsInterior = 2, double forceBoundary = 200.0, double forceBoundaryInside = 400.0, double forceInterior = 1.0, double stiffnessBoundary = 20.0, double stiffnessInterior = 0.1, int resolution = 16); }; private: Parameter m_params; NurbsDataSurface m_data; ON_NurbsSurface m_nurbs; ON_3dPoint m_corners[4]; pcl::PointCloud::Ptr m_cloud; pcl::PointIndices::Ptr m_boundary_indices; pcl::PointIndices::Ptr m_interior_indices; Eigen::Matrix4d m_intrinsic; Eigen::Matrix4d m_extrinsic; bool m_have_cloud; bool m_have_corners; int m_surf_id; void compute_quadfit (); void compute_refinement (FittingSurface* fitting) const; void compute_boundary (FittingSurface* fitting) const; void compute_interior (FittingSurface* fitting) const; Eigen::Vector2d project (const Eigen::Vector3d &pt); bool is_back_facing (const Eigen::Vector3d &v0, const Eigen::Vector3d &v1, const Eigen::Vector3d &v2, const Eigen::Vector3d &v3); public: SequentialFitter (Parameter p = Parameter ()); inline void setParams (const Parameter &p) { m_params = p; } /** \brief Set input point cloud **/ void setInputCloud (pcl::PointCloud::Ptr &pcl_cloud); /** \brief Set boundary points of input point cloud **/ void setBoundary (pcl::PointIndices::Ptr &pcl_cloud_indices); /** \brief Set interior points of input point cloud **/ void setInterior (pcl::PointIndices::Ptr &pcl_cloud_indices); /** \brief Set corner points of input point cloud **/ void setCorners (pcl::PointIndices::Ptr &corners, bool flip_on_demand = true); /** \brief Set camera- and world matrices, for projection and back-face detection * \param[in] intrinsic The camera projection matrix. * \param[in] intrinsic The world matrix.*/ void setProjectionMatrix (const Eigen::Matrix4d &intrinsic, const Eigen::Matrix4d &extrinsic); /** \brief Compute point cloud and fit (multiple) models */ ON_NurbsSurface compute (bool assemble = false); /** \brief Compute boundary points and fit (multiple) models * (without interior points - minimal curvature surface) */ ON_NurbsSurface compute_boundary (const ON_NurbsSurface &nurbs); /** \brief Compute interior points and fit (multiple) models * (without boundary points)*/ ON_NurbsSurface compute_interior (const ON_NurbsSurface &nurbs); /** \brief Get resulting NURBS surface. */ inline ON_NurbsSurface getNurbs () { return m_nurbs; } /** \brief Get error of each interior point (L2-norm of point to closest point on surface) and square-error */ void getInteriorError (std::vector &error) const; /** \brief Get error of each boundary point (L2-norm of point to closest point on surface) and square-error */ void getBoundaryError (std::vector &error) const; /** \brief Get parameter of each interior point */ void getInteriorParams (std::vector > ¶ms) const; /** \brief Get parameter of each boundary point */ void getBoundaryParams (std::vector > ¶ms) const; /** \brief get the normals to the interior points given by setInterior() */ void getInteriorNormals (std::vector > &normal) const; /** \brief get the normals to the boundary points given by setBoundary() */ void getBoundaryNormals (std::vector > &normals) const; /** \brief Get the closest point on a NURBS from a point pt in parameter space * \param[in] nurbs The NURBS surface * \param[in] pt A point in 3D from which the closest point is calculated * \param[out] params The closest point on the NURBS in parameter space * \param[in] maxSteps Maximum iteration steps * \param[in] accuracy Accuracy below which the iterations stop */ static void getClosestPointOnNurbs (ON_NurbsSurface nurbs, const Eigen::Vector3d &pt, Eigen::Vector2d& params, int maxSteps = 100, double accuracy = 1e-4); /** \brief Growing algorithm (TODO: under construction) */ ON_NurbsSurface grow (float max_dist = 1.0f, float max_angle = M_PI_4, unsigned min_length = 0, unsigned max_length = 10); /** \brief Convert point-cloud */ static unsigned PCL2ON (pcl::PointCloud::Ptr &pcl_cloud, const pcl::Indices &indices, vector_vec3d &cloud); public: PCL_MAKE_ALIGNED_OPERATOR_NEW }; } }