VIR-SLAM代码分析2——VIR_VINS详解

前言

VIR-SLAM中VIR_VINS文件夹下是基于VINS-mono的结合UWB传感器的估计器，主要改动的文件在uwb_posegraph，vir_estimator中。其他文件夹完成的是UWB数据的处理问题，比较简单上一节介绍足够，代码也容易看懂。本节介绍的VIR_VINS是VIR-SLAM的核心内容。
在这里插入图片描述

1、uwb_posegraph

启动uwb_posegraph节点的启动代码在launch（vir_estimator/launch下的launch文件）文件当中。这个文件夹是通过复制修改原本的posegraph得到的，用来添加长窗口的结合UWB约束的全局位姿图优化。

uwb_factor.h

class UWBFactor : public ceres::SizedCostFunction<1,3,3>函数
UWBFactor的代价函数构建，残差维度为1，输入参数为三维位移和三维锚点位置。这个factor面向的是全局位姿图优化，是长窗口对于关键帧之间的估计的优化，因此这里也包含了对于锚点位置的优化。在后面四自由度优化中使用。

class UWBFactor : public ceres::SizedCostFunction<1,3,3>
{
  public:
    UWBFactor() = delete;
    float uwbmeas;
    UWBFactor(float dist)
    {
      uwbmeas = dist;
    }

    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {
      Eigen::Vector3d pos(parameters[0][0], parameters[0][1], parameters[0][2]);
      Eigen::Vector3d anchor_pos(parameters[1][0], parameters[1][1], parameters[1][2]);
      Eigen::Vector3d tmp(pos-anchor_pos);
      float predist = tmp.norm();
      float weight = 1;
      residuals[0] = weight*(uwbmeas - predist );
      if(jacobians)
      {
            jacobians[0][0] = weight*(parameters[1][0]-parameters[0][0])/predist;
            jacobians[0][1] = weight*(parameters[1][1]-parameters[0][1])/predist;
            jacobians[0][2] = 0;//-weight*parameters[0][2]/predist;

            jacobians[1][0] = -weight*(parameters[1][0]-parameters[0][0])/predist;
            jacobians[1][1] = -weight*(parameters[1][1]-parameters[0][1])/predist;
            jacobians[1][2] = 0;//-weight*parameters[0][2]/predist;
      }
      //printf(" Meas of uwb is : %f, residual: %f \n", uwbmeas, residuals[0]);
      return true;
    }
};

uwb_posegraph_node.cpp

这个文件中含有主函数，完成对关键帧位姿信息和UWB测距信息的接收，以及开启处理函数线程，长窗口长度为2000个数据点。

int main(int argc, char **argv)
{
    printf("start uwb_posgraph \n");
    ros::init(argc, argv, "uwb_pose_graph");
    ros::NodeHandle n("~");
    posegraph.registerPub(n);

    // read param
    ros::Subscriber sub_pose = n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback);
    ros::Subscriber sub_uwb = n.subscribe("/uwb/corrected_range", 2000, uwb_callback);
    std::thread measurement_process;
    measurement_process = std::thread(process);
    ros::spin();
    return 0;
}

void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)函数
位姿信息保存入栈函数

void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
    m_buf.lock();
    pose_buf.push(pose_msg);
    m_buf.unlock();
}

void uwb_callback(const geometry_msgs::PointStamped &pos_msg)
UWB测量信息保存入栈函数，根据主函数可知这里保存的是经过预处理后的距离测量信息。

void uwb_callback(const geometry_msgs::PointStamped &pos_msg)
{
    m_buf.lock();
    uwb_buf.push_back(pos_msg);
    m_buf.unlock();
}

void process()
主线程处理函数，process中的核心操作，就是根据estimator节点当中发送的关键帧位姿来创建“符合条件”的新关键帧添加到位姿图当中。这里添加的关键帧数据主要是UWB测量值，根据时间戳来确定关键帧的UWB信息，找时间上接近关键帧位姿的UWB测量值添加为关键帧数据，超过1秒舍弃该关键帧。

