2021SC@SDUSC

visual_odometry_callback

​ 在上一章，我们大致把visual_odometry的main函数过了一遍，把节点在生成回调函数之前的准备工作介绍了一遍。我们已经大致了解了节点的输入和输出。其实，通过上一章的分析，可以发现，这个节点和我之前分析的visual_feature的关联还是比较紧密，需要用到visual_feature发布的视觉特征点。

​ 下面，我们开始分析这个节点最重要的回调函数部分，以及主线程process。一共有4个，分别是：imu_callback，feature_callback，odom_callback，restart_callback。

​ 其中，imu_callback，主线程process是主要部分。

1. *imu_callback

​ IMU回调函数，将imu_msg保存到imu_buf，IMU状态递推并发布[P,Q,V,header, failureCount].

a. 数据校验

​ 这里判断每个IMU数据帧到达的时间顺序是否正确，如果不正确则会提示，并且不处理。如果正确，继续往下执行，并保存这一帧的时间，给下一帧的时间做判断。

// 判断这一帧的时间是否小于等于上一帧的时间，如果结果为true，则说明乱序，时间顺序错误
if (imu_msg->header.stamp.toSec() <= last_imu_t)
{ROS_WARN("imu message in disorder!");return;
}
last_imu_t = imu_msg->header.stamp.toSec();

b. 保存数据，并通知主线程

​ 通过加锁，防止多线程访问IMU数据缓存队列出现问题。并在取出数据之后，通知主线程process，从阻塞状态唤醒。唤醒的具体功能，见下面注释。

m_buf.lock();
imu_buf.push(imu_msg);
m_buf.unlock();
con.notify_one(); // 唤醒作用于process线程中的获取观测值数据的函数last_imu_t = imu_msg->header.stamp.toSec(); // 不知道为啥又加了一次赋值操作

c. IMU状态递推，并发布

​ 首先，所有代码都被一个大括号包围住，表示这是一个作用域。那为什么要这么做呢？其实是因为里面的第一行的互斥锁lock_guard。当第一行代码执行的时候，相当于加了锁，当离开这个作用域的时候，临时变量会释放，而lock_guard析构的时候就会解锁。这相当于一个编程的小技巧。

​ 接着执行predict函数，这个函数的作用是从IMU测量值imu_msg和上一个PVQ递推得到下一个tmp_Q，tmp_P，tmp_V，使用中值积分。

​ 最后，发布最新的由IMU直接递推得到的PQV。

{// 构造互斥锁m_state，析构时解锁std::lock_guard<std::mutex> lg(m_state);predict(imu_msg); // 递推得到IMU的PQVstd_msgs::Header header = imu_msg->header;// 发布最新的由IMU直接递推得到的PQVif (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header, estimator.failureCount);
}

predict函数

​ 从IMU测量值imu_msg和上一个PQV递推得到下一个tmp_Q，tmp_P，tmp_V。下面的代码就是使用积分中值定理得到tmp_Q，tmp_P，tmp_V. 不过这里我也不是特别了解。

void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{double t = imu_msg->header.stamp.toSec();// 如果是第一个IMU的数据，啧init_imu为1，不处理，直接返回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;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;
}

d. 完整代码

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();con.notify_one();last_imu_t = imu_msg->header.stamp.toSec();{std::lock_guard<std::mutex> lg(m_state);predict(imu_msg);std_msgs::Header header = imu_msg->header;if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header, estimator.failureCount);}
}

2. odom_callback

​ 这个函数就是简单的把重定位后的里程计信息放入缓存队列中。

void odom_callback(const nav_msgs::Odometry::ConstPtr& odom_msg)
{m_odom.lock();odomQueue.push_back(*odom_msg);m_odom.unlock();
}

3. feature_callback

​ 这个函数也较为简单，先验证是否是第一帧，如果是的话，不处理。因为我前面的blog有分析到，我们对于第一帧图像是认为没有光流速度的，所以，没有办法追踪并找到特征点。后面的话，也是把消息放到缓冲区内，然后，也需要和imu_callback一样，通知主线程。

