| 52 | po->normal_y = pi->normal_y; |
| 53 | } |
| 54 | void MAP_MANAGER::featureAssociateToMap(const pcl::PointCloud<PointType>::Ptr& laserCloudCorner, |
| 55 | const pcl::PointCloud<PointType>::Ptr& laserCloudSurf, |
| 56 | const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeature, |
| 57 | const pcl::PointCloud<PointType>::Ptr& laserCloudCornerToMap, |
| 58 | const pcl::PointCloud<PointType>::Ptr& laserCloudSurfToMap, |
| 59 | const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeatureToMap, |
| 60 | const Eigen::Matrix4d& transformTobeMapped){ |
| 61 | |
| 62 | int laserCloudCornerNum = laserCloudCorner->points.size(); |
| 63 | int laserCloudSurfNum = laserCloudSurf->points.size(); |
| 64 | int laserCloudNonFeatureNum = laserCloudNonFeature->points.size(); |
| 65 | PointType pointSel1,pointSel2,pointSel3; |
| 66 | for (int i = 0; i < laserCloudCornerNum; i++) { |
| 67 | pointAssociateToMap(&laserCloudCorner->points[i], &pointSel1, transformTobeMapped); |
| 68 | laserCloudCornerToMap->push_back(pointSel1); |
| 69 | } |
| 70 | for (int i = 0; i < laserCloudSurfNum; i++) { |
| 71 | pointAssociateToMap(&laserCloudSurf->points[i], &pointSel2, transformTobeMapped); |
| 72 | laserCloudSurfToMap->push_back(pointSel2); |
| 73 | } |
| 74 | for (int i = 0; i < laserCloudNonFeatureNum; i++) { |
| 75 | pointAssociateToMap(&laserCloudNonFeature->points[i], &pointSel3, transformTobeMapped); |
| 76 | laserCloudNonFeatureToMap->push_back(pointSel3); |
| 77 | } |
| 78 | |
| 79 | } |
| 80 | /** \brief add new lidar points to the map |
| 81 | * \param[in] laserCloudCornerStack: coner feature points that need to be added to map |
| 82 | * \param[in] laserCloudSurfStack: surf feature points that need to be added to map |
no test coverage detected