项目中常见shared_ptr, make_shared(), iteration的使用

博客提及了shared_ptr和make_shared的使用,还涉及vector的查找操作以及std::copy()函数,这些都是信息技术领域中C++编程的相关内容。
  • shared_ptr make_shared
boost::shared_ptr<ScopedNvmemCommitter::DeviceParameters> deviceParameters
            = boost::make_shared<ScopedNvmemCommitter::DeviceParameters>()
  • vector find
   std::vector<int> v{ 1,2,3 };
   std::vector<int>::iterator itr;
   itr = find(v.begin(), v.end(), 1);
   std::cout << *itr << std::endl;
  • std::copy()
   std::string serializedScaling = nNISLSCCAPS::CapsUtils_scalingToJson(m_scaling);
   std::vector<uint8_t> data(serializedScaling.size() + 1, '\0');
   std::copy(serializedScaling.begin(), serializedScaling.end(), data.begin());
#ifndef BRRT_H #define BRRT_H #include "occ_grid/occ_map.h" #include "visualization/visualization.hpp" #include "sampler.h" #include "node.h" #include "kdtree.h" #include <ros/ros.h> #include <utility> #include <queue> #include <algorithm> namespace path_plan { class BRRT { public: BRRT(){}; BRRT(const ros::NodeHandle &nh, const env::OccMap::Ptr &mapPtr) : nh_(nh), map_ptr_(mapPtr) { nh_.param("BRRT/steer_length", steer_length_, 0.0); nh_.param("BRRT/search_time", search_time_, 0.0); nh_.param("BRRT/max_tree_node_nums", max_tree_node_nums_, 0); ROS_WARN_STREAM("[BRRT] param: steer_length: " << steer_length_); ROS_WARN_STREAM("[BRRT] param: search_time: " << search_time_); ROS_WARN_STREAM("[BRRT] param: max_tree_node_nums: " << max_tree_node_nums_); sampler_.setSamplingRange(mapPtr->getOrigin(), mapPtr->getMapSize()); valid_tree_node_nums_ = 0; nodes_pool_.resize(max_tree_node_nums_); for (int i = 0; i < max_tree_node_nums_; ++i) { nodes_pool_[i] = new TreeNode; } } ~BRRT(){}; bool plan(const Eigen::Vector3d &s, const Eigen::Vector3d &g) { reset(); if (!map_ptr_->isStateValid(s)) { ROS_ERROR("[BRRT]: Start pos collide or out of bound"); return false; } if (!map_ptr_->isStateValid(g)) { ROS_ERROR("[BRRT]: Goal pos collide or out of bound"); return false; } /* construct start and goal nodes */ start_node_ = nodes_pool_[1]; start_node_->x = s; start_node_->cost_from_start = 0.0; goal_node_ = nodes_pool_[0]; goal_node_->x = g; goal_node_->cost_from_start = 0.0; // important valid_tree_node_nums_ = 2; // put start and goal in tree vis_ptr_->visualize_a_ball(s, 0.3, "start", visualization::Color::pink); vis_ptr_->visualize_a_ball(g, 0.3, "goal", visualization::Color::steelblue); ROS_INFO("[BRRT]: BRRT starts planning a path"); return brrt(s, g); } vector<Eigen::Vector3d> getPath() { return final_path_; } vector<vector<Eigen::Vector3d>> getAllPaths() { return path_list_; } vector<std::pair<double, double>> getSolutions() { return solution_cost_time_pair_list_; } void setVisualizer(const std::shared_ptr<visualization::Visualization> &visPtr) { vis_ptr_ = visPtr; }; private: // nodehandle params ros::NodeHandle nh_; BiasSampler sampler_; double steer_length_; double search_time_; int max_tree_node_nums_; int valid_tree_node_nums_; double first_path_use_time_; double final_path_use_time_; double cost_best_; std::vector<TreeNode *> nodes_pool_; TreeNode *start_node_; TreeNode *goal_node_; vector<Eigen::Vector3d> final_path_; vector<vector<Eigen::Vector3d>> path_list_; vector<std::pair<double, double>> solution_cost_time_pair_list_; // environment env::OccMap::Ptr map_ptr_; std::shared_ptr<visualization::Visualization> vis_ptr_; void reset() { final_path_.clear(); path_list_.clear(); cost_best_ = DBL_MAX; solution_cost_time_pair_list_.clear(); for (int i = 0; i < valid_tree_node_nums_; i++) { nodes_pool_[i]->parent = nullptr; nodes_pool_[i]->children.clear(); } valid_tree_node_nums_ = 0; } double calDist(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2) { return (p1 - p2).norm(); } RRTNode3DPtr addTreeNode(RRTNode3DPtr &parent, const Eigen::Vector3d &state, const double &cost_from_start, const double &cost_from_parent) { RRTNode3DPtr new_node_ptr = nodes_pool_[valid_tree_node_nums_]; valid_tree_node_nums_++; new_node_ptr->parent = parent; parent->children.push_back(new_node_ptr); new_node_ptr->x = state; new_node_ptr->cost_from_start = cost_from_start; new_node_ptr->cost_from_parent = cost_from_parent; return new_node_ptr; } void changeNodeParent(RRTNode3DPtr &node, RRTNode3DPtr &parent, const double &cost_from_parent) { if (node->parent) node->parent->children.remove(node); //DON'T FORGET THIS, remove it form its parent's children list node->parent = parent; node->cost_from_parent = cost_from_parent; node->cost_from_start = parent->cost_from_start + cost_from_parent; parent->children.push_back(node); // for all its descedants, change the cost_from_start and tau_from_start; RRTNode3DPtr descendant(node); std::queue<RRTNode3DPtr> Q; Q.push(descendant); while (!Q.empty()) { descendant = Q.front(); Q.pop(); for (const auto &leafptr : descendant->children) { leafptr->cost_from_start = leafptr->cost_from_parent + descendant->cost_from_start; Q.push(leafptr); } } } void fillPath(const RRTNode3DPtr &node_A, const RRTNode3DPtr &node_B, vector<Eigen::Vector3d> &path) { path.clear(); RRTNode3DPtr node_ptr = node_A; while (node_ptr->parent) { path.push_back(node_ptr->x); node_ptr = node_ptr->parent; } path.push_back(start_node_->x); std::reverse(std::begin(path), std::end(path)); node_ptr = node_B; while (node_ptr->parent) { path.push_back(node_ptr->x); node_ptr = node_ptr->parent; } path.push_back(goal_node_->x); } Eigen::Vector3d steer(const Eigen::Vector3d &nearest_node_p, const Eigen::Vector3d &rand_node_p, double len) { Eigen::Vector3d diff_vec = rand_node_p - nearest_node_p; double dist = diff_vec.norm(); if (diff_vec.norm() <= len) return rand_node_p; else return nearest_node_p + diff_vec * len / dist; } bool greedySteer(const Eigen::Vector3d &x_near, const Eigen::Vector3d &x_target, vector<Eigen::Vector3d> &x_connects, const double len) { double vec_length = (x_target - x_near).norm(); Eigen::Vector3d vec_unit = (x_target - x_near) / vec_length; x_connects.clear(); if(vec_length < len) return map_ptr_->isSegmentValid(x_near,x_target); Eigen::Vector3d x_new, x_pre = x_near; double steered_dist = 0; while(steered_dist + len < vec_length) { x_new = x_pre + len * vec_unit; if( (!map_ptr_->isStateValid(x_new)) || (!map_ptr_->isSegmentValid(x_new,x_pre)) ) return false; x_pre = x_new; x_connects.push_back(x_new); steered_dist += len; } return map_ptr_->isSegmentValid(x_target,x_pre); } bool brrt(const Eigen::Vector3d &s, const Eigen::Vector3d &g) { ros::Time rrt_start_time = ros::Time::now(); bool tree_connected = false; bool path_reverse = false; /* kd tree init */ kdtree *kdtree_1 = kd_create(3); kdtree *kdtree_2 = kd_create(3); //Add start and goal nodes to kd trees kd_insert3(kdtree_1, start_node_->x[0], start_node_->x[1], start_node_->x[2], start_node_); kd_insert3(kdtree_2, goal_node_->x[0], goal_node_->x[1], goal_node_->x[2], goal_node_); kdtree *treeA = kdtree_1; kdtree *treeB = kdtree_2; /* main loop */ int idx = 0; for (idx = 0; (ros::Time::now() - rrt_start_time).toSec() < search_time_ && valid_tree_node_nums_ < max_tree_node_nums_; ++idx) { /* random sampling */ Eigen::Vector3d x_rand; sampler_.samplingOnce(x_rand); // samplingOnce(x_rand); if (!map_ptr_->isStateValid(x_rand)) { continue; } /* request nearest node in treeA */ struct kdres *p_nearestA = kd_nearest3(treeA, x_rand[0], x_rand[1], x_rand[2]); if (p_nearestA == nullptr) { ROS_ERROR("nearest query error"); continue; } RRTNode3DPtr nearest_nodeA = (RRTNode3DPtr)kd_res_item_data(p_nearestA); kd_res_free(p_nearestA); /* Extend treeA */ Eigen::Vector3d x_new = steer(nearest_nodeA->x, x_rand, steer_length_); if ( (!map_ptr_->isStateValid(x_new)) || (!map_ptr_->isSegmentValid(nearest_nodeA->x, x_new)) ) { /* Steer Trapped */ std::swap(treeA, treeB); path_reverse = !path_reverse; continue; } /* Add x_new to treeA */ double dist_from_A = nearest_nodeA->cost_from_start + steer_length_; RRTNode3DPtr new_nodeA(nullptr); new_nodeA = addTreeNode(nearest_nodeA, x_new, dist_from_A, steer_length_); kd_insert3(treeA, x_new[0], x_new[1], x_new[2], new_nodeA); /* request x_new's nearest node in treeB */ struct kdres *p_nearestB = kd_nearest3(treeB, x_new[0], x_new[1], x_new[2]); if (p_nearestB == nullptr) { ROS_ERROR("nearest query error"); continue; } RRTNode3DPtr nearest_nodeB = (RRTNode3DPtr)kd_res_item_data(p_nearestB); kd_res_free(p_nearestB); /* Greedy steer & check connection */ vector<Eigen::Vector3d> x_connects; bool isConnected = greedySteer(nearest_nodeB->x, x_new, x_connects, steer_length_); /* Add the steered nodes to treeB */ RRTNode3DPtr new_nodeB = nearest_nodeB; if(!x_connects.empty()){ if( valid_tree_node_nums_ + (int)x_connects.size() >= max_tree_node_nums_ ){ valid_tree_node_nums_ = max_tree_node_nums_; //max_node_num reached break;} for(auto x_connect: x_connects){ new_nodeB = addTreeNode(new_nodeB, x_connect, new_nodeB->cost_from_start + steer_length_, steer_length_); kd_insert3(treeB, x_connect[0], x_connect[1], x_connect[2], new_nodeB); } } /* If connected, trace the connected path */ if(isConnected){ tree_connected = true; double path_cost = new_nodeA->cost_from_start + new_nodeB->cost_from_start + calDist(new_nodeB->x, new_nodeA->x); if(path_cost < cost_best_) { vector<Eigen::Vector3d> curr_best_path; if(path_reverse) fillPath(new_nodeB, new_nodeA, curr_best_path); else fillPath(new_nodeA, new_nodeB, curr_best_path); path_list_.emplace_back(curr_best_path); solution_cost_time_pair_list_.emplace_back(path_cost, (ros::Time::now() - rrt_start_time).toSec()); cost_best_ = path_cost; } } /* Swap treeA&B */ std::swap(treeA, treeB); path_reverse = !path_reverse; }//End of sampling iteration if (tree_connected) { final_path_use_time_ = (ros::Time::now() - rrt_start_time).toSec(); ROS_INFO_STREAM("[BRRT]: find_path_use_time: " << solution_cost_time_pair_list_.front().second << ", length: " << solution_cost_time_pair_list_.front().first); visualizeWholeTree(); final_path_ = path_list_.back(); } else if (valid_tree_node_nums_ == max_tree_node_nums_) { visualizeWholeTree(); ROS_ERROR_STREAM("[BRRT]: NOT CONNECTED TO GOAL after " << max_tree_node_nums_ << " nodes added to rrt-tree"); } else { ROS_ERROR_STREAM("[BRRT]: NOT CONNECTED TO GOAL after " << (ros::Time::now() - rrt_start_time).toSec() << " seconds"); } return tree_connected; } void visualizeWholeTree(){ // Sample and visualize the resultant tree vector<Eigen::Vector3d> vertice; vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> edges; vertice.clear(); edges.clear(); sampleWholeTree(start_node_, vertice, edges); sampleWholeTree(goal_node_, vertice, edges); std::vector<visualization::BALL> tree_nodes; tree_nodes.reserve(vertice.size()); visualization::BALL node_p; node_p.radius = 0.12; for (size_t i = 0; i < vertice.size(); ++i) { node_p.center = vertice[i]; tree_nodes.push_back(node_p); } vis_ptr_->visualize_balls(tree_nodes, "tree_vertice", visualization::Color::blue, 1.0); vis_ptr_->visualize_pairline(edges, "tree_edges", visualization::Color::red, 0.06); } void sampleWholeTree(const RRTNode3DPtr &root, vector<Eigen::Vector3d> &vertice, vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> &edges) { if (root == nullptr) return; // whatever dfs or bfs RRTNode3DPtr node = root; std::queue<RRTNode3DPtr> Q; Q.push(node); while (!Q.empty()) { node = Q.front(); Q.pop(); for (const auto &leafptr : node->children) { vertice.push_back(leafptr->x); edges.emplace_back(std::make_pair(node->x, leafptr->x)); Q.push(leafptr); } } } public: void samplingOnce(Eigen::Vector3d &sample) { static int i = 0; sample = preserved_samples_[i]; i++; i = i % preserved_samples_.size(); } void setPreserveSamples(const vector<Eigen::Vector3d> &samples) { preserved_samples_ = samples; } vector<Eigen::Vector3d> preserved_samples_; }; } // namespace path_plan #endif 注释上述代码
最新发布
09-19
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; }
08-29
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值