VINS-Mono 代码详细解读——基础储备:在线Cam到IMU的外参标定 InitialEXRotation类

2023-05-16

本讲还是为了estimator类中最主要的函数processImage()做知识储备。

前面两讲知识储备主要讲了IMU预积分相关的integrationBase类以及图像特征点管理器feature_manager.cpp,本节将对processImage()的相机到IMU的外参矩阵的代码讲解。

当外参完全不知道的时候,VINS也可以在线对其进行估计(rotation),先在processImage内进行初步估计,然后在后续优化时,会在optimize函数中再次优化。

目录

InitialEXRotation类

1、CalibrationExRotation()函数

1.初始化获取几个旋转矩阵,放入到vector中

2、SVD分解,Ax=0,对A填充。多帧组成A,一对点组成的A是4*4的。

3.至少迭代计算了WINDOW_SIZE次,且R的奇异值大于0.25才认为标定成功

2、solveRelativeR()

3、decomposeE()

4、testTriangulation()


我们首先来回顾以下整体的processImage()函数流程 

processImage()中:

第一步为检测当前输入的image是否为关键帧,f_manager.addFeatureCheckParallax()

第二步为更新临时预积分初始值  tmp_pre_integration = new IntegrationBase() 

第三步为:本文的贡献中相机和IMU外参的在线标定作为创新点。initial_ex_rotation.CalibrationExRotation()代码如下:

// 3. 如果没有外参则标定IMU到相机的外参
// 0表示外参优化完成;1表示粗略估计,后面作为初始值放进非线性优化;2表示需要进行标定。
    if(ESTIMATE_EXTRINSIC == 2)
    {
        ROS_INFO("calibrating extrinsic param, rotation movement is needed");
        if (frame_count != 0)
        {
            //得到corres传入的是当前帧和其之前一帧的对应特征点对的归一化坐标。
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            Matrix3d calib_ric;
            //标定从camera到IMU之间的外参数
            if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
            {
                ROS_WARN("initial extrinsic rotation calib success");
                ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
                //有几个相机,就有几个ric,目前单目情况下,ric内只有一个值
                ric[0] = calib_ric;
                RIC[0] = calib_ric;
                ESTIMATE_EXTRINSIC = 1;// 完成外参标定
            }
        }
    }

InitialEXRotation类

位于vins_estimator/src/initial/initial_ex_rotation.cpp,首先介绍一个这个类的成员变量以及成员函数:

/* 标定IMU和相机的外参数 */
class InitialEXRotation
{
public:
	InitialEXRotation();
    // !!!重要,主函数标定外参旋转矩阵
    bool CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result);
private:
    // 两帧之间相机系旋转矩阵 
	Matrix3d solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres);

    double testTriangulation(const vector<cv::Point2f> &l,
                             const vector<cv::Point2f> &r,
                             cv::Mat_<double> R, cv::Mat_<double> t);
    // 本质矩阵SVD分解求得4组RT                         
    void decomposeE(cv::Mat E,
                    cv::Mat_<double> &R1, cv::Mat_<double> &R2,
                    cv::Mat_<double> &t1, cv::Mat_<double> &t2);
    int frame_count;

    vector< Matrix3d > Rc;// 相机之间旋转矩阵,对极几何得到
    vector< Matrix3d > Rimu;// 两个IMU之间旋转矩阵,由IMU预积分得到
    vector< Matrix3d > Rc_g;  // 每个IMU相对于起始IMU的旋转矩阵
    Matrix3d ric;// 相机到IMU的外参
};

1、CalibrationExRotation()函数

标定相机和IMU的外参数。

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)

输入参数为:vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, 匹配的特征点 和 IMU预积分得的旋转矩阵Q

输出参数:Matrix3d &calib_ric_result    标定的外参数

1.初始化获取几个旋转矩阵,放入到vector中

记录第几次进入这个函数,标定的过程会多次进入到这个约束,直到标定成功,每进一次都会得到公式的约束。