void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{if (!init_feature){//skip the first detected feature, which doesn't contain optical flow speedinit_feature = 1;return;}m_buf.lock();feature_buf.push(feature_msg);m_buf.unlock();con.notify_one();
}

4. restart_callback

​ 这里的话，是会从visual_feature节点可能会接受到重启的消息，我之前的blog也有分析到，如果相机的数据流不稳定，是需要发布restart消息。这里可以看我的第11篇blog。然后再重启之前，我们需要清空缓冲区，并且重置所有数据和状态。

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;
}

5. *主线程process

​ 这个线程是整个节点最重要的部分。

​ process里面只有一个while循环，运行条件为ros::ok()。因此，如果当前节点正在运行，ros::ok()为true，为无限循环。

​ 接下来是while循环里面的功能。首先是等待并获取measurements：(IMUs, img_msg)s，计算dt。接着使用，estimator.processIMU()进行IMU预积分。然后，通过estimator.setReloFrame()设置重定位帧。最后，estimator.processImage()处理图像帧：初始化，紧耦合的非线性优化。

a. 接受数据，提取measurements数组

​ 一开始，先创建临时变量，用来保存提取到的measurements。然后，对m_buf创建唯一锁，防止访问共享内存时出现问题。然后进入陷入等待。如果唤醒了，会给m_buf加上锁，然后执行getMeasurements函数，获得相关的值，然后判断取出来的measurement的size值是不是不等于0，如果不等于0，则继续往下执行。如果等于0，则继续挂起。

std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
std::unique_lock<std::mutex> lk(m_buf);
con.wait(lk, [&]{return (measurements = getMeasurements()).size() != 0;});
lk.unlock();

getMeasurements()函数

​ 这里的返回值是数据对数组。其中数组对中的第一个数据为IMU数组，第二个数据为img_msg。这里将缓冲区imu_buf和feature_buf的数据都取出来。除此之外，还让IMU数据和图像进行对齐并组合。其中对齐的方式是要让所有数据帧的时间一定要顺序。具体的对齐方法，见下面的代码注释。

std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> getMeasurements()
{// 返回结果std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;while (ros::ok()){if (imu_buf.empty() || feature_buf.empty())return measurements;// 对齐标准：IMU最后一个数据的时间要大于第一个图像特征数据的时间if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td)){return measurements;}// 对齐标准：IMU第一个数据的时间要小于第一个图像特征数据的时间if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td)){ROS_WARN("throw img, only should happen at the beginning");feature_buf.pop();continue;}sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();feature_buf.pop();std::vector<sensor_msgs::ImuConstPtr> IMUs;// 图像数据(img_msg)，对应多组在时间戳内的imu数据,然后塞入measurementswhile (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td){IMUs.emplace_back(imu_buf.front());imu_buf.pop();}// 这里把下一个imu_msg也放进去了,但没有pop，因此当前图像帧和下一图像帧会共用这个imu_msgIMUs.emplace_back(imu_buf.front());if (IMUs.empty())ROS_WARN("no imu between two image");measurements.emplace_back(IMUs, img_msg);}return measurements;
}

b. IMU预积分

​ 下面开始正式的处理。首先会加上m_estimator.lock()，这里是为了防止正在执行主线程的时候，突然visual_feature节点突然来了一个重启信号，导致同时访问接下来操作要访问到的共享缓冲区以及共享变量。然后，接着为一个for-each操作，把measurements数组的每一个元素取出来，然后循环执行for里面的内容。这一部分的代码就不单独显示，会放在后面的完整代码里面。

​ 接着就是对每一个measurement元素进行操作。首先IMU的预积分。首先就是把数据取出来，然后接着就预积分，详细见下面代码注释。

