From 3ee92d922308cdfd78e59d8f326383432e46dc4a Mon Sep 17 00:00:00 2001 From: Dirk Holz Date: Tue, 16 Jun 2015 15:16:36 +0200 Subject: [PATCH] (Minor) modifications for the registration example in the RAM article: * GICP now allows setting (externally computed) covariance matrices. If not set, GICP will compute these as usual. * OrganizedFastMesh now accepts a complete quadratic error in the form of sigma_d = a*d*d + b*d + c. * OrganizedFastMesh now allows to either use the measured depth (z) or the actual distance to the point (e.g., for 3D laser scans) * from_meshes.h contains two additional inline functions for computing approximate local surface normals on polygons and a PolygonMesh, respectively, and approximate covariance matrices given points and normals (GICP-style). --- features/CMakeLists.txt | 1 + features/include/pcl/features/from_meshes.h | 105 ++++++++++++ registration/include/pcl/registration/gicp.h | 42 +++-- .../include/pcl/registration/impl/gicp.hpp | 22 ++- registration/src/gicp6d.cpp | 8 +- .../include/pcl/surface/organized_fast_mesh.h | 162 ++++++++++++++++-- 6 files changed, 301 insertions(+), 39 deletions(-) create mode 100644 features/include/pcl/features/from_meshes.h diff --git a/features/CMakeLists.txt b/features/CMakeLists.txt index cd023ed43a6..ad5d60bbd7e 100644 --- a/features/CMakeLists.txt +++ b/features/CMakeLists.txt @@ -24,6 +24,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/feature.h" "include/pcl/${SUBSYS_NAME}/fpfh.h" "include/pcl/${SUBSYS_NAME}/fpfh_omp.h" + "include/pcl/${SUBSYS_NAME}/from_meshes.h" "include/pcl/${SUBSYS_NAME}/gfpfh.h" "include/pcl/${SUBSYS_NAME}/integral_image2D.h" "include/pcl/${SUBSYS_NAME}/integral_image_normal.h" diff --git a/features/include/pcl/features/from_meshes.h b/features/include/pcl/features/from_meshes.h new file mode 100644 index 00000000000..04229167ed1 --- /dev/null +++ b/features/include/pcl/features/from_meshes.h @@ -0,0 +1,105 @@ +#ifndef PCL_FEATURES_FROM_MESHES_H_ +#define PCL_FEATURES_FROM_MESHES_H_ + +#include + +namespace pcl +{ + namespace features + { + + /** \brief Compute approximate surface normals on a mesh. + * \param[in] cloud Point cloud containing the XYZ coordinates. + * \param[in] polygons Polygons from the mesh. + * \param[out] normals Point cloud with computed surface normals + */ + template inline void + computeApproximateNormals(const pcl::PointCloud& cloud, const std::vector& polygons, pcl::PointCloud& normals) + { + int nr_points = static_cast(cloud.points.size()); + int nr_polygons = static_cast(polygons.size()); + + normals.header = cloud.header; + normals.width = cloud.width; + normals.height = cloud.height; + normals.points.resize(nr_points); + + for ( int i = 0; i < nr_points; ++i ) + normals.points[i].getNormalVector3fMap() = Eigen::Vector3f::Zero(); + + // NOTE: for efficiency the weight is computed implicitly by using the + // cross product, this causes inaccurate normals for meshes containing + // non-triangle polygons (quads or other types) + for ( int i = 0; i < nr_polygons; ++i ) + { + const int nr_points_polygon = (int)polygons[i].vertices.size(); + if (nr_points_polygon < 3) continue; + + // compute normal for triangle + Eigen::Vector3f vec_a_b = cloud.points[polygons[i].vertices[0]].getVector3fMap() - cloud.points[polygons[i].vertices[1]].getVector3fMap(); + Eigen::Vector3f vec_a_c = cloud.points[polygons[i].vertices[0]].getVector3fMap() - cloud.points[polygons[i].vertices[2]].getVector3fMap(); + Eigen::Vector3f normal = vec_a_b.cross(vec_a_c); + pcl::flipNormalTowardsViewpoint(cloud.points[polygons[i].vertices[0]], 0.0f, 0.0f, 0.0f, normal(0), normal(1), normal(2)); + + // add normal to all points in polygon + for ( int j = 0; j < nr_points_polygon; ++j ) + normals.points[polygons[i].vertices[j]].getNormalVector3fMap() += normal; + } + + for ( int i = 0; i < nr_points; ++i ) + { + normals.points[i].getNormalVector3fMap().normalize(); + pcl::flipNormalTowardsViewpoint(cloud.points[i], 0.0f, 0.0f, 0.0f, normals.points[i].normal_x, normals.points[i].normal_y, normals.points[i].normal_z); + } + } + + + /** \brief Compute GICP-style covariance matrices given a point cloud and + * the corresponding surface normals. + * \param[in] cloud Point cloud containing the XYZ coordinates, + * \param[in] normals Point cloud containing the corresponding surface normals. + * \param[out] covariances Vector of computed covariances. + * \param[in] Optional: Epsilon for the expected noise along the surface normal (default: 0.001) + */ + template inline void + computeApproximateCovariances(const pcl::PointCloud& cloud, + const pcl::PointCloud& normals, + std::vector >& covariances, + double epsilon = 0.001) + { + assert(cloud.points.size() == normals.points.size()); + + int nr_points = static_cast(cloud.points.size()); + covariances.resize(nr_points); + for (int i = 0; i < nr_points; ++i) + { + Eigen::Vector3d normal(normals.points[i].normal_x, + normals.points[i].normal_y, + normals.points[i].normal_z); + + // compute rotation matrix + Eigen::Matrix3d rot; + Eigen::Vector3d y; + y << 0, 1, 0; + rot.row(2) = normal; + y = y - normal(1) * normal; + y.normalize(); + rot.row(1) = y; + rot.row(0) = normal.cross(rot.row(1)); + + // comnpute approximate covariance + Eigen::Matrix3d cov; + cov << 1, 0, 0, + 0, 1, 0, + 0, 0, epsilon; + covariances[i] = rot.transpose()*cov*rot; + } + } + + } +} + + +#endif // PCL_FEATURES_FROM_MESHES_H_ + + diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h index ba11d622490..361e5043f91 100644 --- a/registration/include/pcl/registration/gicp.h +++ b/registration/include/pcl/registration/gicp.h @@ -90,6 +90,10 @@ namespace pcl typedef PointIndices::Ptr PointIndicesPtr; typedef PointIndices::ConstPtr PointIndicesConstPtr; + typedef std::vector< Eigen::Matrix3d, Eigen::aligned_allocator > MatricesVector; + typedef boost::shared_ptr< MatricesVector > MatricesVectorPtr; + typedef boost::shared_ptr< const MatricesVector > MatricesVectorConstPtr; + typedef typename Registration::KdTree InputKdTree; typedef typename Registration::KdTreePtr InputKdTreePtr; @@ -104,8 +108,6 @@ namespace pcl : k_correspondences_(20) , gicp_epsilon_(0.001) , rotation_epsilon_(2e-3) - , input_covariances_(0) - , target_covariances_(0) , mahalanobis_(0) , max_inner_iterations_(20) { @@ -144,10 +146,20 @@ namespace pcl input[i].data[3] = 1.0; pcl::IterativeClosestPoint::setInputSource (cloud); - input_covariances_.clear (); - input_covariances_.reserve (input_->size ()); + input_covariances_.reset (); } + /** \brief Provide a pointer to the covariances of the input source (if computed externally!). + * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. + * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances). + * \param[in] target the input point cloud target + */ + inline void + setSourceCovariances (const MatricesVectorPtr& covariances) + { + input_covariances_ = covariances; + } + /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) * \param[in] target the input point cloud target */ @@ -155,10 +167,20 @@ namespace pcl setInputTarget (const PointCloudTargetConstPtr &target) { pcl::IterativeClosestPoint::setInputTarget(target); - target_covariances_.clear (); - target_covariances_.reserve (target_->size ()); + target_covariances_.reset (); } + /** \brief Provide a pointer to the covariances of the input target (if computed externally!). + * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. + * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances). + * \param[in] target the input point cloud target + */ + inline void + setTargetCovariances (const MatricesVectorPtr& covariances) + { + target_covariances_ = covariances; + } + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using an iterative * non-linear Levenberg-Marquardt approach. * \param[in] cloud_src the source point cloud dataset @@ -266,13 +288,13 @@ namespace pcl /** \brief Input cloud points covariances. */ - std::vector > input_covariances_; + MatricesVectorPtr input_covariances_; /** \brief Target cloud points covariances. */ - std::vector > target_covariances_; + MatricesVectorPtr target_covariances_; /** \brief Mahalanobis matrices holder. */ - std::vector > mahalanobis_; + std::vector mahalanobis_; /** \brief maximum number of optimizations */ int max_inner_iterations_; @@ -286,7 +308,7 @@ namespace pcl template void computeCovariances(typename pcl::PointCloud::ConstPtr cloud, const typename pcl::search::KdTree::Ptr tree, - std::vector >& cloud_covariances); + MatricesVector& cloud_covariances); /** \return trace of mat1^t . mat2 * \param mat1 matrix of dimension nxm diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp index 43079f4c9cb..11cc0d8320d 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -56,7 +56,7 @@ template template void pcl::GeneralizedIterativeClosestPoint::computeCovariances(typename pcl::PointCloud::ConstPtr cloud, const typename pcl::search::KdTree::Ptr kdtree, - std::vector >& cloud_covariances) + MatricesVector& cloud_covariances) { if (k_correspondences_ > int (cloud->size ())) { @@ -73,7 +73,7 @@ pcl::GeneralizedIterativeClosestPoint::computeCovarian cloud_covariances.resize (cloud->size ()); typename pcl::PointCloud::const_iterator points_iterator = cloud->begin (); - std::vector >::iterator matrices_iterator = cloud_covariances.begin (); + MatricesVector::iterator matrices_iterator = cloud_covariances.begin (); for(; points_iterator != cloud->end (); ++points_iterator, ++matrices_iterator) @@ -361,11 +361,17 @@ pcl::GeneralizedIterativeClosestPoint::computeTransfor // Set the mahalanobis matrices to identity mahalanobis_.resize (N, Eigen::Matrix3d::Identity ()); // Compute target cloud covariance matrices - if (target_covariances_.empty ()) - computeCovariances (target_, tree_, target_covariances_); + if ((!target_covariances_) || (target_covariances_->empty ())) + { + target_covariances_.reset (new MatricesVector); + computeCovariances (target_, tree_, *target_covariances_); + } // Compute input cloud covariance matrices - if (input_covariances_.empty ()) - computeCovariances (input_, tree_reciprocal_, input_covariances_); + if ((!input_covariances_) || (input_covariances_->empty ())) + { + input_covariances_.reset (new MatricesVector); + computeCovariances (input_, tree_reciprocal_, *input_covariances_); + } base_transformation_ = guess; nr_iterations_ = 0; @@ -404,8 +410,8 @@ pcl::GeneralizedIterativeClosestPoint::computeTransfor // Check if the distance to the nearest neighbor is smaller than the user imposed threshold if (nn_dists[0] < dist_threshold) { - Eigen::Matrix3d &C1 = input_covariances_[i]; - Eigen::Matrix3d &C2 = target_covariances_[nn_indices[0]]; + Eigen::Matrix3d &C1 = (*input_covariances_)[i]; + Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]]; Eigen::Matrix3d &M = mahalanobis_[i]; // M = R*C1 M = R * C1; diff --git a/registration/src/gicp6d.cpp b/registration/src/gicp6d.cpp index 08ebeeb8aef..a8f1da3531a 100644 --- a/registration/src/gicp6d.cpp +++ b/registration/src/gicp6d.cpp @@ -191,10 +191,10 @@ namespace pcl mahalanobis_.resize (N, Eigen::Matrix3d::Identity ()); // Compute target cloud covariance matrices - computeCovariances (target_, tree_, target_covariances_); + computeCovariances (target_, tree_, *target_covariances_); // Compute input cloud covariance matrices computeCovariances (input_, tree_reciprocal_, - input_covariances_); + *input_covariances_); base_transformation_ = guess; nr_iterations_ = 0; @@ -237,8 +237,8 @@ namespace pcl // Check if the distance to the nearest neighbor is smaller than the user imposed threshold if (nn_dists[0] < dist_threshold) { - Eigen::Matrix3d &C1 = input_covariances_[i]; - Eigen::Matrix3d &C2 = target_covariances_[nn_indices[0]]; + Eigen::Matrix3d &C1 = (*input_covariances_)[i]; + Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]]; Eigen::Matrix3d &M = mahalanobis_[i]; // M = R*C1 M = R * C1; diff --git a/surface/include/pcl/surface/organized_fast_mesh.h b/surface/include/pcl/surface/organized_fast_mesh.h index f6452368c6e..54bbd3ddd32 100644 --- a/surface/include/pcl/surface/organized_fast_mesh.h +++ b/surface/include/pcl/surface/organized_fast_mesh.h @@ -57,7 +57,7 @@ namespace pcl * In Proceedings of the 12th International Conference on Intelligent Autonomous Systems (IAS), * Jeju Island, Korea, June 26-29 2012. * http://purl.org/holz/papers/holz_2012_ias.pdf - * + * * \author Dirk Holz, Radu B. Rusu * \ingroup surface */ @@ -85,12 +85,20 @@ namespace pcl /** \brief Constructor. Triangulation type defaults to \a QUAD_MESH. */ OrganizedFastMesh () - : max_edge_length_squared_ (0.025f) + : max_edge_length_a_ (0.0f) + , max_edge_length_b_ (0.0f) + , max_edge_length_c_ (0.0f) + , max_edge_length_set_ (false) + , max_edge_length_dist_dependent_ (false) , triangle_pixel_size_rows_ (1) , triangle_pixel_size_columns_ (1) , triangulation_type_ (QUAD_MESH) + , viewpoint_ (Eigen::Vector3f::Zero ()) , store_shadowed_faces_ (false) , cos_angle_tolerance_ (fabsf (cosf (pcl::deg2rad (12.5f)))) + , distance_tolerance_ (-1.0f) + , distance_dependent_ (false) + , use_depth_as_distance_(false) { check_tree_ = false; }; @@ -98,15 +106,31 @@ namespace pcl /** \brief Destructor. */ virtual ~OrganizedFastMesh () {}; - /** \brief Set a maximum edge length. TODO: Implement! - * \param[in] max_edge_length the maximum edge length + /** \brief Set a maximum edge length. + * Using not only the scalar \a a, but also \a b and \a c, allows for using a distance threshold in the form of: + * threshold(x) = c*x*x + b*x + a + * \param[in] a scalar coefficient of the (distance-dependent polynom) threshold + * \param[in] b linear coefficient of the (distance-dependent polynom) threshold + * \param[in] c quadratic coefficient of the (distance-dependent polynom) threshold */ inline void - setMaxEdgeLength (float max_edge_length) + setMaxEdgeLength (float a, float b = 0.0f, float c = 0.0f) { - max_edge_length_squared_ = max_edge_length * max_edge_length; + max_edge_length_a_ = a; + max_edge_length_b_ = b; + max_edge_length_c_ = c; + if ((max_edge_length_a_ + max_edge_length_b_ + max_edge_length_c_) > std::numeric_limits::min()) + max_edge_length_set_ = true; + else + max_edge_length_set_ = false; }; + inline void + unsetMaxEdgeLength () + { + max_edge_length_set_ = false; + } + /** \brief Set the edge length (in pixels) used for constructing the fixed mesh. * \param[in] triangle_size edge length in pixels * (Default: 1 = neighboring pixels are connected) @@ -148,18 +172,74 @@ namespace pcl triangulation_type_ = type; } + /** \brief Set the viewpoint from where the input point cloud has been acquired. + * \param[in] viewpoint Vector containing the viewpoint coordinates (in the coordinate system of the data) + */ + inline void setViewpoint (const Eigen::Vector3f& viewpoint) + { + viewpoint_ = viewpoint; + } + + /** \brief Get the viewpoint from where the input point cloud has been acquired. */ + const inline Eigen::Vector3f& getViewpoint () const + { + return viewpoint_; + } + /** \brief Store shadowed faces or not. - * \param[in] enable set to true to store shadowed faces - */ + * \param[in] enable set to true to store shadowed faces + */ inline void storeShadowedFaces (bool enable) { store_shadowed_faces_ = enable; } + /** \brief Set the angle tolerance used for checking whether or not an edge is occluded. + * Standard values are 5deg to 15deg (input in rad!). Set a value smaller than zero to + * disable the check for shadowed edges. + * \param[in] angle_tolerance Angle tolerance (in rad). Set a value <0 to disable. + */ + inline void + setAngleTolerance(float angle_tolerance) + { + if (angle_tolerance > 0) + cos_angle_tolerance_ = fabsf (cosf (angle_tolerance)); + else + cos_angle_tolerance_ = -1.0f; + } + + + inline void setDistanceTolerance(float distance_tolerance, bool depth_dependent = false) + { + distance_tolerance_ = distance_tolerance; + if (distance_tolerance_ < 0) + return; + + distance_dependent_ = depth_dependent; + if (!distance_dependent_) + distance_tolerance_ *= distance_tolerance_; + } + + /** \brief Use the points' depths (z-coordinates) instead of measured distances (points' distances to the viewpoint). + * \param[in] enable Set to true skips comptations and further speeds up computation by using depth instead of computing distance. false to disable. */ + inline void useDepthAsDistance(bool enable) + { + use_depth_as_distance_ = enable; + } + protected: - /** \brief max (squared) length of edge */ - float max_edge_length_squared_; + /** \brief max length of edge, scalar component */ + float max_edge_length_a_; + /** \brief max length of edge, scalar component */ + float max_edge_length_b_; + /** \brief max length of edge, scalar component */ + float max_edge_length_c_; + /** \brief flag whether or not edges are limited in length */ + bool max_edge_length_set_; + + /** \brief flag whether or not max edge length is distance dependent. */ + bool max_edge_length_dist_dependent_; /** \brief size of triangle edges (in pixels) for iterating over rows. */ int triangle_pixel_size_rows_; @@ -170,12 +250,26 @@ namespace pcl /** \brief Type of meshing scheme (quads vs. triangles, left cut vs. right cut ... */ TriangulationType triangulation_type_; + /** \brief Viewpoint from which the point cloud has been acquired (in the same coordinate frame as the data). */ + Eigen::Vector3f viewpoint_; + /** \brief Whether or not shadowed faces are stored, e.g., for exploration */ bool store_shadowed_faces_; /** \brief (Cosine of the) angle tolerance used when checking whether or not an edge between two points is shadowed. */ float cos_angle_tolerance_; + /** \brief distance tolerance for filtering out shadowed/occluded edges */ + float distance_tolerance_; + + /** \brief flag whether or not \a distance_tolerance_ is distance dependent (multiplied by the squared distance to the point) or not. */ + bool distance_dependent_; + + /** \brief flag whether or not the points' depths are used instead of measured distances (points' distances to the viewpoint). + This flag may be set using useDepthAsDistance(true) for (RGB-)Depth cameras to skip computations and gain additional speed up. */ + bool use_depth_as_distance_; + + /** \brief Perform the actual polygonal reconstruction. * \param[out] polygons the resultant polygons */ @@ -259,16 +353,50 @@ namespace pcl inline bool isShadowed (const PointInT& point_a, const PointInT& point_b) { - Eigen::Vector3f viewpoint = Eigen::Vector3f::Zero (); // TODO: allow for passing viewpoint information - Eigen::Vector3f dir_a = viewpoint - point_a.getVector3fMap (); + bool valid = true; + + Eigen::Vector3f dir_a = viewpoint_ - point_a.getVector3fMap (); Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap (); float distance_to_points = dir_a.norm (); float distance_between_points = dir_b.norm (); - float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points); - if (cos_angle != cos_angle) - cos_angle = 1.0f; - return (fabs (cos_angle) >= cos_angle_tolerance_); - // TODO: check for both: angle almost 0/180 _and_ distance between points larger than noise level + + if (cos_angle_tolerance_ > 0) + { + float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points); + if (cos_angle != cos_angle) + cos_angle = 1.0f; + bool check_angle = fabs (cos_angle) >= cos_angle_tolerance_; + + bool check_distance = true; + if (check_angle && (distance_tolerance_ > 0)) + { + float dist_thresh = distance_tolerance_; + if (distance_dependent_) + { + float d = distance_to_points; + if (use_depth_as_distance_) + d = std::max(point_a.z, point_b.z); + dist_thresh *= d*d; + dist_thresh *= dist_thresh; // distance_tolerance_ is already squared if distance_dependent_ is false. + } + check_distance = (distance_between_points > dist_thresh); + } + valid = !(check_angle && check_distance); + } + + // check if max. edge length is not exceeded + if (max_edge_length_set_) + { + float dist = (use_depth_as_distance_ ? std::max(point_a.z, point_b.z) : distance_to_points); + float dist_thresh = max_edge_length_a_; + if (fabs(max_edge_length_b_) > std::numeric_limits::min()) + dist_thresh += max_edge_length_b_ * dist; + if (fabs(max_edge_length_c_) > std::numeric_limits::min()) + dist_thresh += max_edge_length_c_ * dist * dist; + valid = (distance_between_points <= dist_thresh); + } + + return !valid; } /** \brief Check if a triangle is valid.