Freeecode / loopclosure_for_aloam

A simple Loopclosure detection for A-LOAM
13 stars 3 forks source link

test version #1

Open pxc2017 opened 4 years ago

pxc2017 commented 4 years ago

hello,i also want to add a loop closure module in aloam.just don’t know how to handle it,could you please open the test version?thank you very much.

Freeecode commented 4 years ago

Hi! in order to add a loop closure module, you can refer to lego-loam. The test version still has some issues,which need to be fixed.

pxc2017 commented 3 years ago

hello,打扰了。看大佬简介应该是国人吧,最近在改aloam,检测回环后gtsam报错,大佬知道是怎么回事吗,困扰两天了不知道哪里的问题,可以交流一下吗:D 2020-09-01 16-58-59 的屏幕截图

Freeecode commented 3 years ago

可能是因子图构建存在问题,检查一下因子图构建过程中的噪声模型、先验因子、位姿之间的约束添加是否正确

pxc2017 commented 3 years ago

感谢回复,之前用ndt加这个设置的后端跑通了,现在加到aloam里面跑同一个数据结果报错,最近将lio-sam中保存关键帧函数和回环检测线程也类比加到aloam的ceres优化之后,还是报一样的错误,用的都是同一个数据包,都是在匹配到回环帧后报上面的错误,这个应该怎么查,大佬有招吗?感谢 ` /parameter/ ISAM2Params params;
params.relinearizeThreshold = 0.01;
params.relinearizeSkip = 1;
isam = new ISAM2(params);
gtsam::Vector vector6(6);
vector6 << 1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-6;
priornoise = noiseModel::Diagonal::Variances(vector6);
odomnoise = noiseModel::Diagonal::Variances(vector6);

/priorfactor/ gtSAMgraph_.add(PriorFactor(0, Pose3(Rot3::Quaternion(q_w_curr.w(), q_w_curr.x(), q_w_curr.y(), q_w_curr.z()), Point3(t_w_curr.x(), t_w_curr.y(), t_w_curr.z())), priornoise));

/**odomfactor**/ gtsam::Pose3 pose_from = Pose3(Rot3::RzRyRx(pre_pose.roll, pre_pose.pitch, pre_pose.yaw), Point3(pre_pose.x, pre_pose.y, pre_pose.z)); gtsam::Pose3 pose_to = Pose3(Rot3::Quaternion(q_w_curr.w(), q_w_curr.x(), q_w_curr.y(), q_w_curr.z()), Point3(t_w_curr.x(), t_w_curr.y(), t_wcurr.z())); gtSAMgraph.add(BetweenFactor(cloud_keyposes3d->points.size() - 1, cloud_keyposes3d->points.size(), pose_from.between(pose_to), odomnoise));

/*loopclosure factor**/ Eigen::Matrix4f t_wrong = initial_guess;//当前关键帧位姿 Eigen::Matrix4f t_correct = correction_frame t_wrong;//ICP 当前关键帧位姿 // Eigen::Matrix4f t_correct = correction_frame; Eigen::Quaternionf r_correct(t_correct.block<3, 3>(0, 0)); gtsam::Pose3 pose_from = Pose3(Rot3::Quaternion(r_correct.w(), r_correct.x(), r_correct.y(), r_correct.z()), Point3(t_correct(0, 3), t_correct(1, 3), t_correct(2, 3))); gtsam::Pose3 pose_to = Pose3(Rot3::RzRyRx(cloud_keyposes6d->points[closest_history_frameid].roll, cloud_keyposes6d->points[closest_history_frameid].pitch, cloud_keyposes6d->points[closest_history_frameid].yaw), Point3(cloud_keyposes6d->points[closest_history_frameid].x, cloud_keyposes6d->points[closest_history_frameid].y, cloud_keyposes6d->points[closest_history_frameid].z)); float noise_score = fitness_score; gtsam::Vector vector6(6); vector6 << noise_score, noise_score, noise_score, noise_score, noise_score, noise_score; constraintnoise = noiseModel::Diagonal::Variances(vector6);`

Freeecode commented 3 years ago

别叫我大佬了,承受不起。你是添加玩回环约束后,进行优化出的问题吗?如果是的话,可以和ndt那个比较一下,回环处的当前帧和历史帧之间的相对变换

pxc2017 commented 3 years ago

哈哈能者为师,之前用的ndt+loop+gtsam没问题,而且在闭合处有很大的相对位姿也能矫正,我当时设的搜索半径都到30m了,但是把这一套搬到aloam里面时,过程中还加了GPS约束,闭合处的相对位姿其实已经比较小了,但就是在匹配到回环那一下出上面的错,很是烦恼。都不知道为啥

皮新成

邮箱:18813127865@163.com |

签名由 网易邮箱大师 定制

On 09/04/2020 23:05, Houzhan Zhang wrote:

别叫我大佬了,承受不起。你是添加玩回环约束后,进行优化出的问题吗?如果是的话,可以和ndt那个比较一下,回环处的当前帧和历史帧之间的相对变换

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub, or unsubscribe.

pxc2017 commented 3 years ago

感谢深夜回复 image image 这是出错的截图,真不知道为啥会出现这种情况 休息吧,感谢

Freeecode commented 3 years ago

能详细说下到哪一步出问题吗?

pxc2017 commented 3 years ago

其实我确实还没准确定位到错误,就现象而言,加入了gpsfactor后gtsam会隔段时间有优化输出执行correctpose,而在那个闭合的十字路口处icp匹配到了相似帧,进行发布了,也就是在订阅到发不出来的匹配后相似帧时gtsam报错,整个回环检测线程也终止了,但同样的回环检测放在之前的ndt后面就没问题。而且后来另外又把liosam的回环和优化换进来 还是同样的问题。🤷‍♂️