| 1181 | |
| 1182 | |
| 1183 | void saveKeyFramesAndFactor() |
| 1184 | { |
| 1185 | int indoorJudgement = saveFrame(); |
| 1186 | if ( indoorJudgement < 0) |
| 1187 | return; |
| 1188 | |
| 1189 | if (localizationMode) |
| 1190 | { |
| 1191 | *errCloudAll += *errCloud; |
| 1192 | } |
| 1193 | |
| 1194 | |
| 1195 | isIndoorKeyframe.push_back(indoorJudgement); |
| 1196 | |
| 1197 | //save key poses |
| 1198 | PointType thisPose3D; |
| 1199 | PointTypePose thisPose6D; |
| 1200 | thisPose3D.x = transformTobeMapped[3]; |
| 1201 | thisPose3D.y = transformTobeMapped[4]; |
| 1202 | thisPose3D.z = transformTobeMapped[5]; |
| 1203 | thisPose3D.intensity = cloudKeyPoses3D->size(); // this can be used as keyframe index |
| 1204 | cloudKeyPoses3D->push_back(thisPose3D); |
| 1205 | |
| 1206 | thisPose6D.x = thisPose3D.x; |
| 1207 | thisPose6D.y = thisPose3D.y; |
| 1208 | thisPose6D.z = thisPose3D.z; |
| 1209 | thisPose6D.intensity = thisPose3D.intensity ; // this can be used as index |
| 1210 | thisPose6D.roll = transformTobeMapped[0]; |
| 1211 | thisPose6D.pitch = transformTobeMapped[1]; |
| 1212 | thisPose6D.yaw = transformTobeMapped[2]; |
| 1213 | thisPose6D.time = cloudInfoTime; |
| 1214 | cloudKeyPoses6D->push_back(thisPose6D); |
| 1215 | |
| 1216 | // save all the received edge and surf points |
| 1217 | pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>()); |
| 1218 | pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>()); |
| 1219 | |
| 1220 | pcl::copyPointCloud(*lidarCloudCornerLast, *thisCornerKeyFrame); |
| 1221 | pcl::copyPointCloud(*lidarCloudSurfLast, *thisSurfKeyFrame); |
| 1222 | // save key frame cloud |
| 1223 | cornerCloudKeyFrames.push_back(thisCornerKeyFrame); // 这个全局都存着，但每次局部匹配只搜索50m内的关键帧 |
| 1224 | surfCloudKeyFrames.push_back(thisSurfKeyFrame); |
| 1225 | |
| 1226 | |
| 1227 | |
| 1228 | |
| 1229 | } |
| 1230 | |
| 1231 | void publishOdometry() |
| 1232 | { |