void process()
{
    if (!UWB_POSEGRAPH)//判断是否使能UWB全局位姿图优化
        return;
    while (true)
    {
        geometry_msgs::PointStampedPtr uwb_msg = NULL;
        nav_msgs::Odometry::ConstPtr pose_msg = NULL;
        double uwb_curRange = -1;

        // find out the messages with same time stamp
        m_buf.lock();
        if(!pose_buf.empty())
        {
            pose_msg = pose_buf.front();
            while (!pose_buf.empty())
                pose_buf.pop();

        }

        m_buf.unlock();

        if (pose_msg != NULL)// when receive the key pose topic, means this is the keyFrame.
        {
            m_buf.lock();
            
            double t_time;
            t_time = pose_msg->header.stamp.toSec();

            // Find the uwb measurement for the pose
            if (!uwb_buf.empty() && uwb_buf.back().header.stamp.toSec()>=t_time)
            {
                //find the first time smaller than pose time
                for (std::deque<geometry_msgs::PointStamped>::reverse_iterator it = uwb_buf.rbegin(); it!=lastUwbIt; it++)
                {
                    if (it->header.stamp.toSec()<t_time && it->header.stamp.toSec()>t_time-1)
                    {
                        uwb_curRange = it->point.x;
                        lastUwbIt = it;
                        printf(" received uwb_curRange %f \n", uwb_curRange);
                        break;
                    }
                    else if (it->header.stamp.toSec()<t_time-1)
                    {
                        printf(" No approriate UWB measure matched \n");
                        lastUwbIt = it;
                        break;
                    }
                }
            }
            else
            {
                printf(" No UWB measure received or no new \n");
            }
            
            m_buf.unlock();
            // build keyframe
            Vector3d T = Vector3d(pose_msg->pose.pose.position.x,
                                  pose_msg->pose.pose.position.y,
                                  pose_msg->pose.pose.position.z);
            Matrix3d R = Quaterniond(pose_msg->pose.pose.orientation.w,
                                     pose_msg->pose.pose.orientation.x,
                                     pose_msg->pose.pose.orientation.y,
                                     pose_msg->pose.pose.orientation.z).toRotationMatrix();
            
            KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R);   
            if(uwb_curRange != -1)
            {
                // add uwb meas to keyframe.
                keyframe->hasUwbMeas = true;
                keyframe->uwbMeas = lastUwbIt->point.x;
                printf(" uwb meas %f \n", lastUwbIt->point.x);
            }

            m_process.lock();
            posegraph.addKeyFrame(keyframe);
            m_process.unlock();
            frame_index++;
       
        }

        std::chrono::milliseconds dura(5);
        std::this_thread::sleep_for(dura);
    }
}

uwb_posegraph.cpp/.h

uwbPoseGraph::uwbPoseGraph()
为uwbPoseGraph类的构造函数，该函数里自动启动optimize4DoF函数线程，用来对全局信息长窗口进行优化。

uwbPoseGraph::uwbPoseGraph()
{

	t_optimization = std::thread(&uwbPoseGraph::optimize4DoF, this);
    t_drift = Eigen::Vector3d(0, 0, 0);
    yaw_drift = 0;
    r_drift = Eigen::Matrix3d::Identity();
    global_index = 0;
    nav_msgs::Path uwbpg_path;

    anchor_pos[0]=1.0;
    anchor_pos[1]=1.0;
    anchor_pos[2]=1.0;

}

void uwbPoseGraph::optimize4DoF()函数

进行位姿图优化是为了将已经产生的所有位姿统一到一个全局一致的配置当中。如VINS论文中所说，参考帧处于世界坐标系下，当相机运动的时候在这里插入图片描述会相对于参考帧发生变化。而由于重力向量始终不会发生变化，所以从重力方向得到的水平面也不会发生变化，进而该水平面对应的两个向量在这里插入图片描述也不会发生变换。所以，系统中需要计算并且优化的向量只有在这里插入图片描述（也就是位置和旋转），这就是4自由度优化的由来。
参考:https://blog.csdn.net/moyu123456789/article/details/104041710

这里的optimize4DoF()使用的是UWB测量进行约束优化，而不是VINS中的回环检测优化。