frame_count++;

 计算前后两帧的旋转矩阵,加到Rc向量内。

  • 相机帧之间匹配点得到本质矩阵,分解得到旋转矩阵R_ck+1^ck
  • IMU之间预积分得到旋转矩阵R_bk+1^bk
  • 每次迭代之前先用上一次估计的ric将R_bk+1^bk转换为R_ck+1^ck,为了下面求解核函数。

    Rc.push_back(solveRelativeR(corres));// 相机帧之间旋转矩阵
    Rimu.push_back(delta_q_imu.toRotationMatrix());//IMU之间旋转矩阵,由IMU预积分得到
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);//每帧IMU相对于起始帧IMU的R,初始化为IMU预积分

2、旋转约束估计外参数q_{b}^{c}

SVD分解,Ax=0,对A填充。多帧组成A,一对点组成的A是4*4的

机器人手眼标定的方法计算出外参的矩阵。

其中,q_{b_{k+1}}^{b_{k}}是IMU预积分得到的,q_{c_{k+1}}^{c_{k}}是用8点法对前后帧的特征点计算得到的。

详细见《Monocular Visual-Inertial State Estimation With Online Initialization and Camera-IMU Extrinsic Calibration》

b_{k}c_{k+1}有两条路径:第一条是从b_{k}\rightarrow b_{k+1 }\rightarrow c_{k+1 },对应绿色部分。

                                        第二条是b_{k}\rightarrow c_{k}\rightarrow c_{k+1 },对应红色部分。

核函数说明

angularDistance就是计算两个坐标系之间相对旋转矩阵在做轴角变换后(u * theta)
的角度theta, theta越小说明两个坐标系的姿态越接近,这个角度距离用于后面计算权重,
这里计算权重就是为了降低外点的干扰,意思就是为了防止出现误差非常大的R_bk+1^bk和 
R_ck+1^ck约束导致估计的结果偏差太大 

这里权重用来抑制过大残差值的影响,表现为w_{k+1}^{k}

残差r_{k+1}^{k}是两条路径构建的误差。

 // 2.1求解核函数
        Quaterniond r1(Rc[i]);
        Quaterniond r2(Rc_g[i]);
        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        ROS_DEBUG(
            "%d %f", i, angular_distance);
        //huber核函数做加权
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;

 对L和R赋值的情况做以下说明

四元数左乘(最后一列是b):对应相机L

四元数右乘(最后一列是a):对应IMU

    // 2.SVD分解,Ax=0,对A填充
    Eigen::MatrixXd A(frame_count * 4, 4);// 多帧组成A,一对点组成的A是4*4的。
    A.setZero();
    int sum_ok = 0;
    for (int i = 1; i <= frame_count; i++)
    {
        // 2.1求解核函数
        Quaterniond r1(Rc[i]);
        Quaterniond r2(Rc_g[i]);
        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        ROS_DEBUG(
            "%d %f", i, angular_distance);
        //huber核函数做加权
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        // 2.2 分别为L和R赋值,L R 分别为左乘和右乘矩阵,都是4*4矩阵
        //R_bk+1^bk * R_c^b = R_c^b * R_ck+1^ck
        //[Q1(q_bk+1^bk) - Q2(q_ck+1^ck)] * q_c^b = 0
        Matrix4d L, R;

        double w = Quaterniond(Rc[i]).w();// 相机间旋转矩阵
        Vector3d q = Quaterniond(Rc[i]).vec();
        L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
        L.block<3, 1>(0, 3) = q;
        L.block<1, 3>(3, 0) = -q.transpose();
        L(3, 3) = w;

        Quaterniond R_ij(Rimu[i]);// IMU间旋转矩阵
        w = R_ij.w();
        q = R_ij.vec();
        R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
        R.block<3, 1>(0, 3) = q;
        R.block<1, 3>(3, 0) = -q.transpose();
        R(3, 3) = w;
        // A=Q1(q_bk+1^bk) - Q2(q_ck+1^ck)=huber(L-R)
        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }

    // 2.3 svd分解中最小奇异值对应的右奇异向量作为旋转四元数
    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    //这里的四元数存储的顺序是[x,y,z,w]',即[qv qw]'
    Matrix<double, 4, 1> x = svd.matrixV().col(3);// 右奇异向量作为旋转四元数
    Quaterniond estimated_R(x);
    ric = estimated_R.toRotationMatrix().inverse();// 求逆
    //cout << svd.singularValues().transpose() << endl;
    //cout << ric << endl;
    Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();

