void Mapserver::closed_loop_detection_graph(int max_frame_size, bool use_graph_pose)
{
// 更新进度
std::stringstream _mesg;
_mesg << "开始优化位姿,截止帧ID:" << max_frame_size - 1;
// ROS_INFO_STREAM(_mesg.str(););
sendBuildMessageToPC(14, "信息", "建图程序", "更新地图过程中", _mesg, 0, 10);
// 初始化闭环
//points_map_.loop_edge_size = 0;
// 闭环的帧
frame _closed_frame;
// 闭环的帧
int _closed_frame_index;
// 最好i评分
double _best_score;
// 距离平方
double _distance_sqr;
// 级别差值
int _diffe_level;
/// @brief 弧度差距
double _diffe_yaw;
// 评分
double _score;
// 先复位帧
// if (false)
// {
// // 用来恢复初值
// points_map_.frames[0].odom = points_map_.frames[0].pose;
// for (int i = 0; i < max_frame_size && i < points_map_.frame_size; ++i)
// {
// // 没有固定的恢复初数值
// if (i > points_map_.fixed_frame_index)
// points_map_.frames[i].odom = points_map_.frames[points_map_.frames_edge[i].seq_i].odom * points_map_.frames_edge[i].measure;
// else // 固定的就算pose
// points_map_.frames[i].odom = points_map_.frames[i].pose;
// }
// std::stringstream _mesg2;
// _mesg2 << "位姿优化复位结束,截止帧ID:" << max_frame_size - 1;
// sendBuildMessageToPC(14, "信息", "建图程序", "更新地图过程中", _mesg2, 0, 10);
// }
// int _diffe_level_min = (floor)(5.5 / mapping_params_.distance_step);
// 6米内的都参与计算
double _max_sqr = mapping_params_.loop_search_range;
//_max_sqr=6.9;
//_max_sqr = 5.2;
// 以下代码为小闭环用的代码
// int _diffe_level_min = (ceil)(_max_sqr / mapping_params_.distance_step * 1.1);
// if (_diffe_level_min > 9)
// _diffe_level_min = 9;
int _diffe_level_min = mapping_params_.loop_min_level;
//_diffe_level_min=3;
//_diffe_level_min = 5;
////
// double _max_sqr = 3.6;
double _dis_max_sqr = _max_sqr * _max_sqr;
std::vector<Eigen::Vector2i> _near_frames;
bool small_close_loop=false;
//for (int i_1 = 0; i_1 < max_frame_size && i_1 < points_map_.frame_size; i_1+=5)
for (int i_1 = 0; i_1 < max_frame_size && i_1 < points_map_.frame_size; ++i_1)
{
if(i_1%2==0)
small_close_loop=true;
else
small_close_loop=false;
// 三级后再开始找回环,并且不是长巷道,帧也没有被弃用
if (points_map_.frames[i_1].level > _diffe_level_min && points_map_.frames[i_1].status != 3 && points_map_.frames[i_1].status != 7&&points_map_.frames[i_1].close_index==0) // points_map_.frames[i_1].num_links == 1 &&
{
_near_frames.clear();
// 初始化评分
_best_score = 0;
_score = 0;
// 轮训其他帧,找到合适闭环的
int _i_2_max = std::min(max_frame_size, points_map_.frame_size);
if(small_close_loop)
_i_2_max=i_1-1;
//_i_2_max = i_1 - _diffe_level_min+1;
//for (int i_2 = 1; i_2 < _i_2_max; i_2+=5)
if(points_map_.frames[i_1].closeloop_num<=3)
for (int i_2 = 1; i_2 < _i_2_max; ++i_2)
{
// 同一个点跳过
if (i_1 == i_2)
continue;
// 弃用帧跳过
if (points_map_.frames[i_2].status == 7)
continue;
// 长巷道跳过
// if (points_map_.frames[i_2].status!=3)
// continue;
// 是取回环前的位子还是回环后的位子
if (!use_graph_pose)
_distance_sqr = points_map_.frames[i_1].pose.getSquare(points_map_.frames[i_2].pose);
else
_distance_sqr = points_map_.frames[i_1].pose.getSquare(points_map_.frames[i_2].pose);
_diffe_yaw = points_map_.frames[i_1].pose.getYaw() - points_map_.frames[i_2].pose.getYaw() ;
/// 角度差异也加入运算
if(_diffe_yaw>M_PI)_diffe_yaw-=M_PI;
else if(_diffe_yaw<=-M_PI)_diffe_yaw+=M_PI;
_diffe_yaw=abs(_diffe_yaw);
_diffe_level = points_map_.frames[i_1].level - points_map_.frames[i_2].level;
/// 往前闭合
if (small_close_loop)
_diffe_level_min = 4;
else
_diffe_level_min = mapping_params_.loop_min_level;
if (small_close_loop)
_dis_max_sqr = 4.5 * 4.5 ;
else
_dis_max_sqr = _max_sqr * _max_sqr;
// 距离小于3.6米的才闭环,节点级别差N代
// ROS_INFO_STREAM("_distance_sqr"<<_distance_sqr<<"_diffe_level"<<_diffe_level);
if (_distance_sqr < _dis_max_sqr && abs(_diffe_level) >= _diffe_level_min)
{
// 目标级别差越好,距离越近评分越高
// _score = abs(_diffe_level) * 13.0 + (_max_sqr - sqrt(_distance_sqr)) * 10.0-_diffe_yaw*20.0;
_score = abs(_diffe_level) * 13.0 + (_max_sqr - sqrt(_distance_sqr)) * 50.0-_diffe_yaw*20.0;
/// 单雷达最看重角度
if(mapping_params_.scanner_num==1)
_score = abs(_diffe_level) * 13.0 + (_max_sqr - sqrt(_distance_sqr)) * 50.0-_diffe_yaw*40.0;
/// 如果_diffe_level>0,表示目标级别低,时间早,精度高,闭环效果好,则增加个固定评分
if(_diffe_level>0)
_score+=100.0;
Eigen::Vector2i _near_frame = Eigen::Vector2i(i_2, (int)_score);
_near_frames.push_back(_near_frame);
if (_score > _best_score)
{
_closed_frame = points_map_.frames[i_2];
_closed_frame_index = i_2;
_best_score = _score;
}
}
}
else if(points_map_.frames[i_1].close_index>=1)
{
_near_frames.push_back(Eigen::Vector2i(points_map_.frames[i_1].close_index,100));
}
if (_near_frames.size() > 0)
{
// 降顺序排列
std::sort(_near_frames.begin(), _near_frames.end(), [](Eigen::Vector2i a, Eigen::Vector2i b)
{ return a(1) > b(1); });
bool _scan_matcher_ok = false;
// 设置帧建匹配过来的关键帧作为点云
std::shared_ptr<frame> _source_cloud_ptr = std::make_shared<frame>(points_map_.frames[i_1]);
scan_matcher_build_->setSourceCloud(_source_cloud_ptr);
// 限制在10个以内
for (int i_2 = 0; i_2 < _near_frames.size() && i_2 <= 8; ++i_2)
{
scan_matcher_build_->initMatch();
_closed_frame_index = _near_frames[i_2](0);
std::shared_ptr<frame> _target_cloud_ptr = std::make_shared<frame>(points_map_.frames[_closed_frame_index]);
scan_matcher_build_->setTargetCloud(_target_cloud_ptr);
PoseWithCov _deviation;
// if (!use_graph_pose)
// _deviation = points_map_.frames[_closed_frame_index].pose.getmeasurement(points_map_.frames[i_1].pose);
// else
//好像反了
_deviation = points_map_.frames[_closed_frame_index].pose.getmeasurement(points_map_.frames[i_1].pose);
//_deviation = points_map_.frames[i_1].pose.getmeasurement(points_map_.frames[_closed_frame_index].pose);
scan_matcher_build_->setInitialGuass(_deviation);
/// 进行一次ICP的匹配
if(small_close_loop)
scan_matcher_build_->match();
else
scan_matcher_build_->match(true);
// 长巷道,不进行闭环
if (scan_matcher_build_->result_.probability_straight < 0.92 && scan_matcher_build_->result_.success && scan_matcher_build_->result_.health > mapping_params_.loop_success_threshold*1.2)
{
if (points_map_.loop_edge_size < points_map_.frame_capacity && points_map_.loop_edge_size < points_map_.loop_edge.size())
{
scan_matcher_build_small_->initMatch();
scan_matcher_build_small_->setSourceCloud(_source_cloud_ptr);
scan_matcher_build_small_->setTargetCloud(_target_cloud_ptr);
scan_matcher_build_small_->setInitialGuass(scan_matcher_build_->result_.pose);
scan_matcher_build_small_->match();
if (scan_matcher_build_small_->result_.success && scan_matcher_build_small_->result_.health > mapping_params_.loop_success_threshold)
{
// 添加回环边
measurement edge_temp;
edge_temp.seq_i = _closed_frame_index;
edge_temp.seq_j = i_1;
edge_temp.measure = scan_matcher_build_small_->result_.pose;
//edge_temp.infoMatrix = Eigen::Matrix3d::Identity();
edge_temp.infoMatrix = scan_matcher_build_small_->result_.infoMatrix;
points_map_.loop_edge[points_map_.loop_edge_size] = edge_temp;
// points_map_.loop_edge[points_map_.loop_edge_size].infoMatrix = Eigen::Matrix3d::Identity();;
points_map_.loop_edge_size++;
_scan_matcher_ok = true;
points_map_.frames[i_1].close_index = _closed_frame_index;
// ROS_INFO_STREAM("建图程序,回环成功:ID:" << i_1 << " 与ID:" << _closed_frame_index << " ,匹配健康值:" << scan_matcher_build_->result_.health);
break;
}
}
else
{
ROS_ERROR_STREAM("建图:严重错误:闭环边数量超出容量");
}
}
// 释放
_target_cloud_ptr.reset();
}
_source_cloud_ptr.reset();
if (points_map_.frames[i_1].closeloop_num < 1000)
points_map_.frames[i_1].closeloop_num++;
// if (!_scan_matcher_ok)
// ROS_WARN_STREAM("建图程序,回环失败,两帧匹配不上:ID:" << i_1 << " 与ID:" << _closed_frame_index << " ,匹配健康值:" << scan_matcher_build_->result_.health);
}
//// 闭合后的点云不再进行闭合
// if (points_map_.frames[i_1].close_index == 0)
// points_map_.frames[i_1].close_index = 99999;
// // 有闭环的点
// if (_best_score > 0)
// {
// bool _scan_matcher_ok = false;
// // 设置帧建匹配过来的关键帧作为点云
// boost::shared_ptr<frame> _source_cloud_ptr = boost::make_shared<frame>(points_map_.frames[i_1]);
// scan_matcher_build_->setSourceCloud(_source_cloud_ptr);
// boost::shared_ptr<frame> _target_cloud_ptr = boost::make_shared<frame>(points_map_.frames[_closed_frame_index]);
// scan_matcher_build_->setTargetCloud(_target_cloud_ptr);
// PoseWithCov _deviation = points_map_.frames[_closed_frame_index].pose.getmeasurement(points_map_.frames[i_1].pose);
// scan_matcher_build_->setInitialGuass(_deviation);
// /// 进行一次ICP的匹配
// scan_matcher_build_->match();
// if (scan_matcher_build_->result_.success && scan_matcher_build_->result_.health > mapping_params_.loop_success_threshold)
// {
// if (points_map_.loop_edge_size < points_map_.frame_capacity && points_map_.loop_edge_size < points_map_.loop_edge.size())
// {
// // 添加回环边
// measurement edge_temp;
// edge_temp.seq_i = _closed_frame_index;
// edge_temp.seq_j = i_1;
// edge_temp.measure = scan_matcher_build_->result_.result;
// edge_temp.infoMatrix = Eigen::Matrix3d::Identity();
// points_map_.loop_edge[points_map_.loop_edge_size] = edge_temp;
// points_map_.loop_edge_size++;
// _scan_matcher_ok = true;
// ROS_INFO_STREAM("建图程序,回环成功:ID:" << i_1 << " 与ID:" << _closed_frame_index << " ,匹配健康值:" << scan_matcher_build_->result_.health);
// }
// else
// {
// ROS_ERROR_STREAM("建图程序,回环严重错误:回环边数量:" << points_map_.loop_edge_size << " 大于等于预设容量:" << points_map_.frame_capacity << " ,或大于等于实际容量:" << points_map_.loop_edge.size());
// }
// }
// if (!_scan_matcher_ok)
// ROS_WARN_STREAM("建图程序,回环失败,两帧匹配不上:ID:" << i_1 << " 与ID:" << _closed_frame_index << " ,匹配健康值:" << scan_matcher_build_->result_.health);
// };
}
if (i_1 > 50 && i_1 % 20 == 0)
{
// 进度从10到30
int _step = 10 + i_1 * 20 / max_frame_size;
// 更新进度
std::stringstream _mesg2("");
//_mesg.clear();
_mesg2 << "优化位姿,预处理中,截止帧ID:" << max_frame_size - 1;
sendBuildMessageToPC(14, "信息", "建图程序", "更新地图过程中", _mesg2, 0, _step);
}
}
/// 有强制闭环的帧的ID输入,表示需要强制闭环
if (A_frame_ID_force_loop_ != B_frame_ID_force_loop_ && A_frame_ID_force_loop_ >= 0 && B_frame_ID_force_loop_ >= 0 && A_frame_ID_force_loop_ < points_map_.frame_size && B_frame_ID_force_loop_ < points_map_.frame_size)
{
int _A_frame_ID = A_frame_ID_force_loop_;
int _B_frame_ID = B_frame_ID_force_loop_;
double _search_range_force_loop = (double)search_range_force_loop_;
A_frame_ID_force_loop_ = 0;
B_frame_ID_force_loop_ = 0;
forceLoop(_A_frame_ID, _B_frame_ID, _search_range_force_loop);
// if(forceLoop(_A_frame_ID, _B_frame_ID))
// {
// //扩大上限
// max_frame_size=std::max(max_frame_size, std::max(_A_frame_ID+1,_B_frame_ID+1));
// //max_loop_edge_size=std::max(max_loop_edge_size, points_map_.loop_edge_size);
// }
}
if (points_map_.loop_edge_size > 0)
{
double graph_residual = 0;
// 图优化
std::stringstream _mesg3;
_mesg3 << "优化位姿,大量计算中,请耐心等待,截止帧ID:" << max_frame_size - 1;
sendBuildMessageToPC(14, "信息", "建图程序", "更新地图过程中", _mesg3, 0, 40);
bool _use_aruco = false;
if (status_aruco_frame_ == 1)
_use_aruco = true;
////帧间匹配
current_Iteration_ceres_ = 0;
int _result = ceres_graph_method(points_map_.fixed_frame_index, graph_results_, points_map_, graph_residual, current_Iteration_ceres_, max_frame_size, points_map_.loop_edge_size, true, _use_aruco,auto_flat_k_,mapping_params_.num_threads);
current_Iteration_ceres_ = -1;
if (_result > 0)
{
std::stringstream _mesg4("");
_mesg4 << "位姿优化完成,截止帧ID:" << max_frame_size - 1;
sendBuildMessageToPC(14, "信息", "建图程序", "更新地图过程中", _mesg4, 0, 60);
}
else
{
std::stringstream _mesg4("");
_mesg4 << "位姿优化失败!失败!失败!无法优化位姿,截止帧ID:" << max_frame_size - 1;
sendBuildMessageToPC(14, "错误", "建图程序", "更新地图过程中", _mesg4, 0, 60);
std::stringstream _mesg5("");
_mesg5 << "位姿优化失败!失败!失败!无法优化位姿,截止帧ID:" << max_frame_size - 1;
sendBuildMessageToPC(0, "重要错误", "建图程序", "更新地图过程中", _mesg5, 30, 0);
}
new_graph_pose_input_update_ = true;
// if (points_map_.fixed_frame_index < max_frame_size - 1)
// {
// ceres_graph_method(points_map_.fixed_frame_index, graph_results_, points_map_, graph_residual, max_frame_size, max_edge_size, use_graph_pose);
// std::stringstream _mesg4("");
// _mesg4 << "建图程序,进度:60%,帧间优化完成";
// sendBuildMessageToPC(4, _mesg4);
// new_graph_pose_input_update_ = true;
// }
// else
// {
// std::stringstream _mesg4("");
// _mesg4 << "建图程序,进度:60%,所有帧固定,不进行帧优化";
// sendBuildMessageToPC(4, _mesg4);
// new_graph_pose_input_update_ = true;
// }
}
else
{
std::stringstream _mesg4("");
_mesg4 << "没有回环,位姿优化失败";
sendBuildMessageToPC(14, "信息", "建图程序", "更新地图过程中", _mesg4, 0, 60);
new_graph_pose_input_update_ = true;
}
// // 线程内处理上一次的结果,现移到外部
// if (new_graph_pose_input_update_)
// {
// // for (int i = 0; i < graph_results_.size() && i < points_map_.frame_size && i < points_map_.frames.size(); ++i)
// // {
// // PoseWithCov _measurement(graph_results_[i]);
// // points_map_.frames[i].pose_graph = _measurement;
// // }
// // change_graph_ = false;
// }
return;
}