1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2009-2012, Willow Garage, Inc. 6 * Copyright (c) 2012-, Open Perception, Inc. 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of the copyright holder(s) nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 * 37 * $Id$ 38 * 39 */ 40 41 #pragma once 42 43 #include <cstdint> 44 45 #include <pcl/apps/in_hand_scanner/common_types.h> 46 #include <pcl/pcl_exports.h> 47 #include <pcl/kdtree/kdtree.h> 48 49 //////////////////////////////////////////////////////////////////////////////// 50 // Integration 51 //////////////////////////////////////////////////////////////////////////////// 52 53 namespace pcl 54 { 55 namespace ihs 56 { 57 /** \brief Integrate several clouds into a common mesh. 58 * \author Martin Saelzle 59 * \ingroup apps 60 */ 61 class PCL_EXPORTS Integration 62 { 63 public: 64 65 using PointXYZRGBNormal = pcl::PointXYZRGBNormal; 66 using CloudXYZRGBNormal = pcl::PointCloud<PointXYZRGBNormal>; 67 using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; 68 using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; 69 70 using Mesh = pcl::ihs::Mesh; 71 using MeshPtr = pcl::ihs::MeshPtr; 72 using MeshConstPtr = pcl::ihs::MeshConstPtr; 73 using VertexIndex = Mesh::VertexIndex; 74 using VertexIndices = Mesh::VertexIndices; 75 76 /** \brief Constructor. */ 77 Integration (); 78 79 /** \brief Reconstructs a mesh from an organized cloud. 80 * \param[in] cloud_data Input cloud. Must be organized. 81 * \param[in] mesh_model Reconstructed mesh. 82 * \return true if success. 83 */ 84 bool 85 reconstructMesh (const CloudXYZRGBNormalConstPtr& cloud_data, 86 MeshPtr& mesh_model) const; 87 88 /** \brief Merge the organized cloud into the mesh. 89 * \param[in] cloud_data Input cloud. Must be organized. 90 * \param[in,out] mesh_model Mesh with new points integrated. 91 * \param[in] T Transformation that aligns the data cloud with the model mesh. 92 * \return true if success. 93 */ 94 bool 95 merge (const CloudXYZRGBNormalConstPtr& cloud_data, 96 MeshPtr& mesh_model, 97 const Eigen::Matrix4f& T) const; 98 99 /** \brief Outlier rejection. In each merge step points that have not been observed again age by one iteration. Points that are observed again get an age of 0. Once a point reaches the maximum age it is decided if the point is removed or kept in the mesh. A point is removed if it has not been observed from a minimum number of directions. 100 * \param[in,out] mesh The mesh which should be processed. 101 * \param[in] cleanup Calls mesh.cleanup () if true. 102 */ 103 void 104 age (const MeshPtr& mesh, const bool cleanup=true) const; 105 106 /** \brief Removes unfit vertices regardless of their age. Unfit vertices are those that have not been observed from enough directions. 107 * \param[in,out] mesh The which should be processed. 108 * \param[in] cleanup Calls mesh.cleanup () if true. 109 */ 110 void 111 removeUnfitVertices (const MeshPtr& mesh, const bool cleanup=true) const; 112 113 /** @{ */ 114 /** \brief Corresponding points are averaged out if their distance is below a distance threshold. Else the points are added to the mesh as new vertices (Set in cm^2). 115 * \note Must be greater than zero. 116 */ 117 void setMaxSquaredDistance (const float squared_distance); 118 float getMaxSquaredDistance () const; 119 /** @} */ 120 121 /** @{ */ 122 /** \brief Corresponding points are only averaged out if the angle between the normals is smaller than an angle threshold. 123 * \note Must be between 0 and 180. Values outside this range are clamped to the nearest valid value. 124 */ 125 void setMaxAngle (const float angle); 126 float getMaxAngle () const; 127 /** @} */ 128 129 /** @{ */ 130 /** \brief Once a point reaches the maximum age it is decided if the point is removed or kept in the mesh. 131 * \note Must be greater than zero. 132 */ 133 void setMaxAge (const unsigned int age); 134 unsigned int getMaxAge () const; 135 /** @} */ 136 137 /** @{ */ 138 /** \brief A point is removed if it has not been observed from a minimum number of directions. 139 * \note Must be greater than zero. 140 */ 141 void setMinDirections (const unsigned int directions); 142 unsigned int getMinDirections () const; 143 /** @} */ 144 145 private: 146 147 using PointXYZ = pcl::PointXYZ; 148 using CloudXYZ = pcl::PointCloud<PointXYZ>; 149 using CloudXYZPtr = CloudXYZ::Ptr; 150 using CloudXYZConstPtr = CloudXYZ::ConstPtr; 151 152 using PointIHS = pcl::ihs::PointIHS; 153 using CloudIHS = pcl::ihs::CloudIHS; 154 using CloudIHSPtr = pcl::ihs::CloudIHSPtr; 155 using CloudIHSConstPtr = pcl::ihs::CloudIHSConstPtr; 156 157 using KdTree = pcl::KdTree<PointXYZ>; 158 using KdTreePtr = KdTree::Ptr; 159 using KdTreeConstPtr = KdTree::ConstPtr; 160 161 std::uint8_t 162 trimRGB (const float val) const; 163 164 /** \brief Adds two triangles between points 0-1-3 and 1-2-3 to the mesh. */ 165 void 166 addToMesh (const PointIHS& pt_0, 167 const PointIHS& pt_1, 168 const PointIHS& pt_2, 169 const PointIHS& pt_3, 170 VertexIndex& vi_0, 171 VertexIndex& vi_1, 172 VertexIndex& vi_2, 173 VertexIndex& vi_3, 174 const MeshPtr& mesh) const; 175 176 /** \brief Adds a triangle between the points 0-1-2 to the mesh. */ 177 void 178 addToMesh (const PointIHS& pt_0, 179 const PointIHS& pt_1, 180 const PointIHS& pt_2, 181 VertexIndex& vi_0, 182 VertexIndex& vi_1, 183 VertexIndex& vi_2, 184 const MeshPtr& mesh) const; 185 186 /** \brief Returns true if the distance between the three points is below a threshold. */ 187 bool 188 distanceThreshold (const PointIHS& pt_0, 189 const PointIHS& pt_1, 190 const PointIHS& pt_2) const; 191 192 /** \brief Returns true if the distance between the four points is below a threshold. */ 193 bool 194 distanceThreshold (const PointIHS& pt_0, 195 const PointIHS& pt_1, 196 const PointIHS& pt_2, 197 const PointIHS& pt_3) const; 198 199 //////////////////////////////////////////////////////////////////////// 200 // Members 201 //////////////////////////////////////////////////////////////////////// 202 203 /** \brief Nearest neighbor search. */ 204 KdTreePtr kd_tree_; 205 206 /** \brief Maximum squared distance below which points are averaged out. */ 207 float max_squared_distance_; 208 209 /** \brief Maximum angle between normals below which points are averaged out. In degrees. */ 210 float max_angle_; 211 212 /** \brief Minimum weight above which points are added. */ 213 float min_weight_; 214 215 /** \brief Once a point reaches the maximum age it is decided if the point is removed or kept in the mesh. */ 216 unsigned int max_age_; 217 218 /** \brief A point is removed if it has not been observed from a minimum number of directions. */ 219 unsigned int min_directions_; 220 }; 221 } // End namespace ihs 222 } // End namespace pcl 223