参考:https://zhuanlan.zhihu.com/p/108658768

3.至少迭代计算了WINDOW_SIZE次,且R的奇异值大于0.25才认为标定成功

if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;

2、solveRelativeR()

根据两帧归一化特征点求解两帧的旋转矩阵

//根据两帧归一化特征点求解两帧的旋转矩阵
Matrix3d InitialEXRotation::solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres)
{
    if (corres.size() >= 9)// 需要特征点大于9对,否则返回单位矩阵
    {
        // 归一化相机系下前二维坐标SVD分解
        vector<cv::Point2f> ll, rr;
        for (int i = 0; i < int(corres.size()); i++)
        {
            ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
            rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
        }
        // 求解两帧的本质矩阵E
        cv::Mat E = cv::findFundamentalMat(ll, rr);
        cv::Mat_<double> R1, R2, t1, t2;
        // 本质矩阵svd分解得到4组Rt解
        decomposeE(E, R1, R2, t1, t2);
        // 如果行列式为负,SVD分解-E
        if (determinant(R1) + 1.0 < 1e-09)
        {
            E = -E;
            decomposeE(E, R1, R2, t1, t2);
        }

        // 通过三角化得到的正深度选择Rt解
        double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));
        double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));
        cv::Mat_<double> ans_R_cv = ratio1 > ratio2 ? R1 : R2;

        // 对R求转置
        Matrix3d ans_R_eigen;
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
                ans_R_eigen(j, i) = ans_R_cv(i, j);
        return ans_R_eigen;
    }
    return Matrix3d::Identity();
}

3、decomposeE()

void InitialEXRotation::decomposeE(cv::Mat E,
                                 cv::Mat_<double> &R1, cv::Mat_<double> &R2,
                                 cv::Mat_<double> &t1, cv::Mat_<double> &t2)
{
    cv::SVD svd(E, cv::SVD::MODIFY_A);
    cv::Matx33d W(0, -1, 0,
                  1, 0, 0,
                  0, 0, 1);
    cv::Matx33d Wt(0, 1, 0,
                   -1, 0, 0,
                   0, 0, 1);
    R1 = svd.u * cv::Mat(W) * svd.vt;
    R2 = svd.u * cv::Mat(Wt) * svd.vt;
    t1 = svd.u.col(2);
    t2 = -svd.u.col(2);
}

4、testTriangulation()

double InitialEXRotation::testTriangulation(const vector<cv::Point2f> &l,
                                          const vector<cv::Point2f> &r,
                                          cv::Mat_<double> R, cv::Mat_<double> t)
{
    cv::Mat pointcloud;
    cv::Matx34f P = cv::Matx34f(1, 0, 0, 0,
                                0, 1, 0, 0,
                                0, 0, 1, 0);
    cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0),
                                 R(1, 0), R(1, 1), R(1, 2), t(1),
                                 R(2, 0), R(2, 1), R(2, 2), t(2));
    cv::triangulatePoints(P, P1, l, r, pointcloud);// 三角化得到路标3D坐标
    int front_count = 0;
    for (int i = 0; i < pointcloud.cols; i++)
    {
        double normal_factor = pointcloud.col(i).at<float>(3);

        cv::Mat_<double> p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor);
        cv::Mat_<double> p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor);
        if (p_3d_l(2) > 0 && p_3d_r(2) > 0)
            front_count++;
    }
    ROS_DEBUG("MotionEstimator: %f", 1.0 * front_count / pointcloud.cols);
    return 1.0 * front_count / pointcloud.cols;
}

 

