| 2319 | } |
| 2320 | |
| 2321 | void saveTemporaryKeyframes() |
| 2322 | { |
| 2323 | //save temporary key poses |
| 2324 | int isIndoorJudgement = saveFrame(); |
| 2325 | if ( isIndoorJudgement < 0 || frameTobeAbandoned) return; |
| 2326 | |
| 2327 | isIndoorKeyframeTMM.push_back(isIndoorJudgement); |
| 2328 | int temporaryKeyPoseSize = temporaryCloudKeyPoses3D->size(); |
| 2329 | PointType thisPose3D; |
| 2330 | PointTypePose thisPose6D; |
| 2331 | thisPose3D.x = transformTobeMapped[3]; |
| 2332 | thisPose3D.y = transformTobeMapped[4]; |
| 2333 | thisPose3D.z = transformTobeMapped[5]; |
| 2334 | thisPose3D.intensity = temporaryKeyPoseSize; // this can be used as keyframe index |
| 2335 | |
| 2336 | thisPose6D.x = thisPose3D.x; |
| 2337 | thisPose6D.y = thisPose3D.y; |
| 2338 | thisPose6D.z = thisPose3D.z; |
| 2339 | thisPose6D.intensity = thisPose3D.intensity ; // this can be used as index |
| 2340 | thisPose6D.roll = transformTobeMapped[0]; |
| 2341 | thisPose6D.pitch = transformTobeMapped[1]; |
| 2342 | thisPose6D.yaw = transformTobeMapped[2]; |
| 2343 | thisPose6D.time = cloudInfoTime; |
| 2344 | |
| 2345 | temporaryCloudKeyPoses3D->push_back(thisPose3D); |
| 2346 | temporaryCloudKeyPoses6D->push_back(thisPose6D); |
| 2347 | // save all the received edge and surf points |
| 2348 | pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>()); |
| 2349 | pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>()); |
| 2350 | pcl::copyPointCloud(*lidarCloudCornerLast, *thisCornerKeyFrame); |
| 2351 | pcl::copyPointCloud(*lidarCloudSurfLast, *thisSurfKeyFrame); |
| 2352 | // save key frame cloud |
| 2353 | temporaryCornerCloudKeyFrames.push_back(thisCornerKeyFrame); // 这个全局都存着，但每次局部匹配只搜索50m内的关键帧 |
| 2354 | temporarySurfCloudKeyFrames.push_back(thisSurfKeyFrame); |
| 2355 | |
| 2356 | if (!temporaryMappingMode && temporaryKeyPoseSize > slidingWindowSize) // sliding-window local map |
| 2357 | { |
| 2358 | temporaryCloudKeyPoses3D->erase(temporaryCloudKeyPoses3D->begin()); |
| 2359 | temporaryCloudKeyPoses6D->erase(temporaryCloudKeyPoses6D->begin()); |
| 2360 | temporaryCornerCloudKeyFrames.erase(temporaryCornerCloudKeyFrames.begin()); |
| 2361 | temporarySurfCloudKeyFrames.erase(temporarySurfCloudKeyFrames.begin()); |
| 2362 | isIndoorKeyframeTMM.erase(isIndoorKeyframeTMM.begin()); |
| 2363 | // reindexing: key poses and key frames are corresponding with respect to the adding sequence |
| 2364 | for (int i = 0 ; i< (int)temporaryCloudKeyPoses3D->size(); i++) |
| 2365 | { |
| 2366 | temporaryCloudKeyPoses3D->points[i].intensity = i; |
| 2367 | temporaryCloudKeyPoses6D->points[i].intensity = i; |
| 2368 | } |
| 2369 | } |
| 2370 | |
| 2371 | } |
| 2372 | |
| 2373 | void saveKeyFramesAndFactor() |
| 2374 | { |