void uwbPoseGraph::optimize4DoF()
{
    while(true)
    {
        int cur_index = -1;
        m_optimize_buf.lock();
        while(!optimize_buf.empty())
        {
            cur_index = optimize_buf.front(); // Triger of real optimization work.
            while (!optimize_buf.empty())
            {
                optimize_buf.pop();
            }
        }
        m_optimize_buf.unlock();
        if (cur_index != -1)
        {
            printf("optimize pose graph \n");
            TicToc tmp_t;
            m_keyframelist.lock();
            KeyFrame* cur_kf = getKeyFrame(cur_index);

            int max_length = cur_index + 1;

            // w^t_i   w^q_i
            double t_array[max_length][3];
            Quaterniond q_array[max_length];
            double euler_array[max_length][3];

            ceres::Problem problem;
            ceres::Solver::Options options;
            options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
            //options.minimizer_progress_to_stdout = true;
            options.max_solver_time_in_seconds = 3;
            options.max_num_iterations = 50*3;
            ceres::Solver::Summary summary;
            ceres::LocalParameterization* angle_local_parameterization =
                AngleLocalParameterization::Create();

            list<KeyFrame*>::iterator it;

            // Add anchor position as parameter in the problem.
            
            problem.AddParameterBlock(anchor_pos, 3);

            int i = 0;
            for (it = keyframelist.begin(); it != keyframelist.end(); it++)
            {
                Quaterniond tmp_q;
                Matrix3d tmp_r;
                Vector3d tmp_t;
                (*it)->getVioPose(tmp_t, tmp_r);
                tmp_q = tmp_r;
                t_array[i][0] = tmp_t(0);
                t_array[i][1] = tmp_t(1);
                t_array[i][2] = tmp_t(2);
                q_array[i] = tmp_q;

                Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());
                euler_array[i][0] = euler_angle.x();
                euler_array[i][1] = euler_angle.y();
                euler_array[i][2] = euler_angle.z();


                problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
                problem.AddParameterBlock(t_array[i], 3);

                //add edge
                for (int j = 1; j < 5; j++)
                {
                  if (i - j >= 0)// Frame i and previous frame[i-j]
                  {
                      // Calculate the RT from frame i to i-j: last 5 frame.; hardly calculated.
                    Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
                    Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);
                    relative_t = q_array[i-j].inverse() * relative_t;
                    double relative_yaw = euler_array[i][0] - euler_array[i-j][0];
                    ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
                                                   relative_yaw, euler_conncected.y(), euler_conncected.z());
                    problem.AddResidualBlock(cost_function, NULL, euler_array[i-j], 
                                            t_array[i-j], 
                                            euler_array[i], 
                                            t_array[i]);
                  }
                }

                //add loop edge(removed)

                // add uwb edge
                if((*it)->hasUwbMeas && (*it)->uwbMeas>1)
                {
                    float rawdist = (*it)->uwbMeas;
                    int noise = 0.0 ; //rand()%200;
                    UWBFactor* uwb_factor = new UWBFactor(rawdist+noise/1000.0);
                    problem.AddResidualBlock(uwb_factor, NULL, t_array[i], anchor_pos);     
                }
                if ((*it)->index == cur_index)// add frames untile cur_index.
                    break;
                i++;
            }
            m_keyframelist.unlock();

            ceres::Solve(options, &problem, &summary);
            std::cout << summary.BriefReport()<< "\n";
            if(cur_kf->hasUwbMeas)
                printf("Anchor: %f, %f, %f \n",anchor_pos[0],anchor_pos[1],anchor_pos[2]);
            
            m_keyframelist.lock();
            i = 0;
            geometry_msgs::PoseStamped curPose;
            for (it = keyframelist.begin(); it != keyframelist.end(); it++)
            {
                Quaterniond tmp_q;
                tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));
                Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);
                Matrix3d tmp_r = tmp_q.toRotationMatrix();
                (*it)-> updatePose(tmp_t, tmp_r);

                if ((*it)->index == cur_index)
                    break;
                i++;
            }

            Vector3d cur_t, vio_t;
            Matrix3d cur_r, vio_r;
            cur_kf->getPose(cur_t, cur_r);
            cur_kf->getVioPose(vio_t, vio_r);
            m_drift.lock();
            yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();
            r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));
            t_drift = cur_t - r_drift * vio_t;
            m_drift.unlock();
            it++;//update the frames after cur_index that has not optimimzed with drifts.
            for (; it != keyframelist.end(); it++)
            {
                Vector3d P;
                Matrix3d R;
                (*it)->getVioPose(P, R);
                P = r_drift * P + t_drift;
                R = r_drift * R;
                (*it)->updatePose(P, R);
            }
            m_keyframelist.unlock();
            updatePath();
            pub_pose(cur_kf);
        }
    }
}

剩下的函数是发布优化后的位姿、轨迹用的，还有析构函数保持优化线程的持续、关键帧添加与获取等函数，这里不赘述，这些代码功能比较好理解。

2、vir_estimator

