| 62 | //////////////////////////////////////////////////////////////////////////////// |
| 63 | |
| 64 | bool |
| 65 | pcl::ihs::Integration::reconstructMesh(const CloudXYZRGBNormalConstPtr& cloud_data, |
| 66 | MeshPtr& mesh_model) const |
| 67 | { |
| 68 | if (!cloud_data) { |
| 69 | std::cerr << "ERROR in integration.cpp: Cloud pointer is invalid\n"; |
| 70 | return (false); |
| 71 | } |
| 72 | if (!cloud_data->isOrganized()) { |
| 73 | std::cerr << "ERROR in integration.cpp: Cloud is not organized\n"; |
| 74 | return (false); |
| 75 | } |
| 76 | const int width = static_cast<int>(cloud_data->width); |
| 77 | const int height = static_cast<int>(cloud_data->height); |
| 78 | |
| 79 | if (!mesh_model) |
| 80 | mesh_model = MeshPtr(new Mesh()); |
| 81 | |
| 82 | mesh_model->clear(); |
| 83 | mesh_model->reserveVertices(cloud_data->size()); |
| 84 | mesh_model->reserveEdges((width - 1) * height + width * (height - 1) + |
| 85 | (width - 1) * (height - 1)); |
| 86 | mesh_model->reserveFaces(2 * (width - 1) * (height - 1)); |
| 87 | |
| 88 | // Store which vertex is set at which position (initialized with invalid indices) |
| 89 | VertexIndices vertex_indices(cloud_data->size(), VertexIndex()); |
| 90 | |
| 91 | // Convert to the model cloud type. This is actually not needed but avoids code |
| 92 | // duplication (see merge). And reconstructMesh is called only the first |
| 93 | // reconstruction step anyway. NOTE: The default constructor of PointIHS has to |
| 94 | // initialize with NaNs! |
| 95 | CloudIHSPtr cloud_model(new CloudIHS()); |
| 96 | cloud_model->resize(cloud_data->size()); |
| 97 | |
| 98 | // Set the model points not reached by the main loop |
| 99 | for (int c = 0; c < width; ++c) { |
| 100 | const PointXYZRGBNormal& pt_d = cloud_data->operator[](c); |
| 101 | const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1]) |
| 102 | |
| 103 | if (!std::isnan(pt_d.x) && weight > min_weight_) { |
| 104 | cloud_model->operator[](c) = PointIHS(pt_d, weight); |
| 105 | } |
| 106 | } |
| 107 | for (int r = 1; r < height; ++r) { |
| 108 | for (int c = 0; c < 2; ++c) { |
| 109 | const PointXYZRGBNormal& pt_d = cloud_data->operator[](r * width + c); |
| 110 | const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1]) |
| 111 | |
| 112 | if (!std::isnan(pt_d.x) && weight > min_weight_) { |
| 113 | cloud_model->operator[](r * width + c) = PointIHS(pt_d, weight); |
| 114 | } |
| 115 | } |
| 116 | } |
| 117 | |
| 118 | // 4 2 - 1 // |
| 119 | // | | // |
| 120 | // * 3 - 0 // |
| 121 | // // |
no test coverage detected