auto img_msg = measurement.second;// IMU的预积分
double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
for (auto &imu_msg : measurement.first)
{double t = imu_msg->header.stamp.toSec();double img_t = img_msg->header.stamp.toSec() + estimator.td;if (t <= img_t){ if (current_time < 0)current_time = t;double dt = t - current_time;ROS_ASSERT(dt >= 0);current_time = t;dx = imu_msg->linear_acceleration.x;dy = imu_msg->linear_acceleration.y;dz = imu_msg->linear_acceleration.z;rx = imu_msg->angular_velocity.x;ry = imu_msg->angular_velocity.y;rz = imu_msg->angular_velocity.z;// 这里就是执行IMU预积分的地方estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));//printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);}else{double dt_1 = img_t - current_time;double dt_2 = t - img_t;current_time = img_t;ROS_ASSERT(dt_1 >= 0);ROS_ASSERT(dt_2 >= 0);ROS_ASSERT(dt_1 + dt_2 > 0);double w1 = dt_2 / (dt_1 + dt_2);double w2 = dt_1 / (dt_1 + dt_2);dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;rx = w1 * rx + w2 * imu_msg->angular_velocity.x;ry = w1 * ry + w2 * imu_msg->angular_velocity.y;rz = w1 * rz + w2 * imu_msg->angular_velocity.z;// 这里就是执行IMU预积分的地方estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));//printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);}
}

processIMU()函数

​ 这里主要处理IMU数据。让IMU预积分，中值积分得到当前PQV作为优化初值。

oid Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{if (!first_imu){first_imu = true;acc_0 = linear_acceleration;gyr_0 = angular_velocity;}if (!pre_integrations[frame_count]){pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};}if (frame_count != 0){pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);dt_buf[frame_count].push_back(dt);linear_acceleration_buf[frame_count].push_back(linear_acceleration);angular_velocity_buf[frame_count].push_back(angular_velocity);// 采用的是中值积分的传播方式int j = frame_count;         Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;Vs[j] += dt * un_acc;}acc_0 = linear_acceleration;gyr_0 = angular_velocity;
}

c. VINS优化

​ 这里建立每个特征点的(camera_id,[x,y,z,u,v,vx,vy])s的map，索引为feature_id。

map<int, vector<pair<int, Eigen::Matrix<double, 8, 1>>>> image;
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{int v = img_msg->channels[0].values[i] + 0.5;int feature_id = v / NUM_OF_CAM;int camera_id = v % NUM_OF_CAM;double x = img_msg->points[i].x;double y = img_msg->points[i].y;double z = img_msg->points[i].z;double p_u = img_msg->channels[1].values[i];double p_v = img_msg->channels[2].values[i];double velocity_x = img_msg->channels[3].values[i];double velocity_y = img_msg->channels[4].values[i];double depth = img_msg->channels[5].values[i];ROS_ASSERT(z == 1);Eigen::Matrix<double, 8, 1> xyz_uv_velocity_depth;xyz_uv_velocity_depth << x, y, z, p_u, p_v, velocity_x, velocity_y, depth;image[feature_id].emplace_back(camera_id,  xyz_uv_velocity_depth);
}

d. 从激光雷达里程计获取初始化信息

vector<float> initialization_info;
m_odom.lock();
initialization_info = odomRegister->getOdometry(odomQueue, img_msg->header.stamp.toSec() + estimator.td);
m_odom.unlock();

getOdometry()函数

​ 这里属于初始化的内容，可以看我另外一个同学的分析。我这里只是简单贴一下代码。简单的来说，这个函数的作用是将里程计信息，从激光雷达帧转到VINS的图像帧中。下面是我简单的代码分析。