状态估计器，包含VIR-SLAM的完整功能：初始化、位姿图优化等等，经过修改的代码是estimator_node.cpp、estimator.cpp/.h、parameters.cpp/.h、uwb_factor.h。
factor主要用于非线性优化对各个参数块和残差块的定义，VINS采用的是ceres，所以这部分需要对一些状态量和因子进行继承和重写。VIR-SLAM添加了uwb_factor文件用来存放uwb代价函数。

class UWBFactor : public ceres::SizedCostFunction<1,7,3>
{
  public:
    UWBFactor() = delete;
    double uwbmeas;
    double weight;
    UWBFactor(double dist, double _weight)
    {
      uwbmeas = dist;
      weight = _weight;
    }

    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {
      Eigen::Vector3d pos(parameters[0][0], parameters[0][1], parameters[0][2]);
      Eigen::Vector3d anchor_pos(parameters[1][0], parameters[1][1], parameters[1][2]);
      Eigen::Vector3d tmp = pos-anchor_pos;
      double predist = tmp.norm();
      residuals[0] = weight*(uwbmeas - predist );
      if(jacobians)
      {
            jacobians[0][0] = weight*(parameters[1][0]-parameters[0][0])/predist;
            jacobians[0][1] = weight*(parameters[1][1]-parameters[0][1])/predist;
            //jacobians[0][2] = weight*(parameters[1][2]-parameters[0][2])/predist;
            jacobians[0][2] = 0;//-weight*parameters[0][2]/predist;
            jacobians[0][3] = 0;//-weight*parameters[0][2]/predist;
            jacobians[0][4] = 0;//-weight*parameters[0][2]/predist;
            jacobians[0][5] = 0;//-weight*parameters[0][2]/predist;
            jacobians[0][6] = 0;//-weight*parameters[0][2]/predist;

            if (KNOWN_ANCHOR == 1)//锚点已经固定
            {
              jacobians[1][0] = 0.;//
              jacobians[1][1] = 0.;//
              jacobians[1][2] = 0.;//
            }
            else
            {      
              jacobians[1][0] = -weight*(parameters[1][0]-parameters[0][0])/predist;//0.;//
              jacobians[1][1] = -weight*(parameters[1][1]-parameters[0][1])/predist;//0.;//
              jacobians[1][2] = -weight*(parameters[1][2]-parameters[0][2])/predist;//0;//
            }
      }
      return true;
    }
};


class UWBAnchorFactor : public ceres::SizedCostFunction<1,3>
{
  public:
    UWBAnchorFactor() = delete;
    double uwbmeas;
    double weight;
    Eigen::Vector3d position;
    
    UWBAnchorFactor(double dist, double _weight, Eigen::Vector3d _position)
    {
      uwbmeas = dist;
      weight = _weight;
      position = _position;
    }

    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {
      Eigen::Vector3d anchor_pos(parameters[0][0], parameters[0][1], parameters[0][2]);
      Eigen::Vector3d tmp = position-anchor_pos;
      double predist = tmp.norm();
      residuals[0] = weight*(uwbmeas - predist );
      if(jacobians)
      {
            jacobians[0][0] = -weight*(parameters[0][0]-position[0])/predist;
            jacobians[0][1] = -weight*(parameters[0][1]-position[1])/predist;
            jacobians[0][2] = -weight*(parameters[0][2]-position[2])/predist;
            //jacobians[0][2] = 0;//-weight*parameters[0][2]/predist;

      }
      return true;
    }
};

initial初始化程序，主要用于初始化，VINS采用的初始化策略是先SfM进行视觉初始化，再与IMU进行松耦合，与VINS-mono一致。
utility，里面放着用于可视化的函数和tictok计时器，与VINS-mono一致。

feature_manager.cpp/.h特征点管理，三角化，关键帧等，与VINS-mono一致。

下面这三个文件相比于VINS-mono进行了修改

estimator_node.cpp