本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)

VINS-Mono 代码详细解读——基础储备:在线Cam到IMU的外参标定 InitialEXRotation类 的相关文章

  • 【FFTW库】编译生成 x86、arm 环境下的FFTW库

    FFTW是一个快速计算离散傅里叶变换的标准C语言程序集 xff0c 可计算一维或多维实和复数据以及任意规模的DFT 下面主要介绍的是 x86 环境下 FFTW库的编译过程 xff0c arm环境下的编译过程和FFTW类似 xff0c 不同之
  • C语言学习笔记w2d4

    文章目录 流程控制二循环语句gotowhile练习 do whilefor breakcontinue 作业 流程控制二 循环语句 循环的开始条件 循环的控制条件 循环的结束条件 goto 无条件跳转 xff0c 跳转到指定位置执行 xff
  • C语言学习笔记w2d5

    文章目录 数组一维数组练习字符数组字符串输入与输出 练习 多维数组 作业 数组 一维数组 用一个变量来存储具有一定关系的数据 xff0c 的数据集合叫数组 其中存储的变量是数组元素 a span class token punctuatio
  • Linux基础与C高级w3d4:linux的文件管理(续)、了解shell编程

    管道 作用 xff1a 把一个命令的结果作为另一个命令的输入参数 符号 xff1a 用法 xff1a ls grep test 用户管理 切换用户 xff1a su 用户名 修改用户密码 xff1a sudo passwd 用户 注册用户
  • ARM:day4

    ARM 的接口技术 裸机编程 例 xff1a LED灯 根据电路图找到 LED 找到控制的管脚 GPX2 7根据芯片手册 找到需要配置的地址空间地址 和使能值写汇编代码 编译工具 arm gcc 安装方式 把 bin 添加到PATH bas
  • ARM:day5

    ARM 的接口技术 串口 UART 工作模式 全双工发送二进制 ASCII码 两个设备通过各自的定时器来接收数据 空闲时拉高电平 要开始发数据时起始位拉低 0 发数据 一般是 8 位带一位校验位 结束发送 拉高电平回到空闲状态 缺点 特点
  • Ubuntu上igraph的安装教程

    暑假终于到了 xff0c 距离上一篇博客应该有3个月的时间了 xff0c 没有学期末的忙忙碌碌 xff0c 接下来会陆陆续续地回顾自己学过的和将学的东西一并整理成博客 关于igraph 因为课程需要 xff0c 在信息资源管理的课程上曾学过
  • 还在按鼠标调试?键盘F1-F10取消Fn快速调试

    传统功能键 xff1a F1 F12媒体功能键 xff1a 音量 xff0c 亮度 xff0c 锁屏 xff0c 飞行模式等 多数电脑 xff0c 或者有些升级为win10的电脑 xff0c 传统功能键变为了媒体功能键 xff0c 开发人员
  • Jupyter Notebook+VSCode环境搭建及原理讲解

    版权所有 xff0c 转载注明原地址 时间 xff1a 2020年1月17日17 33 15 created by xff1a Hpbbs 使用JupyterNotebook编辑 文章目录 0x00 前言 Python命令行模式的单一与Ju
  • Rust 安装,卸载,nightly/stable版本切换(全局或工作空间),提高下载速度

    版权所有 xff0c 转载前注明原址 时间 xff1a 2020年1月26日20 10 44 created by xff1a Hpbbs 文章目录 1 1 安装1 2 卸载1 3 更新1 4 Rust镜像源切换1 5 Rust night
  • Sql to Object VS Sql of Object

    文章目录 Java Object 中的 setter 与 getterLINQ 的定位 xff1a sql to objectSql of Object 的思维提出SQL of object 43 er实现SQLer Java Object
  • Ubuntu 桌面版无网络标识

    Ubuntu 配置网络的方式有两种 xff1a 通过桌面网络标识直接配置通过编辑配置文件配置 xff08 很麻烦 xff0c 而且不方便 xff09 因此 xff0c 下面介绍如何恢复桌面的网络标识 xff0c 以便于后续的网络配置 目录
  • repo sync 会自动切换本地分支与remote 分支的对应---如何自定义这种行为

    日后再写
  • [BugFix] [Android] DownloadManager使用流程问题导致的 下载下来的文件找不到

    DownloadManager使用流程错误导致的无法找到下载完成的文件 1 问题描述2 问题解决3 解决后的思考5 场景利用 1 问题描述 先提交 enqueue 请求 xff0c 后配置 DownloadManager span clas
  • View.post VS Handler.post的区别和使用场景对比

    View post 当对应的Widget View 没有attach到window的时候 xff0c 对应的Runnable会被提交到 ViewRootImpl RunQueue xff0c 如果已经attach的话 xff0c 会提交给U
  • Android 数据库安全:用户退出后,事务回滚日志依然保存有相关的数据信息

    详情 xff1a data data package name databases dday db data data package name databases dday db shm data data package name da
  • 关于IPhone无法收发短信---设置iphone短信中心号码

    这是个人拨打客服所收到解决短信 xff1a 尊敬的客户 xff0c 您好 xff01 如您反映的问题未解决或还有其他手机问题 xff0c 您可直接关注终端服务基地的官方微信公众号 xff1a cmcczdfw xff0c 随时随地获取便捷终
  • UART串口通信

    什么是串行通信 xff1f 将数据字节分成一位一位的形式在一条传输线上逐位地发送 优点 xff1a 成本低 xff0c 控制复杂 什么是异步通信 xff1f 异步通信是指通信的发送与接收设备使用各自的时钟控制数据的发送和接收过程 为使双方的
  • CAN总线数据帧

    CAN总线数据帧 1 xff0c 帧起始 xff08 SOF xff09 标识一个数据帧的开始 xff0c 用于帧同步 一个显性位 只有总线在空闲期间节点才能够发送SOF 2 ID 用于确定唯一一条报文 标准帧有11位 xff0c 扩展帧有
  • yolov3算法中关于loss={'yolo_loss': lambda y_true, y_pred: y_pred}的理解

    yolov3算法中关于loss 61 yolo loss lambda y true y pred y pred 的理解 参考文献 xff1a xff08 1 xff09 https www jianshu com p 7e45586c44

