| 306 | } |
| 307 | |
| 308 | void projectPointCloud() { |
| 309 | // range image projection |
| 310 | float verticalAngle, horizonAngle, range; |
| 311 | size_t rowIdn, columnIdn, index, cloudSize; |
| 312 | PointTypeIP thisPoint; |
| 313 | |
| 314 | cloudSize = laserCloudIn->points.size(); |
| 315 | |
| 316 | for (size_t i = 0; i < cloudSize; ++i) { |
| 317 | |
| 318 | thisPoint.x = laserCloudIn->points[i].x; |
| 319 | thisPoint.y = laserCloudIn->points[i].y; |
| 320 | thisPoint.z = laserCloudIn->points[i].z; |
| 321 | |
| 322 | // find the row and column index in the iamge for this point |
| 323 | verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI; |
| 324 | rowIdn = (verticalAngle + ang_bottom) / ang_res_y; |
| 325 | |
| 326 | if (rowIdn < 0 || rowIdn >= N_SCAN) |
| 327 | continue; |
| 328 | |
| 329 | horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; |
| 330 | columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN / 2; |
| 331 | if (columnIdn >= Horizon_SCAN) |
| 332 | columnIdn -= Horizon_SCAN; |
| 333 | |
| 334 | if (columnIdn < 0 || columnIdn >= Horizon_SCAN) |
| 335 | continue; |
| 336 | |
| 337 | range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z); |
| 338 | if (range < 0.1) |
| 339 | continue; |
| 340 | |
| 341 | rangeMat.at<float>(rowIdn, columnIdn) = range; |
| 342 | |
| 343 | // ??? |
| 344 | thisPoint.intensity = (float) rowIdn + (float) columnIdn / 10000.0; |
| 345 | |
| 346 | index = columnIdn + rowIdn * Horizon_SCAN; |
| 347 | fullCloud->points[index] = thisPoint; |
| 348 | fullInfoCloud->points[index] = thisPoint; |
| 349 | fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity" |
| 350 | } |
| 351 | |
| 352 | } |
| 353 | |
| 354 | void maskGround() { |
| 355 | // The pixels where the points are not projected are masked (i.e. rangeMat.at<float>(i, j) == FLT_MAX) |