ROS 节点函数，回调函数。
predict()函数：从IMU测量值imu_msg和上一个PVQ递推得到当前PVQ。
update()函数：得到窗口最后一个图像帧的imu项[P,Q,V,ba,bg,a,g]，对imu_buf中剩余imu_msg进行PVQ递推。
getMeasurements()函数：对imu和图像数据进行对齐并组合。
imu_callback()函数：imu回调函数，将imu_msg存入imu_buf，递推IMU的PQV并发布"imu_propagate”。
uwb_callback()函数：uwb回调函数，将uwb_msg.point.x存入uwb_buf。
feature_callback()函数：feature回调函数，将feature_msg放入feature_buf。
restart_callback()函数：restart回调函数，收到restart消息时清空feature_buf和imu_buf，估计器重置，时间重置。
relocalization_callback()函数：relocalization回调函数，将points_msg放入relo_buf。
void process(ros::Publisher pub_debug_uwb)函数
VIO主线程，通过while (true)不断循环，主要功能包括等待并获取measurements，计算dt，然后执行以下功能：进行IMU预积分；设置重定位帧；处理图像帧等，VIR-SLAM不同之处在于添加了对于UWB数据的处理。

void process(ros::Publisher pub_debug_uwb)
{
    while (true)
    {
        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) 遍历获取的Feature和IMU测量值，对measurements中的每一个measurement组合进行操作
        {
            auto img_msg = measurement.second;
            double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
            for (auto &imu_msg : measurement.first)//IMU预积分处理，某一图像帧下遍历对齐的imu
            {
                double t = imu_msg->header.stamp.toSec();
                double img_t = img_msg->header.stamp.toSec() + estimator.td;
                if (t <= img_t)//发送IMU数据进行预积分
                { 
                    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));//processIMU()实现了IMU的预积分，通过中值积分得到当前PQV作为优化初值
                }
                else// 这就是针对最后一个imu数据，需要做一个简单的线性插值
                {
                    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));
                }
            }
            // set relocalization frame，设置重定位帧
            sensor_msgs::PointCloudConstPtr relo_msg = NULL;
            while (!relo_buf.empty())//取出最后一个重定位帧
            {
                relo_msg = relo_buf.front();
                relo_buf.pop();
            }
            if (relo_msg != NULL)
            {
                vector<Vector3d> match_points;
                double frame_stamp = relo_msg->header.stamp.toSec();
                for (unsigned int i = 0; i < relo_msg->points.size(); i++)
                {
                    Vector3d u_v_id;
                    u_v_id.x() = relo_msg->points[i].x;
                    u_v_id.y() = relo_msg->points[i].y;
                    u_v_id.z() = relo_msg->points[i].z;
                    match_points.push_back(u_v_id);
                }
                Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);
                Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);
                Matrix3d relo_r = relo_q.toRotationMatrix();
                int frame_index;
                frame_index = relo_msg->channels[0].values[7];
                estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r); 重定位
            }

            ROS_DEBUG("processing vision data with stamp %f \n", img_msg->header.stamp.toSec());

            TicToc t_s;
            map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;//建立每个特征点的(camera_id,[x,y,z,u,v,vx,vy])s的map，索引为feature_id
            for (unsigned int i = 0; i < img_msg->points.size(); i++)//遍历img_msg里面的每一个特征点的归一化坐标
            {
                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];
                ROS_ASSERT(z == 1);
                Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
                xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
                image[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
            }

            if (USE_UWB){//	UWB数据处理
                if (estimator.uwb_buf.empty()){
                    estimator.uwb_keymeas.push_back(-1);
                }
                else{ 
                    m_buf.lock();
                    // ... fill vec with values (do not use 0; use 0.0)
                    double average = std::accumulate(estimator.uwb_buf.begin(), estimator.uwb_buf.end(), 0.0) / estimator.uwb_buf.size();//对uwb_buf中的uwb数据求和取平均值
                    estimator.uwb_keymeas.push_back(average);//将平均值添加到uwb_keymeas
                    estimator.uwb_buf.clear();//清空uwb_buf
                    m_buf.unlock();

                    geometry_msgs::PointStamped msgnow;
                    msgnow.point.x = average;
                    pub_debug_uwb.publish(msgnow);//发布存下来的当前uwb平均值
                }

                if (KNOWN_ANCHOR == 0 && estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)//锚点没有固定下来时
                {
                    estimator.uwbMeas4AnchorEst.push_back(estimator.uwb_keymeas.back());//将estimator.uwb_keymeas.back()存入estimator.uwbMeas4AnchorEst
                }
                
            }
            estimator.processImage(image, img_msg->header);//处理图像特征，处理图像帧：初始化，紧耦合的非线性优化，这里包含了uwb的处理，这是一个非常重要的函数。

            double whole_t = t_s.toc();
            printStatistics(estimator, whole_t);
            std_msgs::Header header = img_msg->header;
            header.frame_id = "world";
