ROS中节点之间通信机制是基于发布/订阅(publish/subscribe)模型的消息(message)。所以在读一个ROS节点的时候,先看这个节点发布和订阅的消息,以及对应的回调函数。
1. main函数
int main(int argc, char **argv)
{
//初始化ros节点
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显示的Topic
registerPub(n);
//订阅话题及回调函数
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_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);
ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);//重定位的匹配点
//创建process线程
std::thread measurement_process{process};
ros::spin();
return 0;
}
main函数中的主要步骤有以下几点:
(1)初始化ros节点
(2)读取参数
(3)订阅话题
(4)创建process线程
estimator.setParameter()
为什么要在这里写出这个函数呢,因为这里的两个参数让我头疼了好久。
void Estimator::setParameter()
{
//相机IMU的外参tic和ric
for (int i = 0; i < NUM_OF_CAM; i++)
{
tic[i] = TIC[i];
ric[i] = RIC[i];
}
f_manager.setRic(ric);
ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
td = TD;
}
视觉约束的噪声协方差与标定相机内参时的重投影误差,也就是偏离几个像素有关,对应的就是这里的ProjectionFactor::sqrt_info,这里取1.5个像素,对应到归一化相机平面上的协方差矩阵需要除以焦距f,信息矩阵等于协方差矩阵的逆
Ω
=
∑
v
i
s
−
1
=
(
1.5
f
I
2
×
2
)
−
1
=
f
1.5
I
2
×
2
\Omega = {{\sum}_{vis}}^{-1}= (\frac{1.5}{f}I_{2\times2})^{-1}=\frac{f}{1.5}I_{2\times2}
Ω=∑vis−1=(f1.5I2×2)−1=1.5fI2×2
2. 回调函数
imu_callback
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
if (imu_msg->header.stamp.toSec() <= last_imu_t)
{
ROS_WARN("imu message in disorder!");
return;
}
last_imu_t = imu_msg->header.stamp.toSec();
m_buf.lock();
imu_buf.push(imu_msg);
m_buf.unlock();
//唤醒的是process线程
con.notify_one();
last_imu_t = imu_msg->header.stamp.toSec();
{
std::lock_guard<std::mutex> lg(m_state);
//从IMU测量值imu_msg和上一个PVQ递推得出下一个的PVQ
predict(imu_msg);
std_msgs::Header header = imu_msg->header;
header.frame_id = "world";
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
//发布最新的由predict得到的 P V Q
pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);//"imu_propagate"
}
}
(1)首先对imu的时间戳进行检查,判断是否disorder
(2)把imu数据放到imu_buf中
(3)predict()从IMU测量值imu_msg和上一个PVQ递推得出下一个的PVQ
(4)发布消息 imu_propagate
//从IMU测量值imu_msg和上一个PVQ递推得出下一个的PVQ
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
double t = imu_msg->header.stamp.toSec();
if (init_imu)
{
latest_time = t;
init_imu = 0;
return;
}
double dt = t - latest_time;
latest_time = t;
double dx = imu_msg->linear_acceleration.x;
double dy = imu_msg->linear_acceleration.y;
double dz = imu_msg->linear_acceleration.z;
Eigen::Vector3d linear_acceleration{dx, dy, dz};
double rx = imu_msg->angular_velocity.x;
double ry = imu_msg->angular_velocity.y;
double rz = imu_msg->angular_velocity.z;
Eigen::Vector3d angular_velocity{rx, ry, rz};
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;//tmp_Q local->world
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
//中值积分
Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
tmp_V = tmp_V + dt * un_acc;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
上面这段代码对应的公式:
ω
=
1
2
(
(
ω
b
k
−
b
k
g
)
+
(
ω
b
k
+
1
−
b
k
g
)
)
\omega = \frac{1}{2}((\omega^{b_k}-b_k^g)+(\omega^{b_{k+1}}-b_k^g))
ω=21((ωbk−bkg)+(ωbk+1−bkg))
q
b
i
b
k
+
1
=
q
b
i
b
k
⨂
[
1
1
2
ω
δ
t
]
q_{b_i b_{k+1}}=q_{b_i b_k}\bigotimes\begin{bmatrix} 1 \\ \frac{1}{2}\omega\delta t\end{bmatrix}
qbibk+1=qbibk⨂[121ωδt]
a
=
1
2
(
q
b
i
b
k
(
a
b
k
−
b
k
a
)
+
q
b
i
b
k
+
1
(
a
b
k
+
1
−
b
k
a
)
)
a=\frac{1}{2} (q_{b_i b_k}(a^{b_k}-b_k^a)+q_{b_i b_{k+1}}(a^{b_{k+1}}-b_k^a))
a=21(qbibk(abk−bka)+qbibk+1(abk+1−bka))
剩下的就是速度,加速度和位置的关系,就不写了。
feature_callback
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
if (!init_feature)
{
//skip the first detected feature, which doesn't contain optical flow speed
init_feature = 1;
return;
}
m_buf.lock();
feature_buf.push(feature_msg);
m_buf.unlock();
con.notify_one();
}
整个代码就是将除了第一帧的特征点外push到buf中,relocalization_callback也是执行了相似的过程,在这里就不贴出来了。
restart_callback
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
{
if (restart_msg->data == true)
{
ROS_WARN("restart the estimator!");
m_buf.lock();
while(!feature_buf.empty())
feature_buf.pop();
while(!imu_buf.empty())
imu_buf.pop();
m_buf.unlock();
m_estimator.lock();
estimator.clearState();
estimator.setParameter();
m_estimator.unlock();
current_time = -1;
last_imu_t = 0;
}
return;
}
接收到restart的消息后,将所有的buf全部清空,时间置为-1。
至此,estimator_node中的主函数和回调函数已经分析完了。
接下来就是process线程了,在process线程中,包含了预积分、初始化、外参标定、非线性优化等部分。
3. 画个图吧
在开始下面的分析之前,我把estimator_node这部分代码的流程图画在这里,虽然不标准,但是对于理解程序执行的过程还是有帮助的。