| 128 | } |
| 129 | |
| 130 | void |
| 131 | transformPointCloud2 (const pcl::PCLPointCloud2 &input, pcl::PCLPointCloud2 &output, |
| 132 | Eigen::Matrix4f &tform) |
| 133 | { |
| 134 | // Check for 'rgb' and 'normals' fields |
| 135 | bool has_rgb = false; |
| 136 | bool has_normals = false; |
| 137 | for (const auto &field : input.fields) |
| 138 | { |
| 139 | if (field.name.find("rgb") != std::string::npos) |
| 140 | has_rgb = true; |
| 141 | if (field.name == "normal_x") |
| 142 | has_normals = true; |
| 143 | } |
| 144 | |
| 145 | // Handle the following four point types differently: PointXYZ, PointXYZRGB, PointNormal, PointXYZRGBNormal |
| 146 | if (!has_rgb && !has_normals) |
| 147 | transformPointCloud2AsType<PointXYZ> (input, output, tform); |
| 148 | else if (has_rgb && !has_normals) |
| 149 | transformPointCloud2AsType<PointXYZRGB> (input, output, tform); |
| 150 | else if (!has_rgb && has_normals) |
| 151 | transformPointCloud2AsType<PointNormal> (input, output, tform); |
| 152 | else // (has_rgb && has_normals) |
| 153 | transformPointCloud2AsType<PointXYZRGBNormal> (input, output, tform); |
| 154 | } |
| 155 | |
| 156 | void |
| 157 | compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output, |