|
|
@@ -166,11 +166,9 @@ shared_ptr<Preprocess> p_pre(new Preprocess());
|
|
|
shared_ptr<ImuProcess> p_imu(new ImuProcess());
|
|
|
|
|
|
/*back end*/
|
|
|
-vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames; // 历史所有关键帧的角点集合（降采样）
|
|
|
-vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames; // 历史所有关键帧的平面点集合（降采样）
|
|
|
-
|
|
|
+keyFramesManager keyFrames_;
|
|
|
pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D(new pcl::PointCloud<PointType>()); // 历史关键帧位姿（位置）
|
|
|
-pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D(new pcl::PointCloud<PointTypePose>()); // 历史关键帧位姿
|
|
|
+
|
|
|
pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D(new pcl::PointCloud<PointType>());
|
|
|
pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D(new pcl::PointCloud<PointTypePose>());
|
|
|
|
|
|
@@ -512,7 +510,7 @@ bool saveFrame()
|
|
|
return true;
|
|
|
|
|
|
// 前一帧位姿
|
|
|
- Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
|
|
|
+ Eigen::Affine3f transStart = pclPointToAffine3f(keyFrames_.back().Pose());
|
|
|
// 当前帧位姿
|
|
|
Eigen::Affine3f transFinal = trans2Affine3f(transformTobeMapped);
|
|
|
// Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
|
|
|
@@ -544,17 +542,26 @@ void addOdomFactor()
|
|
|
gtSAMgraph.add(gtsam::PriorFactor<gtsam::Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
|
|
|
// 变量节点设置初始值
|
|
|
initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
|
|
|
+ gtsam::Pose3 init=trans2gtsamPose(transformTobeMapped);
|
|
|
+ printf(" add 0 init 0 :%f %f %f %f %f %f\n",init.x(),init.y(),init.z(),
|
|
|
+ init.rotation().roll(),init.rotation().pitch(),init.rotation().yaw());
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
// 添加激光里程计因子
|
|
|
gtsam::noiseModel::Diagonal::shared_ptr odometryNoise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
|
|
|
- gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back()); /// pre
|
|
|
+
|
|
|
+ gtsam::Pose3 poseFrom = pclPointTogtsamPose3(keyFrames_.back().Pose()); /// pre
|
|
|
gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped); // cur
|
|
|
// 参数：前一帧id，当前帧id，前一帧与当前帧的位姿变换（作为观测值），噪声协方差
|
|
|
gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(cloudKeyPoses3D->size() - 1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
|
|
|
// 变量节点设置初始值
|
|
|
initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
|
|
|
+ printf(" add between :%d to %d init %d:%f %f %f %f %f %f\n",
|
|
|
+ cloudKeyPoses3D->size()-1,cloudKeyPoses3D->size(),cloudKeyPoses3D->size(),
|
|
|
+ poseTo.x(),poseTo.y(),poseTo.z(),
|
|
|
+ poseTo.rotation().roll(),poseTo.rotation().pitch(),poseTo.rotation().yaw());
|
|
|
+
|
|
|
}
|
|
|
}
|
|
|
|
|
|
@@ -593,7 +600,7 @@ void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr &nearKeyframes, const
|
|
|
// 提取key索引的关键帧前后相邻若干帧的关键帧特征点集合
|
|
|
nearKeyframes->clear();
|
|
|
int cloudSize = copy_cloudKeyPoses6D->size();
|
|
|
- auto surfcloud_keyframes_size = surfCloudKeyFrames.size() ;
|
|
|
+ auto surfcloud_keyframes_size = keyFrames_.size() ;
|
|
|
for (int i = -searchNum; i <= searchNum; ++i)
|
|
|
{
|
|
|
int keyNear = key + i;
|
|
|
@@ -603,9 +610,10 @@ void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr &nearKeyframes, const
|
|
|
if (keyNear < 0 || keyNear >= surfcloud_keyframes_size)
|
|
|
continue;
|
|
|
|
|
|
- // *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]);
|
|
|
// 注意：cloudKeyPoses6D 存储的是 T_w_b , 而点云是lidar系下的，构建icp的submap时，需要通过外参数T_b_lidar 转换 , 参考pointBodyToWorld 的转换
|
|
|
- *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]); // fast-lio 没有进行特征提取，默认点云就是surf
|
|
|
+ Eigen::Isometry3d T_b_lidar(state_point.offset_R_L_I ); // 获取 body2lidar 外参
|
|
|
+ T_b_lidar.pretranslate(state_point.offset_T_L_I);
|
|
|
+ *nearKeyframes += *keyFrames_[keyNear].world_cloud(T_b_lidar);
|
|
|
}
|
|
|
|
|
|
if (nearKeyframes->empty())
|
|
|
@@ -628,12 +636,12 @@ void performLoopClosure() {
|
|
|
|
|
|
mtx.lock();
|
|
|
*copy_cloudKeyPoses3D = *cloudKeyPoses3D;
|
|
|
- *copy_cloudKeyPoses6D = *cloudKeyPoses6D;
|
|
|
+ *copy_cloudKeyPoses6D = *keyFrames_.GetPoses();
|
|
|
mtx.unlock();
|
|
|
|
|
|
// 当前关键帧索引，候选闭环匹配帧索引
|
|
|
static int LastKeyFrameSize=0;//;
|
|
|
- if(LastKeyFrameSize>=surfCloudKeyFrames.size()) //没有更新的keyframe 无需检测
|
|
|
+ if(LastKeyFrameSize>=keyFrames_.size()) //没有更新的keyframe 无需检测
|
|
|
return ;
|
|
|
|
|
|
int loopKeyCur;
|
|
|
@@ -647,7 +655,7 @@ void performLoopClosure() {
|
|
|
return;
|
|
|
|
|
|
auto detectResult = scManager.detectLoopClosureID(); // first: nn index, second: yaw diff
|
|
|
- LastKeyFrameSize=surfCloudKeyFrames.size();
|
|
|
+ LastKeyFrameSize=keyFrames_.size();
|
|
|
int SCclosestHistoryFrameID = detectResult.first;
|
|
|
if( SCclosestHistoryFrameID != -1 ) {
|
|
|
|
|
|
@@ -680,12 +688,7 @@ void performLoopClosure() {
|
|
|
float noiseScore;
|
|
|
Eigen::Affine3f correctionLidarFrame;
|
|
|
float x, y, z, roll, pitch, yaw;
|
|
|
- /*PointTypePose current = copy_cloudKeyPoses6D->points[loopKeyCur];
|
|
|
- PointTypePose target = copy_cloudKeyPoses6D->points[loopKeyPre];
|
|
|
- gtsam::Pose3 poseSrc(gtsam::Rot3::RzRyRx(double(current.roll), double(current.pitch), double(current.yaw)),
|
|
|
- gtsam::Point3(double(current.x), double(current.y), double(current.z)));
|
|
|
- gtsam::Pose3 poseTar(gtsam::Rot3::RzRyRx(double(target.roll), double(target.pitch), double(target.yaw)),
|
|
|
- gtsam::Point3(double(target.x), double(target.y), double(target.z)));*/
|
|
|
+
|
|
|
// ICP Settings
|
|
|
if(ndt_loop==false) {
|
|
|
pcl::IterativeClosestPoint<PointType, PointType> icp;
|
|
|
@@ -782,12 +785,17 @@ void saveKeyFramesAndFactor()
|
|
|
// 激光里程计因子(from fast-lio), 输入的是frame_relative pose 帧间位姿(body 系下)
|
|
|
addOdomFactor();
|
|
|
scManager.makeAndSaveScancontextAndKeys(*feats_undistort);
|
|
|
+
|
|
|
performLoopClosure(); //闭环检测
|
|
|
+
|
|
|
visualizeLoopClosure();//展示闭环边
|
|
|
+
|
|
|
// 闭环因子 (rs-loop-detect) 基于欧氏距离的检测
|
|
|
addLoopFactor();
|
|
|
+
|
|
|
// 执行优化
|
|
|
isam->update(gtSAMgraph, initialEstimate);
|
|
|
+
|
|
|
isam->update();
|
|
|
if (aLoopIsClosed == true) // 有回环因子，多update几次
|
|
|
{
|
|
|
@@ -800,6 +808,7 @@ void saveKeyFramesAndFactor()
|
|
|
}
|
|
|
// update之后要清空一下保存的因子图，注：历史数据不会清掉，ISAM保存起来了
|
|
|
gtSAMgraph.resize(0);
|
|
|
+
|
|
|
initialEstimate.clear();
|
|
|
|
|
|
PointType thisPose3D;
|
|
|
@@ -810,7 +819,6 @@ void saveKeyFramesAndFactor()
|
|
|
isamCurrentEstimate = isam->calculateBestEstimate();
|
|
|
// 当前帧位姿结果
|
|
|
latestEstimate = isamCurrentEstimate.at<gtsam::Pose3>(isamCurrentEstimate.size() - 1);
|
|
|
-
|
|
|
// cloudKeyPoses3D加入当前帧位置
|
|
|
thisPose3D.x = latestEstimate.translation().x();
|
|
|
thisPose3D.y = latestEstimate.translation().y();
|
|
|
@@ -829,7 +837,7 @@ void saveKeyFramesAndFactor()
|
|
|
thisPose6D.pitch = latestEstimate.rotation().pitch();
|
|
|
thisPose6D.yaw = latestEstimate.rotation().yaw();
|
|
|
thisPose6D.time = lidar_end_time;
|
|
|
- cloudKeyPoses6D->push_back(thisPose6D);
|
|
|
+
|
|
|
|
|
|
// 位姿协方差
|
|
|
poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size() - 1);
|
|
|
@@ -846,32 +854,14 @@ void saveKeyFramesAndFactor()
|
|
|
// if(aLoopIsClosed == true )
|
|
|
ikfom_.change_x(state_updated); // 对cur_pose 进行isam2优化后的修正
|
|
|
|
|
|
- // TODO: P的修正有待考察，按照yanliangwang的做法，修改了p，会跑飞
|
|
|
- // esekfom::esekf<state_ikfom, 12, input_ikfom>::cov P_updated = kf.get_P(); // 获取当前的状态估计的协方差矩阵
|
|
|
- // P_updated.setIdentity();
|
|
|
- // P_updated(6, 6) = P_updated(7, 7) = P_updated(8, 8) = 0.00001;
|
|
|
- // P_updated(9, 9) = P_updated(10, 10) = P_updated(11, 11) = 0.00001;
|
|
|
- // P_updated(15, 15) = P_updated(16, 16) = P_updated(17, 17) = 0.0001;
|
|
|
- // P_updated(18, 18) = P_updated(19, 19) = P_updated(20, 20) = 0.001;
|
|
|
- // P_updated(21, 21) = P_updated(22, 22) = 0.00001;
|
|
|
- // kf.change_P(P_updated);
|
|
|
-
|
|
|
// 当前帧激光角点、平面点，降采样集合
|
|
|
// pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
|
|
|
pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
|
|
|
// pcl::copyPointCloud(*feats_undistort, *thisCornerKeyFrame);
|
|
|
pcl::copyPointCloud(*feats_undistort, *thisSurfKeyFrame); // 存储关键帧,没有降采样的点云
|
|
|
-
|
|
|
- // 保存特征点降采样集合
|
|
|
- // cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
|
|
|
- surfCloudKeyFrames.push_back(thisSurfKeyFrame);
|
|
|
-
|
|
|
- //sc关键帧保存降采样的点云
|
|
|
- /*pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
|
|
|
- downSizeFilterICP.setInputCloud(thisSurfKeyFrame);
|
|
|
- downSizeFilterICP.filter(*cloud_temp);*/
|
|
|
-
|
|
|
-
|
|
|
+ keyFrames_.push_back(keyFrame(thisPose6D,thisSurfKeyFrame));
|
|
|
+ printf(" keyFrames add %d pose: %f %f %f %f %f %f\n",keyFrames_.size(),thisPose6D.x,thisPose6D.y,thisPose6D.z,
|
|
|
+ thisPose6D.roll,thisPose6D.pitch,thisPose6D.yaw);
|
|
|
updatePath(thisPose6D); // 可视化update后的path
|
|
|
}
|
|
|
|
|
|
@@ -906,8 +896,9 @@ void recontructIKdTree(){
|
|
|
if (pointDistance(subMapKeyPosesDS->points[i], cloudKeyPoses3D->back()) > globalMapVisualizationSearchRadius)
|
|
|
continue;
|
|
|
int thisKeyInd = (int)subMapKeyPosesDS->points[i].intensity;
|
|
|
- // *globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
|
|
|
- *subMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]); // fast_lio only use surfCloud
|
|
|
+ Eigen::Isometry3d T_b_lidar(state_point.offset_R_L_I ); // 获取 body2lidar 外参
|
|
|
+ T_b_lidar.pretranslate(state_point.offset_T_L_I);
|
|
|
+ *subMapKeyFrames += *keyFrames_[thisKeyInd].world_cloud(T_b_lidar);
|
|
|
}
|
|
|
// 降采样，发布
|
|
|
pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames; // for global map visualization
|
|
|
@@ -947,15 +938,17 @@ void correctPoses()
|
|
|
cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().y();
|
|
|
cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().z();
|
|
|
|
|
|
- cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
|
|
|
- cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
|
|
|
- cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
|
|
|
- cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().roll();
|
|
|
- cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().pitch();
|
|
|
- cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().yaw();
|
|
|
+ PointTypePose new_pose;
|
|
|
+ new_pose.x = cloudKeyPoses3D->points[i].x;
|
|
|
+ new_pose.y = cloudKeyPoses3D->points[i].y;
|
|
|
+ new_pose.z = cloudKeyPoses3D->points[i].z;
|
|
|
+ new_pose.roll = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().roll();
|
|
|
+ new_pose.pitch = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().pitch();
|
|
|
+ new_pose.yaw = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().yaw();
|
|
|
+ keyFrames_[i].ResetPose(new_pose);
|
|
|
|
|
|
// 更新里程计轨迹
|
|
|
- updatePath(cloudKeyPoses6D->points[i]);
|
|
|
+ updatePath(keyFrames_[i].Pose());
|
|
|
}
|
|
|
// 清空局部map， reconstruct ikdtree submap
|
|
|
recontructIKdTree();
|
|
|
@@ -1643,9 +1636,9 @@ bool savePoseService(fast_lio_sam::save_poseRequest& req, fast_lio_sam::save_pos
|
|
|
CreateFile(file_pose_without_optimized, savePoseDirectory + "/without_optimized_pose.txt");
|
|
|
|
|
|
// save optimize data
|
|
|
- for(int i = 0; i < cloudKeyPoses6D->size(); i++){
|
|
|
- pose_optimized.t = Eigen::Vector3d(cloudKeyPoses6D->points[i].x, cloudKeyPoses6D->points[i].y, cloudKeyPoses6D->points[i].z );
|
|
|
- pose_optimized.R = Exp(double(cloudKeyPoses6D->points[i].roll), double(cloudKeyPoses6D->points[i].pitch), double(cloudKeyPoses6D->points[i].yaw) );
|
|
|
+ for(int i = 0; i < keyFrames_.size(); i++){
|
|
|
+ pose_optimized.t = Eigen::Vector3d(keyFrames_[i].Pose().x, keyFrames_[i].Pose().y,keyFrames_[i].Pose().z );
|
|
|
+ pose_optimized.R = Exp(double(keyFrames_[i].Pose().roll), double(keyFrames_[i].Pose().pitch), double(keyFrames_[i].Pose().yaw) );
|
|
|
WriteText(file_pose_optimized, pose_optimized);
|
|
|
}
|
|
|
cout << "Sucess global optimized poses to pose files ..." << endl;
|
|
|
@@ -1682,19 +1675,18 @@ bool saveMapService(fast_lio_sam::save_mapRequest& req, fast_lio_sam::save_mapRe
|
|
|
// unused = system((std::string("mkdir -p ") + saveMapDirectory).c_str());
|
|
|
// 保存历史关键帧位姿
|
|
|
pcl::io::savePCDFileBinary(saveMapDirectory + "/trajectory.pcd", *cloudKeyPoses3D); // 关键帧位置
|
|
|
- pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *cloudKeyPoses6D); // 关键帧位姿
|
|
|
+ pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *(keyFrames_.GetPoses())); // 关键帧位姿
|
|
|
// 提取历史关键帧点集合
|
|
|
|
|
|
pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());
|
|
|
pcl::PointCloud<PointType>::Ptr globalSurfCloudDS(new pcl::PointCloud<PointType>());
|
|
|
pcl::PointCloud<PointType>::Ptr globalMapCloud(new pcl::PointCloud<PointType>());
|
|
|
|
|
|
- // 注意：拼接地图时，keyframe是lidar系，而fastlio更新后的存到的cloudKeyPoses6D 关键帧位姿是body系下的，需要把
|
|
|
- //cloudKeyPoses6D 转换为T_world_lidar 。 T_world_lidar = T_world_body * T_body_lidar , T_body_lidar 是外参
|
|
|
- for (int i = 0; i < (int)cloudKeyPoses6D->size(); i++) {
|
|
|
- // *globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
|
|
|
- *globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
|
|
|
- cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";
|
|
|
+ for (int i = 0; i < (int)keyFrames_.size(); i++) {
|
|
|
+ Eigen::Isometry3d T_b_lidar(state_point.offset_R_L_I ); // 获取 body2lidar 外参
|
|
|
+ T_b_lidar.pretranslate(state_point.offset_T_L_I);
|
|
|
+ *globalSurfCloud += *keyFrames_[i].world_cloud(T_b_lidar);
|
|
|
+ cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << keyFrames_.size() << " ...";
|
|
|
}
|
|
|
|
|
|
if(req.resolution != 0)
|
|
|
@@ -1788,8 +1780,9 @@ void publishGlobalMap()
|
|
|
if (pointDistance(globalMapKeyPosesDS->points[i], cloudKeyPoses3D->back()) > globalMapVisualizationSearchRadius)
|
|
|
continue;
|
|
|
int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;
|
|
|
- // *globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
|
|
|
- *globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]); // fast_lio only use surfCloud
|
|
|
+ Eigen::Isometry3d T_b_lidar(state_point.offset_R_L_I ); // 获取 body2lidar 外参
|
|
|
+ T_b_lidar.pretranslate(state_point.offset_T_L_I);
|
|
|
+ *globalMapKeyFrames += *keyFrames_[thisKeyInd].world_cloud(T_b_lidar);
|
|
|
}
|
|
|
// 降采样，发布
|
|
|
pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames; // for global map visualization
|
|
|
@@ -2061,6 +2054,7 @@ int main(int argc, char **argv) {
|
|
|
gtsam::ISAM2Params parameters;
|
|
|
parameters.relinearizeThreshold = 0.01;
|
|
|
parameters.relinearizeSkip = 1;
|
|
|
+ parameters.factorization=gtsam::ISAM2Params::QR;
|
|
|
isam = new gtsam::ISAM2(parameters);
|
|
|
|
|
|
path.header.stamp = ros::Time::now();
|
|
|
@@ -2205,7 +2199,6 @@ int main(int argc, char **argv) {
|
|
|
ikf_lock_.unlock();
|
|
|
continue;
|
|
|
}
|
|
|
-
|
|
|
double t0, t1, t2, t3, t4, t5, match_start, solve_start, svd_time;
|
|
|
|
|
|
match_time = 0;
|
|
|
@@ -2228,38 +2221,16 @@ int main(int argc, char **argv) {
|
|
|
continue;
|
|
|
}
|
|
|
|
|
|
+
|
|
|
// 检查当前lidar数据时间，与最早lidar数据时间是否足够,两帧间隔0.1s
|
|
|
flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? false : true;
|
|
|
|
|
|
/*** Segment the map in lidar FOV ***/
|
|
|
if (locate_mode == false)
|
|
|
lasermap_fov_segment(); // 根据lidar在W系下的位置，重新确定局部地图的包围盒角点，移除远端的点
|
|
|
- else {
|
|
|
- /*** downsample the feature points in a scan ***/
|
|
|
- double ndt_beg = omp_get_wtime();
|
|
|
- downSizeFilterSurf.setInputCloud(feats_undistort);
|
|
|
- downSizeFilterSurf.filter(*feats_down_body);
|
|
|
-
|
|
|
- pcl::NormalDistributionsTransform<PointType, PointType> ndt;
|
|
|
- ndt.setTransformationEpsilon(0.001);
|
|
|
- // Setting maximum step size for More-Thuente line search.
|
|
|
- ndt.setStepSize(0.1);
|
|
|
- //Setting Resolution of NDT grid structure (VoxelGridCovariance).
|
|
|
- ndt.setResolution(0.5);
|
|
|
- // Setting max number of registration iterations.
|
|
|
- ndt.setMaximumIterations(50);
|
|
|
- // Setting point cloud to be aligned.
|
|
|
- ndt.setInputSource(feats_down_body);
|
|
|
- // Setting point cloud to be aligned to.
|
|
|
- ndt.setInputTarget(featsFromMap);
|
|
|
- pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
|
|
|
- ndt.align(*unused_result, Eigen::Matrix4f::Identity());
|
|
|
- double ndt_end = omp_get_wtime();
|
|
|
-
|
|
|
- printf(" ndt time :%f , size(%d %d)\n", ndt_end - ndt_beg, feats_down_body->size(), featsFromMap->size());
|
|
|
-
|
|
|
- }
|
|
|
|
|
|
+ downSizeFilterSurf.setInputCloud(feats_undistort);
|
|
|
+ downSizeFilterSurf.filter(*feats_down_body);
|
|
|
t1 = omp_get_wtime();
|
|
|
feats_down_size = feats_down_body->points.size(); //当前帧降采样后点数
|
|
|
|
|
|
@@ -2276,6 +2247,7 @@ int main(int argc, char **argv) {
|
|
|
ikdtree.Build(feats_down_world->points);
|
|
|
}
|
|
|
ikf_lock_.unlock();
|
|
|
+
|
|
|
continue;
|
|
|
}
|
|
|
}
|
|
|
@@ -2315,11 +2287,7 @@ int main(int argc, char **argv) {
|
|
|
bool matched = ikfom_.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);//预测、更新
|
|
|
state_point = ikfom_.get_x();
|
|
|
last_state_point = state_point;
|
|
|
- /*if (matched==false && locate_mode==true) {
|
|
|
- printf(" ikfom state update failed ...continue...\n");
|
|
|
- ikfom_.change_x(last_state_point);//恢复到前一帧的数据
|
|
|
- state_point = last_state_point;
|
|
|
- }*/
|
|
|
+
|
|
|
|
|
|
euler_cur = SO3ToEuler(state_point.rot);
|
|
|
pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; // world系下lidar坐标
|
|
|
@@ -2329,6 +2297,7 @@ int main(int argc, char **argv) {
|
|
|
geoQuat.w = state_point.rot.coeffs()[3];
|
|
|
double t_update_end = omp_get_wtime();
|
|
|
getCurPose(state_point); // 更新transformTobeMapped
|
|
|
+
|
|
|
// Publish points
|
|
|
if (scan_pub_en) {
|
|
|
publish_frame_world(pubLaserCloudFull); // 发布world系下的点云
|
|
|
@@ -2350,6 +2319,7 @@ int main(int argc, char **argv) {
|
|
|
saveKeyFramesAndFactor();
|
|
|
// 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹， 重构ikdtree
|
|
|
correctPoses();
|
|
|
+
|
|
|
}
|
|
|
double tloop = omp_get_wtime();
|
|
|
/*printf(" match time:%fs,kdtree points:%d save KeyFrame loop time:%fs\n", t_update_end - t_update_start,
|
