euroc.launch
<launch>
<arg name="config_path" default = "$(find feature_tracker)/../config/euroc/euroc_config.yaml" />
<arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
<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 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>
</launch>
程序首先加载相机参数
%YAML:1.0
#common parameters
imu_topic: "/imu0"
image_topic: "/cam0/image_raw"
output_path: "/config/euroc/vins_result.csv" # vins outputs will be wrttento vins_folder_path + output_path
#camera calibration
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
k1: -2.917e-01
k2: 8.228e-02
p1: 5.333e-05
p2: -1.578e-04
projection_parameters:
fx: 4.616e+02
fy: 4.603e+02
cx: 3.630e+02
cy: 2.481e+02
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
ex_calib_result_path: "/config/euroc/ex_calib_result.yaml" # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [0, -1, 0,
1, 0, 0,
0, 0, 1]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [-0.02,-0.06, 0.01]
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
equalize: 1 # if image is too dark or light, trun on equalize to find enough features
fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05
acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # gravity magnitude
#loop closure parameters
loop_closure: 1 #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly;
#also give the camera calibration file same as feature_tracker node
pattern_file: "/support_files/brief_pattern.yml"
voc_file: "/support_files/brief_k10L6.bin"
min_loop_num: 25
一、feature_tracker节点
int main(int argc, char **argv)
{
ros::init(argc, argv, "feature_tracker");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
readParameters(n);
//读取相机参数，parameters.cpp->CAM_NAMES,camera_model->cameraFactory.cc->CameraFactory::generateCameraFromYamlFile()
for (int i = 0; i < NUM_OF_CAM; i++)
trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);
//if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
//config/euroc/euroc_config.yaml->parameters.cpp(readParameters)->FISHEYE 0/1
if(FISHEYE)
{
for (int i = 0; i < NUM_OF_CAM; i++)
{
//定义鱼眼遮照
//std::string VINS_FOLDER_PATH = readParam<std::string>(n, "vins_folder");
//FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg"
trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
if(!trackerData[i].fisheye_mask.data)
{
ROS_INFO("load mask fail");
ROS_BREAK();
}
else
ROS_INFO("load mask success");
}
}
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
//在名为feature的话题下发布一条类型为PointCloud的消息
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
//在名为feature_img的话题下发布一条类型为Image的消息
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
/*
if (SHOW_TRACK)
cv::namedWindow("vis", cv::WINDOW_NORMAL);
*/
ros::spin();
return 0;
}
1.1重要函数img_callback
void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
//判断是否为第一帧数据，设置第一帧数据时间
if(first_image_flag)
{
first_image_flag = false;// 更新：不再是第一帧图像
first_image_time = img_msg->header.stamp.toSec();//记录第一个图像帧的时间
}
// frequency control
// 保证发布频率控制，保证每秒钟处理的image小于FREQ,频率控制在10HZ以内
// 并不是每读入一帧图像，就要发布特征点
// 判断间隔时间内的发布次数
if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
PUB_THIS_FRAME = true;
// reset the frequency control
// 发布当前帧
// 时间间隔内的发布频率十分接近设定频率时，更新时间间隔起始时刻，并将数据发布次数置0
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;
//将ros数据转换为Opencv图像
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
cv::Mat show_img = ptr->image;
TicToc t_r;
//对图像用光流法进行特征点跟踪
for (int i = 0; i < NUM_OF_CAM; i++)
{
ROS_DEBUG("processing camera %d", i);
if (i != 1 || !STEREO_TRACK)//单目
//FeatureTracker trackerData[NUM_OF_CAM] 光流程序，表示哪个相机的光流，feature_tracker.cpp中
trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)));
else
{
//双目
if (EQUALIZE)// 光太亮或太暗，自适应直方图均衡化处理
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
}
else
trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
}
#if SHOW_UNDISTORTION
trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
#endif
}
//双目
if ( PUB_THIS_FRAME && STEREO_TRACK && trackerData[0].cur_pts.size() > 0)
{
pub_count++;
r_status.clear();
r_err.clear();
TicToc t_o;
cv::calcOpticalFlowPyrLK(trackerData[0].cur_img, trackerData[1].cur_img, trackerData[0].cur_pts, trackerData[1].cur_pts, r_status, r_err, cv::Size(21, 21), 3);
ROS_DEBUG("spatial optical flow costs: %fms", t_o.toc());
vector<cv::Point2f> ll, rr;
vector<int> idx;
for (unsigned int i = 0; i < r_status.size(); i++)
{
if (!inBorder(trackerData[1].cur_pts[i]))
r_status[i] = 0;
if (r_status[i])
{
idx.push_back(i);
//undistortion
Eigen::Vector3d tmp_p;
trackerData[0].m_camera->liftProjective(Eigen::Vector2d(trackerData[0].cur_pts[i].x, trackerData[0].cur_pts[i].y), tmp_p);
tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
ll.push_back(cv::Point2f(tmp_p.x(), tmp_p.y()));
trackerData[1].m_camera->liftProjective(Eigen::Vector2d(trackerData[1].cur_pts[i].x, trackerData[1].cur_pts[i].y), tmp_p);
tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
rr.push_back(cv::Point2f(tmp_p.x(), tmp_p.y()));
}
}
if (ll.size() >= 8)
{
vector<uchar> status;
TicToc t_f;
cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 1.0, 0.5, status);
ROS_DEBUG("find f cost: %f", t_f.toc());
int r_cnt = 0;
for (unsigned int i = 0; i < status.size(); i++)
{
if (status[i] == 0)
r_status[idx[i]] = 0;
r_cnt += r_status[idx[i]];
}
}
}
//更新全局ID
for (unsigned int i = 0;; i++)
{
bool completed = false;
for (int j = 0; j < NUM_OF_CAM; j++)
if (j != 1 || !STEREO_TRACK)
completed |= trackerData[j].updateID(i);
if (!completed)
break;
}
//发布当前帧，包括id和undistorted后的点，和u,v点
//7、将特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，
//封装成sensor_msgs::PointCloudPtr类型的feature_points实例中,发布到pub_img;
if (PUB_THIS_FRAME)
{
pub_count++;
sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
sensor_msgs::ChannelFloat32 id_of_point;
sensor_msgs::ChannelFloat32 u_of_point;
sensor_msgs::ChannelFloat32 v_of_point;
feature_points->header = img_msg->header;
feature_points->header.frame_id = "world";
vector<set<int>> hash_ids(NUM_OF_CAM);// 哈希表id
for (int i = 0; i < NUM_OF_CAM; i++) // 循环相机数量
{
if (i != 1 || !STEREO_TRACK)// 第一帧不发布，因为没有光流速度
{
auto un_pts = trackerData[i].undistortedPoints();//此函数会触发
auto &cur_pts = trackerData[i].cur_pts;
auto &ids = trackerData[i].ids;
for (unsigned int j = 0; j < ids.size(); j++)// 特征点数量
{
int p_id = ids[j];
hash_ids[i].insert(p_id);// 哈希表id insert
geometry_msgs::Point32 p;// 大规模点云信息
p.x = un_pts[j].x;
p.y = un_pts[j].y;
p.z = 1;
feature_points->points.push_back(p);
id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
u_of_point.values.push_back(cur_pts[j].x);
v_of_point.values.push_back(cur_pts[j].y);
ROS_ASSERT(inBorder(cur_pts[j]));
}
}
else if (STEREO_TRACK)
{
//双目
auto r_un_pts = trackerData[1].undistortedPoints();
auto &ids = trackerData[0].ids;
for (unsigned int j = 0; j < ids.size(); j++)
{
if (r_status[j])
{
int p_id = ids[j];
hash_ids[i].insert(p_id);
geometry_msgs::Point32 p;
p.x = r_un_pts[j].x;
p.y = r_un_pts[j].y;
p.z = 1;
feature_points->points.push_back(p);
id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
}
}
}
}
feature_points->channels.push_back(id_of_point);
feature_points->channels.push_back(u_of_point);
feature_points->channels.push_back(v_of_point);
ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
pub_img.publish(feature_points);
if (SHOW_TRACK)
{
ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);
//cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3);
cv::Mat stereo_img = ptr->image;
for (int i = 0; i < NUM_OF_CAM; i++)
{
cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);
cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);
if (i != 1 || !STEREO_TRACK)
{
//显示追踪状态，越红越好，越蓝越不行
for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
{
double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
//char name[10];
//sprintf(name, "%d", trackerData[i].ids[j]);
//cv::putText(tmp_img, name, trackerData[i].cur_pts[j], cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));
}
}
else
{
//双目
for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
{
if (r_status[j])
{
cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(0, 255, 0), 2);
cv::line(stereo_img, trackerData[i - 1].cur_pts[j], trackerData[i].cur_pts[j] + cv::Point2f(0, ROW), cv::Scalar(0, 255, 0));
}
}
}
}
/*
cv::imshow("vis", stereo_img);
cv::waitKey(5);
*/
pub_match.publish(ptr->toImageMsg());
}
}
ROS_INFO("whole feature tracker processing costs: %f", t_r.toc());
}
1.1.1 trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)));
void FeatureTracker::readImage(const cv::Mat &_img)
{
cv::Mat img;
TicToc t_r;
//EQUALIZE表示太亮或太暗，进行直方图均匀化处理
if (EQUALIZE)
{
//使用createCLAHE对图像进行自适应直方图均衡化
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
TicToc t_c;
clahe->apply(_img, img);
ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
}
else
img = _img;
if (forw_img.empty())
{
prev_img = cur_img = forw_img = img;
}
else
{
forw_img = img;
}
forw_pts.clear();
//当前帧为forw_pts,被追踪到的帧；curr_pts实际上是上一帧
if (cur_pts.size() > 0)
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
/*
https://blog.csdn.net/mars_xiaolei/article/details/80526106?spm=1001.2101.3001.6650.2&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-2.pc_relevant_antiscanv2&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-2.pc_relevant_antiscanv2&utm_relevant_index=5
https://blog.csdn.net/AP1005834/article/details/51226660
cur_img前一帧图像，forw_img当前帧图像，cur_pts前一帧特征点，forw_pts当前帧特征点，
status：输出状态向量（无符号字符）;如果找到相应特征的流，则向量的每个元素设置为1，否则设置为0。
err=当前帧与下一帧的所有像素点进行差分平方，在求和
cv::Size(21, 21)搜索窗口大小21=2wx+1（在特征点的窗口大小内寻找运动后的特征点）
3：最大金字塔等级数
*/
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
//清理vector中无法追踪到的特征点
for (int i = 0; i < int(forw_pts.size()); i++)
if (status[i] && !inBorder(forw_pts[i]))
status[i] = 0;
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
}
//feature_tracker_node.cpp->img_callback(),满足帧率就设置为true
if (PUB_THIS_FRAME)
{
// 通过前后两帧的追踪计算F矩阵，通过F矩阵去除Outliers
rejectWithF();
//在光流追踪成功就记被追踪+1，数值代表被追踪的次数，数值越大，说明被追踪的就越久
for (auto &n : track_cnt)
n++;
ROS_DEBUG("set mask begins");
TicToc t_m;
//为下面的goodFeaturesToTrack保证相邻的特征点之间要相隔30个像素,设置mask image
//按照被追踪到的次数排序，然后加上mask去掉部分点，主要是针对鱼眼相机
setMask();
ROS_DEBUG("set mask costs %fms", t_m.toc());
ROS_DEBUG("detect feature begins");
TicToc t_t;
int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());//最大特征点数-当前帧特征点大小
if (n_max_cnt > 0)
{
if(mask.empty())
cout << "mask is empty " << endl;
if (mask.type() != CV_8UC1)
cout << "mask type wrong " << endl;
if (mask.size() != forw_img.size())
cout << "wrong size " << endl;
//上面通过光流法找到一些对应点，这里是为了确保每个帧有足够点，然后调用addPoint添加点
//提取新的特征角点，特征点检测：Frame1，goofFeaturesToTrack()检测MAX_CNT个特征点，添加到forw_pts中,检测MAX_CNT=150个特征点，MIN_DIST=30,最小距离为30
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.1, MIN_DIST, mask);
}
else
n_pts.clear();
ROS_DEBUG("detect feature costs: %fms", t_t.toc());
ROS_DEBUG("add feature begins");
TicToc t_a;
addPoints();//根据cv::goodFeaturesToTrack结果中的n_pts，特征点索引，添加特征点到forw_pts
ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
prev_img = forw_img;//前前帧图像
prev_pts = forw_pts;//前前帧特征点
}
cur_img = forw_img;//将当前帧forw_img放入前一帧图像cur_img
cur_pts = forw_pts;//将当前帧特征点forw_pts放入前一帧特征点中cur_pts
}
1.1.2 其他函数
int FeatureTracker::n_id = 0;
/**
* @breif 判断特征点是非在范围内
*/
bool inBorder(const cv::Point2f &pt)
{
const int BORDER_SIZE = 1;
int img_x = cvRound(pt.x);
int img_y = cvRound(pt.y);
return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE;
}
/**
* @breif 去除无法追踪的特征
*/
void reduceVector(vector<cv::Point2f> &v, vector<uchar> status)
{
int j = 0;
for (int i = 0; i < int(v.size()); i++)
if (status[i])
v[j++] = v[i];
v.resize(j);
}
/**
* @breif 去除无法追踪的特征
*/
void reduceVector(vector<int> &v, vector<uchar> status)
{
int j = 0;
for (int i = 0; i < int(v.size()); i++)
if (status[i])
v[j++] = v[i];
v.resize(j);
}
FeatureTracker::FeatureTracker()
{
}
/**
* @brief 对图像使用光流法进行特征点跟踪
*
* 按照被追踪到的次数排序，然后加上mask去掉部分点，主要是针对鱼眼相机
*/
void FeatureTracker::setMask()
{
if(FISHEYE)
mask = fisheye_mask.clone();
else
mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
// prefer to keep features that are tracked for long time
vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;
for (unsigned int i = 0; i < forw_pts.size(); i++)
cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i])));
sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair<int, pair<cv::Point2f, int>> &a, const pair<int, pair<cv::Point2f, int>> &b)
{
return a.first > b.first;
});
forw_pts.clear();
ids.clear();
track_cnt.clear();
for (auto &it : cnt_pts_id)
{
if (mask.at<uchar>(it.second.first) == 255)
{
forw_pts.push_back(it.second.first);
ids.push_back(it.second.second);
track_cnt.push_back(it.first);
cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
}
}
}
/**
* @brief 添加新的特征点，ID初始化-1,跟踪次数1
*/
void FeatureTracker::addPoints()
{
for (auto &p : n_pts)
{
forw_pts.push_back(p);//当前真的特征添加
ids.push_back(-1);
track_cnt.push_back(1);
}
}
/**
* @breif 通过前后两帧的追踪计算F矩阵，通过F矩阵去除Outliers
*/
void FeatureTracker::rejectWithF()
{
if (forw_pts.size() >= 8)
{
ROS_DEBUG("FM ransac begins");
TicToc t_f;
vector<cv::Point2f> un_prev_pts(prev_pts.size()), un_forw_pts(forw_pts.size());
for (unsigned int i = 0; i < prev_pts.size(); i++)
{
Eigen::Vector3d tmp_p;
m_camera->liftProjective(Eigen::Vector2d(prev_pts[i].x, prev_pts[i].y), tmp_p);
tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
un_prev_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
}
vector<uchar> status;
cv::findFundamentalMat(un_prev_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
int size_a = prev_pts.size();
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
}
}
/**
* @breif 更新特征点ID
*/
bool FeatureTracker::updateID(unsigned int i)
{
if (i < ids.size())
{
if (ids[i] == -1)
ids[i] = n_id++;
return true;
}
else
return false;
}
//读取相机内参
void FeatureTracker::readIntrinsicParameter(const string &calib_file)
{
ROS_INFO("reading paramerter of camera %s", calib_file.c_str());
m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
}
/**
* @breif Visualize undistortedPoints Points
* 显示去畸变矫正后的特征点
*/
void FeatureTracker::showUndistortion(const string &name)
{
cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0));
vector<Eigen::Vector2d> distortedp, undistortedp;
for (int i = 0; i < COL; i++)
for (int j = 0; j < ROW; j++)
{
Eigen::Vector2d a(i, j);
Eigen::Vector3d b;
m_camera->liftProjective(a, b);
distortedp.push_back(a);
undistortedp.push_back(Eigen::Vector2d(b.x() / b.z(), b.y() / b.z()));
//printf("%f,%f->%f,%f,%f\n)\n", a.x(), a.y(), b.x(), b.y(), b.z());
}
for (int i = 0; i < int(undistortedp.size()); i++)
{
cv::Mat pp(3, 1, CV_32FC1);
pp.at<float>(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + COL / 2;
pp.at<float>(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + ROW / 2;
pp.at<float>(2, 0) = 1.0;
//cout << trackerData[0].K << endl;
//printf("%lf %lf\n", p.at<float>(1, 0), p.at<float>(0, 0));
//printf("%lf %lf\n", pp.at<float>(1, 0), pp.at<float>(0, 0));
if (pp.at<float>(1, 0) + 300 >= 0 && pp.at<float>(1, 0) + 300 < ROW + 600 && pp.at<float>(0, 0) + 300 >= 0 && pp.at<float>(0, 0) + 300 < COL + 600)
{
undistortedImg.at<uchar>(pp.at<float>(1, 0) + 300, pp.at<float>(0, 0) + 300) = cur_img.at<uchar>(distortedp[i].y(), distortedp[i].x());
}
else
{
//ROS_ERROR("(%f %f) -> (%f %f)", distortedp[i].y, distortedp[i].x, pp.at<float>(1, 0), pp.at<float>(0, 0));
}
}
cv::imshow(name, undistortedImg);
cv::waitKey(0);
}
/**
* @breif undistortedPoints Points
* 对特征点的图像坐标去畸变矫正，并计算每个角点的速度
*/
vector<cv::Point2f> FeatureTracker::undistortedPoints()
{
vector<cv::Point2f> un_pts;
//cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
Eigen::Vector3d b;
m_camera->liftProjective(a, b);
un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
}
return un_pts;
}
二、vins_estimator
<node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen">
int main(int argc, char **argv)
{
ros::init(argc, argv, "vins_estimator");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
readParameters(n);
estimator.setParameter();
#ifdef EIGEN_DONT_PARALLELIZE
ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
ROS_WARN("waiting for image and imu...");
//RViz相关话题
registerPub(n);
//订阅IMU,feature,image的topic,然后在各自的回调里处理消息
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
ros::Subscriber sub_raw_image = n.subscribe(IMAGE_TOPIC, 2000, raw_image_callback);
std::thread measurement_process{process};
std::thread loop_detection, pose_graph;
if (LOOP_CLOSURE)
{
ROS_WARN("LOOP_CLOSURE true");
loop_detection = std::thread(process_loop_detection);
pose_graph = std::thread(process_pose_graph);
m_camera = CameraFactory::instance()->generateCameraFromYamlFile(CAM_NAMES);
}
ros::spin();
return 0;
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