//向RVIZ发送topic，向RVIZ发布里程计信息、关键位姿、相机位姿、点云和TF关系
            pubOdometry(estimator, header);//"odometry"里程计PVQ信息
            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" 关键帧位姿和点云
            if (relo_msg != NULL)
                pubRelocalization(estimator);//"relo_relative_pose" 重定位位姿
        }
        m_estimator.unlock();
        m_buf.lock();  //更新IMU参数
        m_state.lock();
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            update();//更新IMU参数[P,Q,V,ba,bg,a,g]
        m_state.unlock();
        m_buf.unlock();
    }
}

主函数main()
完成节点订阅与发布，创建VIO主线程。

int main(int argc, char **argv)
{
    ros::init(argc, argv, "vir_estimator");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
    readParameters(n);
    estimator.setParameter();
#ifdef EIGEN_DONT_PARALLELIZE
    ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
    ROS_WARN("waiting for image and imu...");

    registerPub(n);

    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    ros::Subscriber sub_image = n.subscribe("/vir_feature_tracker/feature", 2000, feature_callback);
    ros::Subscriber sub_restart = n.subscribe("/vir_feature_tracker/restart", 2000, restart_callback);
    ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);

    ros::Subscriber sub_uwb = n.subscribe("/uwb/corrected_range", 1000, uwb_callback);

    ros::Publisher pub_debug_uwb = n.advertise<geometry_msgs::PointStamped>("debug_uwb_keymeas", 1000);


    std::thread measurement_process{process, pub_debug_uwb};
    ros::spin();

    return 0;
}

estimator.cpp/.h

紧耦合的UWB-VIO状态估计器实现，这里存储着实现紧耦合UWB-VIO状态估计的函数，重点是processImage函数，其中含有结合了uwb的优化处理。processImage()->solveOdometry()->optimization()，结合uwb的优化方面主要在optimization()函数中。下一篇博客详细介绍。
函数功能介绍：
void Estimator::setParameter() 设置部分参数
void Estimator::clearState() 清空或初始化滑动窗口中所有的状态量
void Estimator::processIMU() 处理IMU数据，预积分
void Estimator::processImage() 处理图像特征数据
bool Estimator::initialStructure() 视觉的结构初始化
bool Estimator::visualInitialAlign() 视觉惯性联合初始化
bool Estimator::relativePose() 判断两帧有足够视差30且内点数目大于12则可进行初始化，同时得到R和T
void Estimator::solveOdometry() VIO非线性优化求解里程计
void Estimator::vector2double() vector转换成double数组，因为ceres使用数值数组
void Estimator::double2vector() 数据转换，vector2double的相反过程
bool Estimator::failureDetection() 检测系统运行是否失败
void Estimator::optimization() 基于滑动窗口的紧耦合的非线性优化，残差项的构造和求解
void Estimator::slideWindow() 滑动窗口法
void Estimator::setReloFrame() 重定位操作
void Estimator::estimateAnchorPos() 锚点估计函数，同样在imageprocess函数中调用的。
重点是VIR-SLAM中的optimization()以及slideWindow()，下一弹详细介绍estimator.cpp/.h中的函数。

parameters.cpp/.h

处理前端中需要用到的一些参数,根据uwb数据情况进行了修改。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/196125.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

软件测试简历编写技巧，对症下药，一周面10家...

软件测试简历编写技巧，对症下药，一周面10家...

目录&#xff1a;导读 前言一、Python编程入门到精通二、接口自动化项目实战三、Web自动化项目实战四、App自动化项目实战五、一线大厂简历六、测试开发DevOps体系七、常用自动化测试工具八、JMeter性能测试九、总结&#xff08;尾部小惊喜&#xff09; 前言 作为HR来说&#…
阅读更多...
吉利银河L6 纯电模式 空调加热功率

吉利银河L6 纯电模式 空调加热功率

环境 环境10摄氏度左右 空调加热最高档 风速最高档 慢充充电 地下车库 开空调充电 关空调充电 分析 充电功率6kw 开关空调时间差了一倍 所以估算出空调加热峰值功率大概3kw左右
阅读更多...
aPEAR包绘制功能富集网络图

aPEAR包绘制功能富集网络图

本期教程 前言 今天学习aPEAR包&#xff0c;绘制KEGG和GO功能富集网络图&#xff0c;用起来还是比较方便的&#xff0c;直接将clusterProfiler富集结果进行绘制&#xff0c;对人类、动物等分析结果非常方便。对于模式植物&#xff0c;使用自己制作的GO或KEGG背景文件进行富集分…
阅读更多...
UI上传组件异步上传更改为同步

