test version

[pxc2017]

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]

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]

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

[Freeecode]

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

[pxc2017]

感谢回复，之前用ｎｄｔ加这个设置的后端跑通了，现在加到ａｌｏａｍ里面跑同一个数据结果报错，最近将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;
prior_noise_ = noiseModel::Diagonal::Variances(vector6);
odom_noise_ = 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())), prior_noise_));

/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_w_curr.z()));
gtSAMgraph_.add(BetweenFactor(cloud_keyposes_3d_->points.size() - 1, cloud_keyposes_3d_->points.size(), pose_from.between(pose_to), odom_noise_));

/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_keyposes_6d_->points[closest_history_frame_id_].roll, cloud_keyposes_6d_->points[closest_history_frame_id_].pitch, cloud_keyposes_6d_->points[closest_history_frame_id_].yaw),
Point3(cloud_keyposes_6d_->points[closest_history_frame_id_].x, cloud_keyposes_6d_->points[closest_history_frame_id_].y, cloud_keyposes_6d_->points[closest_history_frame_id_].z));
float noise_score = fitness_score;
gtsam::Vector vector6(6);
vector6 << noise_score, noise_score, noise_score, noise_score, noise_score, noise_score;
constraint_noise_ = noiseModel::Diagonal::Variances(vector6);`

[Freeecode]

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

[pxc2017]

哈哈能者为师，之前用的ndt+loop+gtsam没问题，而且在闭合处有很大的相对位姿也能矫正，我当时设的搜索半径都到30m了，但是把这一套搬到aloam里面时，过程中还加了GPS约束，闭合处的相对位姿其实已经比较小了，但就是在匹配到回环那一下出上面的错，很是烦恼。都不知道为啥 | | 皮新成 | | 邮箱：[email protected] | 签名由 网易邮箱大师 定制 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]

感谢深夜回复


这是出错的截图，真不知道为啥会出现这种情况
休息吧，感谢

[Freeecode]

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

[pxc2017]

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