vector<float> getOdometry(deque<nav_msgs::Odometry>& odomQueue, double img_time)
{vector<float> odometry_channel;odometry_channel.resize(18, -1); // 重新设置 id(1), P(3), Q(4), V(3), Ba(3), Bg(3), gravity(1)nav_msgs::Odometry odomCur;// 把旧的里程计信息扔掉，类似我前面的blog中的分析。while (!odomQueue.empty()) {if (odomQueue.front().header.stamp.toSec() < img_time - 0.05)odomQueue.pop_front();elsebreak;}if (odomQueue.empty()){return odometry_channel;}// 找到最接近图像时间的里程计时间for (int i = 0; i < (int)odomQueue.size(); ++i){odomCur = odomQueue[i];if (odomCur.header.stamp.toSec() < img_time - 0.002) // 500Hz imucontinue;elsebreak;}// 时间戳差异仍然太大，进行处理if (abs(odomCur.header.stamp.toSec() - img_time) > 0.05){return odometry_channel;}// 将里程计旋转从激光雷达 ROS 框架转换为 VINS 相机框架（仅旋转，假设激光雷达、相机和 IMU 足够接近）tf::Quaternion q_odom_lidar;tf::quaternionMsgToTF(odomCur.pose.pose.orientation, q_odom_lidar);tf::Quaternion q_odom_cam = tf::createQuaternionFromRPY(0, 0, M_PI) * (q_odom_lidar * q_lidar_to_cam); // 全局旋转 pi // 注意：相机 - 激光雷达tf::quaternionTFToMsg(q_odom_cam, odomCur.pose.pose.orientation);// 将里程计位置从激光雷达 ROS 框架转换为 VINS 相机框架Eigen::Vector3d p_eigen(odomCur.pose.pose.position.x, odomCur.pose.pose.position.y, odomCur.pose.pose.position.z);Eigen::Vector3d v_eigen(odomCur.twist.twist.linear.x, odomCur.twist.twist.linear.y, odomCur.twist.twist.linear.z);Eigen::Vector3d p_eigen_new = q_lidar_to_cam_eigen * p_eigen;Eigen::Vector3d v_eigen_new = q_lidar_to_cam_eigen * v_eigen;odomCur.pose.pose.position.x = p_eigen_new.x();odomCur.pose.pose.position.y = p_eigen_new.y();odomCur.pose.pose.position.z = p_eigen_new.z();odomCur.twist.twist.linear.x = v_eigen_new.x();odomCur.twist.twist.linear.y = v_eigen_new.y();odomCur.twist.twist.linear.z = v_eigen_new.z();// odomCur.header.stamp = ros::Time().fromSec(img_time);// odomCur.header.frame_id = "vins_world";// odomCur.child_frame_id = "vins_body";// pub_latest_odometry.publish(odomCur);odometry_channel[0] = odomCur.pose.covariance[0];odometry_channel[1] = odomCur.pose.pose.position.x;odometry_channel[2] = odomCur.pose.pose.position.y;odometry_channel[3] = odomCur.pose.pose.position.z;odometry_channel[4] = odomCur.pose.pose.orientation.x;odometry_channel[5] = odomCur.pose.pose.orientation.y;odometry_channel[6] = odomCur.pose.pose.orientation.z;odometry_channel[7] = odomCur.pose.pose.orientation.w;odometry_channel[8]  = odomCur.twist.twist.linear.x;odometry_channel[9]  = odomCur.twist.twist.linear.y;odometry_channel[10] = odomCur.twist.twist.linear.z;odometry_channel[11] = odomCur.pose.covariance[1];odometry_channel[12] = odomCur.pose.covariance[2];odometry_channel[13] = odomCur.pose.covariance[3];odometry_channel[14] = odomCur.pose.covariance[4];odometry_channel[15] = odomCur.pose.covariance[5];odometry_channel[16] = odomCur.pose.covariance[6];odometry_channel[17] = odomCur.pose.covariance[7];return odometry_channel;
}

d. 可视化，传输数据给RVIZ

​ 接下来就是把数据通过话题，传给RVIZ了。传输的内容，见下面的注释。至于发布信息的函数，就不详细分析了，就是简单的把数据取出，然后封装起来，发送到话题上。到这里，for-each循环已经结束了，然后就是解锁：m_estimator.unlock();。

std_msgs::Header header = img_msg->header;
pubOdometry(estimator, header); // "odometry" 里程计信息PQV
pubKeyPoses(estimator, header); // "key_poses" 关键点三维坐标
pubCameraPose(estimator, header); // "camera_pose" 相机位姿
pubPointCloud(estimator, header); // "history_cloud" 点云信息
pubTF(estimator, header); // "extrinsic" 相机到IMU的外参
pubKeyframe(estimator); // "keyframe_point"、"keyframe_pose" 关键帧位姿和点云

e. 更新IMU参数PQV

m_buf.lock();
m_state.lock();
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)update(); // 更新操作
m_state.unlock();
m_buf.unlock();

update()函数

​ 从estimator中得到滑动窗口当前图像帧的imu更新项[P,Q,V,ba,bg,a,g]。然后对imu_buf中剩余的imu_msg进行PVQ递推。predict函数在上面有分析过。至此，主线程函数分析完成！

void update()
{TicToc t_predict;latest_time = current_time;tmp_P = estimator.Ps[WINDOW_SIZE];tmp_Q = estimator.Rs[WINDOW_SIZE];tmp_V = estimator.Vs[WINDOW_SIZE];tmp_Ba = estimator.Bas[WINDOW_SIZE];tmp_Bg = estimator.Bgs[WINDOW_SIZE];acc_0 = estimator.acc_0;gyr_0 = estimator.gyr_0;queue<sensor_msgs::ImuConstPtr> tmp_imu_buf = imu_buf;for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop())predict(tmp_imu_buf.front());
}