UI上传组件异步上传更改为同步

实现异步方法 JavaScript 异步 实现异步的五种实现方法_js异步-CSDN博客 这两种比较经常用。 因为上传组件是异步上传的通过Async和await配合使用可以上传完照片视频后返回的地址在继续走下去&#xff0c;而不是图片视频地址还未获取时就上传后端了。
阅读更多...
Notion for Mac：打造您的专属多功能办公笔记软件

Notion for Mac：打造您的专属多功能办公笔记软件

在如今这个信息爆炸的时代&#xff0c;一款高效、便捷的笔记软件对于办公人士来说已经成为必不可少的工具。Notion for Mac&#xff0c;作为一款多功能办公笔记软件&#xff0c;凭借其简洁优雅的界面、强大的功能以及无缝的云端同步&#xff0c;成为了众多用户的首选。 一、多…
阅读更多...
9.二维数组——打印出杨辉三角形（要求打印出10行）

9.二维数组——打印出杨辉三角形（要求打印出10行）

文章目录 前言一、题目描述 二、题目分析 三、解题 程序运行代码 前言 本系列为二维数组编程题&#xff0c;点滴成长&#xff0c;一起逆袭。 一、题目描述 打印出杨辉三角形&#xff08;要求打印出10行&#xff09;。 二、题目分析 三、解题 程序运行代码 #include<s…
阅读更多...
python实现rpc的几种方式（SimpleXMLRPCServer 自带的、第三方ZeroRPC）、连接linux远程开发分布式锁、分布式id

python实现rpc的几种方式（SimpleXMLRPCServer 自带的、第三方ZeroRPC）、连接linux远程开发分布式锁、分布式id

1 python实现rpc的几种方式 1.1 SimpleXMLRPCServer 自带的 1.2 第三方ZeroRPC 2 连接linux远程开发 3 分布式锁 4 分布式id 1 python实现rpc的几种方式 # 远程过程调用-1 借助于rabbitmq,可以跨语言-2 SimpleXMLRPCServer 自带的-3 ZeroRPC-4 GRPC&#xff1a;跨语言的 htt…
阅读更多...
GAN：ImprovedGAN-训练GAN的改进策略

GAN：ImprovedGAN-训练GAN的改进策略

论文&#xff1a;https://arxiv.org/abs/1606.03498 代码&#xff1a;https://github.com/openai/improved_gan 发表&#xff1a;NIPS 2016 一、文章创新 1&#xff1a;Feature matching&#xff1a;特征匹配通过为生成器指定新目标来解决GANs的不稳定性&#xff0c;从而防止…
阅读更多...
富富集网络图绘制教程

富富集网络图绘制教程

本期教程 前言 今天学习aPEAR包&#xff0c;绘制KEGG和GO功能富集网络图&#xff0c;用起来还是比较方便的&#xff0c;直接将clusterProfiler富集结果进行绘制&#xff0c;对人类、动物等分析结果非常方便。对于模式植物&#xff0c;使用自己制作的GO或KEGG背景文件进行富集分…
阅读更多...
Python能否成为大型游戏开发的利器？

Python能否成为大型游戏开发的利器？

你是否曾想过&#xff0c;Python这个备受欢迎的编程语言是否能够胜任大型游戏开发的重任&#xff1f;Python以其简洁、易学的特点而著称&#xff0c;但在游戏世界中&#xff0c;性能和效率常常是关键。小编将带你深入探讨Python在大型游戏开发中的潜力&#xff0c;一探究竟&…
阅读更多...
【Unity细节】为什么加载精灵图集直接导致Unity引擎崩溃

【Unity细节】为什么加载精灵图集直接导致Unity引擎崩溃

&#x1f468;‍&#x1f4bb;个人主页&#xff1a;元宇宙-秩沅 hallo 欢迎 点赞&#x1f44d; 收藏⭐ 留言&#x1f4dd; 加关注✅! 本文由 秩沅 原创 &#x1f636;‍&#x1f32b;️收录于专栏&#xff1a;unity细节和bug &#x1f636;‍&#x1f32b;️优质专栏 ⭐【…
阅读更多...
立即修复计算机显示msvcp110.dll丢失问题！4个快速解决方法大揭秘

立即修复计算机显示msvcp110.dll丢失问题！4个快速解决方法大揭秘