随机推荐

  • 【FreeRTOS】内存溢出检测

    Stack overflow detection FreeRTOS官方给了两种内存溢出检测方案 xff1a FreeRTOS stacks and stack overflow checking FreeRTOS is a portable
  • Linux实现简单的udp服务端和客户端(C/C++)

    udp server cpp include lt iostream gt include lt sys types h gt include lt sys socket h gt include lt unistd h gt includ
  • 开源日志库<log4cplus+VS2008使用>整理

    原创作品 xff0c 允许转载 xff0c 转载时请务必以超链接形式标明文章 原始出处 作者信息和本声明 否则将追究法律责任 http pyhcx blog 51cto com 713166 143549 本文出自 碧海笙箫 博客 xff0
  • 二.extern "C"

    extern关键字 xtern可以置于变量或者函数前 xff0c 以标示变量或者函数的定义在别的文件中 xff0c 提示编译器遇到此变量和函数时在其他模块中寻找其定义 此外extern也可用来进行链接指定 一 extern 34 C 34
  • Matlab2013b和Visual Studio 2013混合编程总结

    Matlab2013b和VisualStudio 2013混合编程总结 一 关于软件版本和安装的说明 一般来说 xff0c Matlab版本需高于或者等于VisualStudio的版本 综合版本功能和兼容问题 xff0c 我们选用了Matl
  • [linux] xlwt引起的字符串长度限制解除 & 递归深度限制解除 &Overflow问题

    1 xlwt引起的字符串长度限制 Exception String longer than 32767 characters 由于xlwt引起的excel写入的字符串 xff0c 长度不能大于32767 1 换txt写入 xff0c 或者用
  • 宏定义 宏参数

    带参数的宏定义 xff0c 利用宏参数创建字符串 运算符 看看以下两个宏定义 xff1a define PSQR x printf 34 The square of x is d n 34 x x define PSQR x printf
  • 舵机的PD控制

    PID 舵机以及差速PD调节 span class token keyword struct span span class token class name PID span span class token punctuation sp
  • 位置环与速度环的串级PID

    WHEELTEC的串级pid参考代码 span class token keyword float span Position KP span class token operator 61 span span class token nu
  • 智能车摄像头算法——圆环元素

    入环 1 入环的函数 xff08 1 xff09 搜上下边线 xff08 2 xff09 找凸起的弧 xff08 3 xff09 两点之间补线 xff08 4 xff09 判断上线是否单调 2 找圆环3 补线入环出环 1 入环的函数 xff
  • ROS的代价地图与AMCL定位原理

    地图服务与AMCL定位 costmap xff08 代价地图 xff09 AMCL定位 xff08 自适应蒙特卡罗定位 xff09 costmap xff08 代价地图 xff09 1 地图文件格式 xff1a 除了pgm xff08 便携
  • ROS路径规划算法

    ROS路径规划算法 全局路径规划Dijkstra算法A 算法 局部路径规划DWA算法TEB算法 全局路径规划 提供Dijkstra和A算法 xff0c 默认使用Dijkstra Dijkstra是把从出发点到终点的整个栅格地图上的所有的点
  • STM32常用功能配置

    STM32基本代码 设置外部中断定时器中断定时器产生pwmAD多通道转换DMA 43 AD扫描多通道转换iic协议读取数据SPI协议读取数据 设置外部中断 中断优先级分组 外部中断 AFIO作用 注意 xff1a 1 相同的Pin不能同时触
  • Ogre-渐变背景色(gradient background)的实现

    转载自 xff1a http blog csdn net hefee article details 6287341 背景色在ogre里面是通过ViewPort类中的setBackgroundColour xff08 xff09 这个成员函
  • Qt::WindowFlags

    查了些资料 xff0c 整理了一下 xff0c 以备查询 枚举类型 Qt WindowFlags低位的一个字节用于定义窗口部件的窗口类型 Qt WindowFlags的高位字节定义了窗口提示 xff0c 窗口提示能够进行位或操作 xff0c
  • java学习记录8

    什么是File 文件夹和文件 xff1a 文件夹是用来组织和管理磁盘文件的一种数据结构 文件是在电脑中 xff0c 以实现某种功能或某个软件的部分功能为目的定义的一个单位 xff0c 文件是由文件名和图标组成 xff0c 一种类型的文件具有
  • 保护模式编程之(一)——分段机制与GDT/LDT

    概述 xff1a 若想理解操作系统程序中的启动相关的部分 xff0c 必须要理解保护模式下的编程 xff0c 而分段机制是保护模式编程下的基础 另外 xff0c 由于实模式与保护模式的不同 xff0c 对保护模式下的分段机制更需要注意 同时
  • C++ 网络编程

    socket通信 xff1a socket 创建TCP套接字 bind 将套接字绑定到本地地址端口上 listen 监听端口 connect accept 接受用户请求 xff0c 返回对应此连接的新套接字 read write close
  • ROS学习(2)——rviz与gazebo问题记录

    ROS学习 xff08 2 xff09 rviz与gazebo问题记录 继续按照教程学习 xff0c 踩了很多坑 1 工作环境配置问题 实践6 2 4在rviz中显示模型时 xff0c 运行launch文件出现如下报错 原因 xff1a 出
  • VINS-Mono 代码详细解读——基础储备:在线Cam到IMU的外参标定 InitialEXRotation类

    本讲还是为了estimator类中最主要的函数processImage xff08 xff09 做知识储备 前面两讲知识储备主要讲了IMU预积分相关的integrationBase类以及图像特征点管理器feature manager cpp