f. 完整代码

void process()
{while (ros::ok()){std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;std::unique_lock<std::mutex> lk(m_buf);con.wait(lk, [&]{return (measurements = getMeasurements()).size() != 0;});lk.unlock();m_estimator.lock();for (auto &measurement : measurements){auto img_msg = measurement.second;double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;for (auto &imu_msg : measurement.first){double t = imu_msg->header.stamp.toSec();double img_t = img_msg->header.stamp.toSec() + estimator.td;if (t <= img_t){ if (current_time < 0)current_time = t;double dt = t - current_time;ROS_ASSERT(dt >= 0);current_time = t;dx = imu_msg->linear_acceleration.x;dy = imu_msg->linear_acceleration.y;dz = imu_msg->linear_acceleration.z;rx = imu_msg->angular_velocity.x;ry = imu_msg->angular_velocity.y;rz = imu_msg->angular_velocity.z;estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));//printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);}else{double dt_1 = img_t - current_time;double dt_2 = t - img_t;current_time = img_t;ROS_ASSERT(dt_1 >= 0);ROS_ASSERT(dt_2 >= 0);ROS_ASSERT(dt_1 + dt_2 > 0);double w1 = dt_2 / (dt_1 + dt_2);double w2 = dt_1 / (dt_1 + dt_2);dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;rx = w1 * rx + w2 * imu_msg->angular_velocity.x;ry = w1 * ry + w2 * imu_msg->angular_velocity.y;rz = w1 * rz + w2 * imu_msg->angular_velocity.z;estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));//printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);}}// TicToc t_s;map<int, vector<pair<int, Eigen::Matrix<double, 8, 1>>>> image;for (unsigned int i = 0; i < img_msg->points.size(); i++){int v = img_msg->channels[0].values[i] + 0.5;int feature_id = v / NUM_OF_CAM;int camera_id = v % NUM_OF_CAM;double x = img_msg->points[i].x;double y = img_msg->points[i].y;double z = img_msg->points[i].z;double p_u = img_msg->channels[1].values[i];double p_v = img_msg->channels[2].values[i];double velocity_x = img_msg->channels[3].values[i];double velocity_y = img_msg->channels[4].values[i];double depth = img_msg->channels[5].values[i];ROS_ASSERT(z == 1);Eigen::Matrix<double, 8, 1> xyz_uv_velocity_depth;xyz_uv_velocity_depth << x, y, z, p_u, p_v, velocity_x, velocity_y, depth;image[feature_id].emplace_back(camera_id,  xyz_uv_velocity_depth);}vector<float> initialization_info;m_odom.lock();initialization_info = odomRegister->getOdometry(odomQueue, img_msg->header.stamp.toSec() + estimator.td);m_odom.unlock();estimator.processImage(image, initialization_info, img_msg->header);// double whole_t = t_s.toc();// printStatistics(estimator, whole_t);std_msgs::Header header = img_msg->header;pubOdometry(estimator, header);pubKeyPoses(estimator, header);pubCameraPose(estimator, header);pubPointCloud(estimator, header);pubTF(estimator, header);pubKeyframe(estimator);}m_estimator.unlock();m_buf.lock();m_state.lock();if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)update();m_state.unlock();m_buf.unlock();}
}

附录：引用

LVI-SAM代码– xuwuzhou的SLAM之路 – 静心，慎思，明辨，笃学

ManiiXu/VINS-Mono-Learning: VINS-Mono

14-[LVI-SAM]visual_odometry_callbackAndprocess相关推荐

1. [Zjoi2015]诸神眷顾的幻想乡

   [Zjoi2015]诸神眷顾的幻想乡 Time Limit: 10 Sec  Memory Limit: 512 MB Submit: 1537  Solved: 892 Description 幽香 ...

2. 中国微量营养素市场趋势报告、技术动态创新及市场预测

   微量营养素市场的企业竞争态势 该报告涉及的主要国际市场参与者有BASF.AkzoNobel.Yara International.Haifa.Agrium.Stoller.Mosaic.JR Simp ...

3. 第三章 使用多维存储(全局变量)（二）

   文章目录 第三章 使用多维存储(全局变量)(二) 遍历全局变量中的数据 $ORDER(下一个/上一个)函数 使用$ORDER循环 其他$ORDER参数 $QUERY函数 第三章 使用多维存储(全局变量 ...

4. SLAM相关学习资料：综述/激光/视觉/数据集/常用库

   点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者丨菠萝包包包@知乎 来源丨https://zhuanlan.zhihu.com/p/4348743 ...

5. elasticsearch之exists查询

   一.exists查询简介 elastic search提供了exists查询,用以返回字段存在值的记录,默认情况下只有字段的值为null或者[]的时候,elasticsearch才会认为字段不存在: ...

6. Hive/Spark SQL使用案例

   Hive/Spark SQL使用案例 求 TOPN:开窗函数 求天数:datediff() 函数 求每个学生的成绩都大于...系列:开窗 / 分组 表转置/行转列系列一:concat_ws 函数 表转 ...

7. 数据结构：答应我，别再逃避我了好吗？

   本文我们来介绍一下编程中常见的一些数据结构. 为什么要学习数据结构? 随着业务场景越来越复杂,系统并发量越来也高,要处理的数据越来越多,特别是大型互联网的高并发.高性能.高可用系统,对技术要求越来越高 ...

8. LVI-SAM 在香港城市数据集UrbanNav的效果

   数据集链接: GitHub - weisongwen/UrbanNavDataset: UrbanNav: an Open-Sourcing Localization Data Collected i ...

9. SQL2008避免出错（聚合函数+Over用法）

   --返回出错 with CTEOrders as(select cast(1 as int) as OrderID, cast('3/1/2012' as date) as OrderDate, ca ...

10. bzoj4516 / P4070 [SDOI2016]生成魔咒

    P4070 [SDOI2016]生成魔咒 后缀自动机 每插入一个字符,对答案的贡献为$len[last]-len[fa[last]]$ 插入字符范围过大,所以使用$map$存储. (去掉第35行就是裸 ...

最新文章

1. Oracle应用开发手记
2. B04_NumPy从已有的数组创建数组（numpy.asarray，numpy.frombuffer，numpy.fromiter）
3. [HNOI2013]数列（差分）
4. 33、JAVA_WEB开发基础之会话机制
5. css-animation-走路动画
6. JavaScript——原型/原型链中的顶层对象（图解）
7. 图书管理系统C语言程序设计课程,vs c语言图书管理平台课程设计_图书管理平台c语言程序设计_c语言课程设计 图书管理系统...
8. 基于JAVA+SpringMVC+MYSQL的酒店客房管理系统
9. Android 应用中十大导航设计错误
10. 360 php SQL注入,php中sql注入漏洞示例
11. VB获取窗体的位置和大小
12. php中的图片变名为8位用什么,CSS_详解PNG图片，1、PNG图片类型 PNG格式有8位、 - phpStudy...
13. 等保三级全称是什么？是什么意思？
14. 图片去水印方法（不用下载任何东西）
15. A - 敌兵布阵--线段树--模板水题
16. 多元线性回归分析spss结果解读_多重线性回归分析SPSS操作与解读
17. 采云端采云链：从订单协同到采购供应链，让采购供应链互联互通
18. 金融学期末复习重点准备
19. oracle数据库中批量把一张表里面的数据插入到不同的表中的方法
20. 301、404、200、304、500等HTTP状态，代表什么意思?

热门文章

1. 用鼠标模拟视线跟踪技术
2. 前端面试题总结（js、html、小程序、React、算法、vue 、全栈热门视频资源）
3. python安装0x80072ee7_错误代码为 0x80072EE7 - 卡饭网
4. 服务器端部署营业执照识别
5. 依存分析：基于序列标注的中文依存句法分析模型实现
6. 【Mac小技巧】如何改变顶部状态栏的颜色
7. 第一次服务器被黑 既惊讶 又后怕!!!
8. 提高生活、学习、工作效率的方法——时间管理Vs个人管理
9. 预约上门App平台的接单方式
10. 再Windows下 .Pages格式怎么保存为word或者PDF格式