在计算机使用过程中&#xff0c;我们可能会遇到一些错误提示&#xff0c;其中之一就是“msvcp110.dll丢失”。这个错误通常会导致某些程序无法正常运行&#xff0c;给用户带来诸多不便。那么&#xff0c;当我们遇到这个问题时&#xff0c;应该如何进行修复呢&#xff1f;本文将…
阅读更多...
数据仓库建模下篇

数据仓库建模下篇

在实际业务中&#xff0c;给了我们一堆数据&#xff0c;我们怎么拿这些数据进行数仓建设呢&#xff0c;数仓工具箱作者根据自身多年的实际业务经验&#xff0c;给我们总结了如下四步。 数仓工具箱中的维度建模四步走&#xff1a; 维度建模四步走 这四步是环环相扣&#xff0c…
阅读更多...
latex中$$中的字母不显示斜体【已解决】

latex中$$中的字母不显示斜体【已解决】

最近在用latex写论文&#xff0c;其中一篇论文的方法名带有平方&#xff0c;但是当我写方法名的时候发现字母名称是斜体的&#xff0c;如下图所示 引用的论文中FedME这几个字显然不是斜体&#xff0c;最后修改完的图片如下图所示 代码如下所示 /非斜体代码 $\text{FedME}^{2}$…
阅读更多...
怎么把dwg格式转换pdf？

怎么把dwg格式转换pdf？

怎么把dwg格式转换pdf&#xff1f;DWG是一种由AutoCAD开发的二维和三维计算机辅助设计&#xff08;CAD&#xff09;文件格式&#xff0c;它的名称是“绘图&#xff08;Drawing&#xff09;”的缩写。DWG文件通常包含了设计图纸、模型和元数据等信息&#xff0c;并且被广泛用于工…
阅读更多...
利用STM32和蓝牙模块构建智能物联网设备的开发指南

利用STM32和蓝牙模块构建智能物联网设备的开发指南

智能物联网设备在现代生活中扮演着重要的角色&#xff0c;而STM32微控制器和蓝牙模块则为实现智能物联网设备提供了基础支持。本文将介绍如何使用STM32微控制器和蓝牙模块构建智能物联网设备的开发指南&#xff0c;包括硬件设计、蓝牙模块配置、传感器数据采集和云平台连接等关…
阅读更多...
【开源威胁情报挖掘1】引言 + 开源威胁情报挖掘框架 + 开源威胁情报采集与识别提取

【开源威胁情报挖掘1】引言 + 开源威胁情报挖掘框架 + 开源威胁情报采集与识别提取

基于开源信息平台的威胁情报挖掘综述 写在最前面摘要1 引言近年来的一些新型网络安全威胁类型挖掘网络威胁的情报信息威胁情报分类&#xff1a;内、外部威胁情报国内外开源威胁情报挖掘分析工作主要贡献研究范围和方法 2 开源威胁情报挖掘框架1. 开源威胁情报采集与识别2. 开源…
阅读更多...
Android Studio 添加so无法打包进apk问题

Android Studio 添加so无法打包进apk问题

1.开发环境&#xff1a; Android Studio 2022.3.1 Patch 2 jdk 17 gradle-7.4 2.build.grade配置检查 首先查看build.gradle中是否设置sourceSets &#xff0c;如果设置的话&#xff0c;打包的时候so是被指导libs目录下的&#xff0c;所有就不能把jnilibs下。 sourceSets {mai…
阅读更多...
vue中keep-alive的使用

vue中keep-alive的使用

什么是keep-alive&#xff1f; keep-alive是一个内置组件&#xff0c;用于缓存和管理组件的状态。 当 keep-alive包裹一个组件时&#xff0c;这个组件的状态将会被缓存起来&#xff0c;而不是每次重新渲染。这在多个视图之间切换时特别有用&#xff0c;可以避免重复的创建和销…
阅读更多...
RT_Thread_内核包版本与芯片包版本不一致的编译报错排查

RT_Thread_内核包版本与芯片包版本不一致的编译报错排查

按时间线写的&#xff0c;建议看完&#xff0c;因为中间有的步骤不必重蹈覆辙。 1、安装RT_Thread Studio、STM32F4的SDK&#xff0c;新建工程编译报错error: struct serial_configure has no member named flowcontrol 1.1、报错含义 结构体struct serial_configure没有flow…
阅读更多...
最新文章

Copyright @ 2022~2023