roslaunch vins_estimator euroc.launch
roslaunch vins_estimator vins_rviz.launch
rosbag play YOUR_PATH/MH_01_easy.bag
使用roslaunch命令通過launch文件，同時啟動多個節(jié)點;
roslaunch pkg_name name.launch
1、 完成啟動節(jié)點feature_tracker、vins_estimator、pose_graph三個節(jié)點；
2、 將相應(yīng)參數(shù)和參數(shù)文件傳遞給對應(yīng)節(jié)點；
3、 注意arg標(biāo)簽和param標(biāo)簽的使用區(qū)別；
4、 find feature_tracker找到對應(yīng)包對應(yīng)路徑
5、解讀 euroc_config.yaml
<launch> //launch文件標(biāo)簽
//arg參數(shù)標(biāo)簽，僅供launch文件內(nèi)部使用//參數(shù)1和2名字分別為config_path和vins_path//參數(shù)1值是feature_tracker所在文件夾+/../config/euroc/euroc_config.yaml"//參數(shù)2值是feature_tracker所在文件夾+/../config/../ <arg name="config_path" default = "$(find feature_tracker)/../config/euroc/euroc_config.yaml" /> <arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
//node節(jié)點標(biāo)簽，啟動feature_tracker包中名為feature_tracker的節(jié)點//param參數(shù)標(biāo)簽，可以傳給ros參數(shù)服務(wù)器，供該節(jié)點讀取參數(shù)文件；//參數(shù)1名為config_file，其值是上面arg名字為config_path的值；//參數(shù)2名為vins_folder，其值是上面arg名字為vins_path的值； <node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log"> <param name="config_file" type="string" value="$(arg config_path)" /> <param name="vins_folder" type="string" value="$(arg vins_path)" /> </node>
//node節(jié)點標(biāo)簽，啟動vins_estimator包中名為vins_estimator的節(jié)點//param參數(shù)標(biāo)簽，可以傳給ros參數(shù)服務(wù)器，供該節(jié)點讀取參數(shù)文件；//參數(shù)1名為config_file，其值是上面arg名字為config_path的值；//參數(shù)2名為vins_folder，其值是上面arg名字為vins_path的值； <node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen"> <param name="config_file" type="string" value="$(arg config_path)" /> <param name="vins_folder" type="string" value="$(arg vins_path)" /> </node>
//node節(jié)點標(biāo)簽，啟動pose_graph包中名為pose_graph的節(jié)點//param參數(shù)標(biāo)簽，可以傳給ros參數(shù)服務(wù)器，供該節(jié)點讀取參數(shù)文件；//參數(shù)1名為visualization_shift_x，值為0；//參數(shù)2名為visualization_shift_y，值為0；//參數(shù)3名為skip_cnt，值為0；//參數(shù)2名為skip_dis，值為0； <node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen"> <param name="config_file" type="string" value="$(arg config_path)" /> <param name="visualization_shift_x" type="int" value="0" /> <param name="visualization_shift_y" type="int" value="0" /> <param name="skip_cnt" type="int" value="0" /> <param name="skip_dis" type="double" value="0" /> </node>
</launch>
注1：啟動過程中，涉重要的yaml參數(shù)設(shè)置文件euroc_config.yaml
注2：注意yaml文件格式
%YAML:1.0
6、解讀vins_rviz.launch
6.1、啟動包名為rviz中的節(jié)點rvizvisualisation，地圖等展示界面
6.2、args參數(shù)標(biāo)簽，提供主函數(shù)入口argv和argc對應(yīng)參數(shù)；
<launch> <node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find vins_estimator)/../config/vins_rviz_config.rviz" /></launch>
rosbag play YOUR_PATH_TO_DATASET/MH_01_easy.bag
rosbag主要用于記錄、回訪、分析rostopic中的數(shù)據(jù)；
rosbag可以將指定rostopic中的數(shù)據(jù)記錄到.bag后綴的數(shù)據(jù)包中，便于離線分析和處理；
rosbag info filename.bag：可以顯示數(shù)據(jù)包中的信息；
rosbag play bagfile：回訪bag中包含的topic內(nèi)容；
rosbag play bagfile –topic /topic：只播放感興趣的topic；
VINS_MONO共有9個package，前端特征追蹤節(jié)點feature_tracker對應(yīng)于feature_tracker包中的feature_tracker_node.cpp，即該節(jié)點主函數(shù)；
int main(int argc, char **argv){ ros::init(argc, argv, "feature_tracker"); // ros節(jié)點初始化 ros::NodeHandle n("~"); // 聲明一個句柄，～代表這個節(jié)點的命名空間 ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); // 設(shè)置ros log級別 //節(jié)點初始化完成
readParameters(n); // 讀取配置文件，根據(jù)rosluanch中傳入的參數(shù)文件讀取euroc.yaml文件中參數(shù)；
for (int i = 0; i < NUM_OF_CAM; i++) trackerData[i].readIntrinsicParameter(CAM_NAMES[i]); // 獲得每個相機(jī)的內(nèi)參
//IMAGE_TOPIC話題名稱，在euroc.yaml中設(shè)置；//異步通訊，發(fā)布者、話題、訂閱者可參考https://mp.weixin.qq.com/s/mCoDiXMSMCqyQm3DhcO0aw//sub_img向roscore注冊訂閱這個topic:IMAGE_TOPIC，收到一次message就執(zhí)行一次回調(diào)函數(shù)//IMAGE_TOPIC：按照euroc.yaml中設(shè)定其為/cam0/image_raw ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
// 創(chuàng)建三個發(fā)布者// 發(fā)布者1：pub_img，發(fā)送數(shù)據(jù)為sensor_msgs::PointCloud，話題為feature，用于發(fā)送點云(特征點)；// 發(fā)布者2：pub_match,發(fā)送數(shù)據(jù)為sensor_msgs::Image，話題為feature_img，用于發(fā)送圖像// 發(fā)布者3：pub_restart，發(fā)送數(shù)據(jù)std_msgs::Bool，話題為restart，用于發(fā)送重啟信號 pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000); // 實際發(fā)出去的是 /feature_tracker/feature pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000); pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
ros::spin(); // spin代表這個節(jié)點開始循環(huán)查詢topic是否接收 return 0;}
//feature、feature_img、restart話題自動成為feature_tracker節(jié)點中的二級話題
//feature_tracker/feature
/feature_tracker/feature_img
/feature_tracker/restart
注：
節(jié)點/feature_tracker中包含一個訂閱者sub_img，三個發(fā)布者pub_match、pub_img、pub_restart;
1、bagfile經(jīng)話題/cam/image_raw將圖像數(shù)據(jù)傳遞給節(jié)點/feature_tracker;
2、節(jié)點/feature_tracker中的訂閱者sub_img收到圖像，進(jìn)行處理；
3、圖像處理后的數(shù)據(jù)，使用節(jié)點/feature_tracker中的發(fā)布者pub_match、pub_image、pub_restart將數(shù)據(jù)分別經(jīng)話題/feature_tracker/feature_img、/feature_tracker/feature、/feature_tracker/restart發(fā)送給后端節(jié)點/vins_estimator;
1、接收從bagfile經(jīng)話題/cam/image_raw發(fā)送來的圖像數(shù)據(jù)const sensor_msgs::ImageConstPtr &img_msg；
2、 調(diào)用回調(diào)函數(shù)img_callback()處理圖像;
注：ros中圖像格式不同與opencv中圖像格式，需要進(jìn)行轉(zhuǎn)換；http://wiki./cv_bridge/Tutorials
2.1、sensor_msgs::ImageConstPtr數(shù)據(jù)格式
std_msgs/Header header #頭信息uint32 height # image height, number of rowsuint32 width # image width, number of columnsstring encoding # Encoding of pixels -- channel meaning, ordering, size # taken from the list of strings in include/sensor_msgs/image_encodings.huint8 is_bigendian #大端big endian(從低地址到高地址的順序存放數(shù)據(jù)的高位字節(jié)到低位字節(jié))和小端little endianuint32 step # Full row length in bytesuint8[] data # actual matrix data, size is (step * rows)
2.2、std_msgs/Header.msg
一般在Image/PointCloud/IMU等各種傳感器數(shù)據(jù)結(jié)構(gòu)中都會出現(xiàn)的頭信息；
uint32 seq # sequence ID: consecutively increasing ID time stamp #時間戳 string frame_id #坐標(biāo)系ID
2.3、ros中圖像數(shù)據(jù)與opencv中圖像數(shù)據(jù)轉(zhuǎn)換
實現(xiàn)：sensor_msgs::ImageConstPtr—> sensor_msgs::Imageàcv::Mat；
sub_img回調(diào)函數(shù)入?yún)onst sensor_msgs::ImageConstPtr &img_msg
//把ros圖像指針數(shù)據(jù)轉(zhuǎn)換成ros圖像數(shù)據(jù)
// sensor_msgs::ImageConstPtr -》sensor_msgs::Image
cv_bridge::CvImageConstPtr ptr;if (img_msg->encoding == "8UC1"){ sensor_msgs::Image img; img.header = img_msg->header; img.height = img_msg->height; img.width = img_msg->width; img.is_bigendian = img_msg->is_bigendian; img.step = img_msg->step; img.data = img_msg->data; img.encoding = "mono8";
// sensor_msgs::Image—>cv::Mat ptr = cv_bridge::toCvCopy(img,sensor_msgs::image_encodings::MONO8);}else ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);cv::Mat show_img = ptr->image;
類FeatureTracker：管理光流法追蹤的特征點
cv::Mat mask;//掩碼，用于做圖像均一化使用
cv::Mat cur_img, forw_img; //上一幀圖像，當(dāng)前幀圖像
vector<cv::Point2f> n_pts; //最大特征點-當(dāng)前幀已經(jīng)提取特征點：剩余需要提取的特征點
vector<cv::Point2f> cur_pts, forw_pts;//上一幀特征點，當(dāng)前幀特征點
vector<cv::Point2f> prev_un_pts, cur_un_pts;//上一幀特征點去畸變相機(jī)歸一化坐標(biāo)，
vector<cv::Point2f> pts_velocity; //點x和y方向運(yùn)動速度 vector<int> ids;//特征點id
vector<int> track_cnt;//對應(yīng)特征點跟蹤次數(shù)
map<int, cv::Point2f> cur_un_pts_map;//上一幀圖像特征點map,id->坐標(biāo)的map
map<int, cv::Point2f> prev_un_pts_map;camodocal::CameraPtr m_camera;double cur_time;
使用FeatureTracker trackerData對象
3.1根據(jù)該對象讀取圖像追蹤特征點，獲得特征點數(shù)據(jù)
in[1] 圖像；
in[2] 圖像時間戳；
trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
3.2、圖像預(yù)處理，是否進(jìn)行圖像敏感度均衡化還是采用源圖像
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));clahe->apply(_img, img);
3.3、光流法跟蹤，并根據(jù)特征點狀態(tài)status[i]和是否在圖像范圍刪減
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
3.4、根據(jù)特征點狀態(tài)，刪除未被連續(xù)跟蹤的特征點信息
reduceVector(prev_pts, status); // 沒用到reduceVector(cur_pts, status);reduceVector(forw_pts, status);reduceVector(ids, status); // 特征點的idreduceVector(cur_un_pts, status); // 去畸變后的坐標(biāo)reduceVector(track_cnt, status); // 追蹤次數(shù)
注：vector<int> track_cnt;//存儲的是最新一直被連續(xù)跟蹤的特征次數(shù)
在光流法跟蹤以后，縮減掉未能連續(xù)跟蹤的特征，剩余的是最新再一次跟蹤的特征，所以每個值加1；
3.5、根據(jù)圖像接收頻率，決定當(dāng)前處理圖像特征結(jié)果是否發(fā)送到后端
// frequency control// 控制一下發(fā)給后端的頻率// PUB_THIS_FRAME 發(fā)送標(biāo)志位if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ) // 保證發(fā)給后端的不超過這個頻率{ PUB_THIS_FRAME = true; // reset the frequency control // 這段時間的頻率和預(yù)設(shè)頻率十分接近，就認(rèn)為這段時間很棒，重啟一下，避免delta t太大 if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ) { first_image_time = img_msg->header.stamp.toSec(); pub_count = 0; }}else PUB_THIS_FRAME = false;
若向后端發(fā)送特征數(shù)據(jù)，則需利用對極約束去除outliers，若不夠max_cnt數(shù)量，需要增加特征點，以便后面發(fā)布數(shù)據(jù)；
3.6、利用虛擬相機(jī)將兩個圖像特征點轉(zhuǎn)化到相機(jī)坐標(biāo)系
使用opencv接口，剔除outliers，status保留標(biāo)志位，刪除原始特征點中outliers；
cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
3.7、給現(xiàn)有的特征點設(shè)置mask，實現(xiàn)特征點均勻化
vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;// 特征點構(gòu)成的向量，成員是map<特征點被跟蹤次數(shù)， <特征點坐標(biāo)， 特征點id>>// 利用光流特點，追蹤多的穩(wěn)定性好，排前面,對cnt_pts_id排序； mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));//與圖像大小一致； for (auto &it : cnt_pts_id) { if (mask.at<uchar>(it.second.first) == 255) { // 把挑選剩下的特征點重新放進(jìn)容器 forw_pts.push_back(it.second.first); ids.push_back(it.second.second); track_cnt.push_back(it.first); // opencv函數(shù)，把周圍一個圓內(nèi)全部置0,這個區(qū)域不允許別的特征點存在，避免特征點過于集中 cv::circle(mask, it.second.first, MIN_DIST, 0, -1); }}
從被跟蹤次數(shù)最多的特征點開始遍歷，最先遍歷的特征點mask中對應(yīng)像素為255，所以加入均一化點集中，并以該點為圓心的院內(nèi)所有像素值都變成0，因此這些點無法在計入到均一化點集中；
3.8、當(dāng)前幀提取特征數(shù)量要求max_cnt,已經(jīng)提取特征forw_pts.size()，因此還需要在提取一部分
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
//將n_pts加入到forw_pts中，對應(yīng)id為-1，追蹤次數(shù)為1；
當(dāng)前幀所有點統(tǒng)一去畸變，同時計算特征點速度，用來后續(xù)時間戳標(biāo)定
void FeatureTracker::undistortedPoints()// PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) // https://blog.csdn.net/u010848251/article/details/118212562
