vector的size(),capacity(),max_size()的用法和区别

本文介绍了C++标准库中Vector容器的几个关键属性:size()用于返回容器的大小;empty()用于判断容器是否为空;max_size()返回容器最大的可存储元素数量;capacity()则返回容器当前能够容纳的元素数量。通过实例演示了这些属性的应用。

        在Vector容器中有以下几个关于大小的函数
方法效果
size()返回容器的大小
empty()判断容器是否为空
max_size()返回容器最大的可以存储的元素
capacity()返回容器当前能够容纳的元素数量



#include <vector>
#include <string>
#include <iostream>
#include <algorithm>
#include <iterator>
using namespace std;
int main()  
{  
	vector<string> sentence;  
	sentence.reserve(5);  

	//append some elements  
	sentence.push_back("hello");  
	sentence.push_back("how");  
	sentence.push_back("are");  
	sentence.push_back("you");  
	sentence.push_back("?");  
	copy(sentence.begin(), sentence.end(), ostream_iterator<string>(cout, " "));  
	cout << endl;  

	//print "technical data"  
	cout << "max_size():" << sentence.max_size() << endl;  
	cout << "size():" << sentence.size() << endl;  
	cout << "capacity():" << sentence.capacity() << endl;  

	cout << "**************************" << endl;  

	//swap second and fourth element  
	swap(sentence[1], sentence[3]);   
	//insert element "always" before element "?"  
	sentence.insert(find(sentence.begin(), sentence.end(), "?"), "always");  
	//assign "!" to the last element  
	sentence.back() = "!";  
	copy(sentence.begin(), sentence.end(), ostream_iterator<string>(cout, " "));  
	cout << endl;  

	//print "technical data"  
	// return maximum possible length of sequence   
	//回容器的最大可以存储的元素个数,这是个极限,当容器扩展到这个最大值时就不能再自动增大  
	cout << "max_size():" << sentence.max_size() << endl;  
	// return length of sequence  
	cout << "size():" << sentence.size() << endl;  
	// return current length of allocated storage  
	cout << "capacity():" << sentence.capacity() << endl;  
	system("Pause");
	return 0;  
}  


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、付费专栏及课程。

余额充值