自动驾驶：基于光流法的相机初始标定

已于 2024-05-14 19:15:12 修改
阅读量1k 收藏 15
点赞数 8
文章标签： 学习
于 2023-12-04 15:48:28 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/dueen1123/article/details/134784988
版权

手眼标定

机器人手眼标定Ax=xB（eye to hand和eye in hand）及平面九点法标定
Ax = xB问题求解，旋转和平移分步求解法
手眼标定AX=XB求解方法（文献总结）

基于靶的方法

  1. 相机标定
    (1) ApriTag
    (2) 棋盘格：cv::findChessboardCorners
    (3) 品字格
  2. 激光雷达标定
    (1) 激光雷达反射板

动态标定

这里面的动态标定指车辆行使时广角前视相机相对路面的位姿估计

1.基于路面稀疏光流的Homography标定

前提：利用H进行优化，特征点需要位于同一个平面上，例如都是路面点的采样

首先利用opencv::calcOpticalFlowPyrLK提取出连续两帧之间的稀疏光流
假设相机在两个不同位置拍摄同一个平面（路面）
在这里插入图片描述

frame1中的点可以由以下公式转换到frame2中
在这里插入图片描述
X1表示三维坐标点，在平面P上，因此X1沿着平面法向量n的投影距离为d
在这里插入图片描述
进一步推导
在这里插入图片描述
结合公式1我们可以得到
在这里插入图片描述
因此我们就得到了平面的单应性矩阵
在这里插入图片描述
因此x2= Hx1，其中x1和x2分别是上一帧和当前帧相机坐标系下的点(x,y,1)，我们可以根据x2 - Hx1构建重投影误差来优化H
那么如何由H分解出来相机与车辆的标定矩阵呢？
H是相机之间的变化Rt_cam求得的（本质上优化H也就是优化Rt_cam），然后根据车辆坐标系的移动Rt_vcs，可以计算出相机到车辆的标定Rt，如下所示
在这里插入图片描述
括号里面可以理解为第一帧车辆系下两个相机之间的平移，前面乘以Rcv是把车辆坐标系转到相机坐标系

//前提：优化的变量直接是rpyxyz_v_c，可以直接分解出来rpy
ceres::CostFunction *cost_function = HomographyReprojectionError::Create(curr_pt, last_pt, R_vv, t_vv, K);
problem.AddResidualBlock(cost_function, loss_func, rpy_v_c.data(), xyz_v_c.data());

class HomographyReprojectionError {
 public:
  HomographyReprojectionError(const cv::Point2f &p1, const cv::Point2f &p2, const Eigen::Matrix3d &R_vv, const Eigen::Vector3d &t_vv, const Eigen::Matrix3d &K)
   : x1_(p1.x), y1_(p1.y), x2_(p2.x), y2_(p2.y), R_vv_(R), t_vv_(t_vv), K_(K){}

  static ceres::CostFunction *Create(const cv::Point2f &p1, const cv::Point2f &p2, const Eigen::Matrix3d &R_vv, const Eigen::Vector3d &t_vv, const Eigen::Matrix3d &K) 
  {
    return (new ceres::AutoDiffCostFunction<HomographyReprojectionError, 2, 6>(new HomographyReprojectionError(p1, p2, R_vv, t_vv, K)));
  }

  template <typename T>
  bool operator()(const T *const rpy_v_c, const T *const xyz_v_c, T *residuals) const {
    // get extrinsics
    Eigen::Matrix<T, 3, 1> rpy_data(rpy_v_c[0], rpy_v_c[1], rpy_v_c[2]);
    Eigen::Matrix<T, 3, 3, Eigen::RowMajor> R_v_c = Intr_XYZ2R(rpy_data);
    Eigen::Matrix<T, 3, 1> t_v_c = t_v_c_.template cast<T>();
    Eigen::Matrix<T, 3, 1> xyz_data(xyz_v_c[0], xyz_v_c[1], xyz_v_c[2]);
    t_v_c += xyz_data;

    // camera relative pose
    Eigen::Matrix<T, 3, 3, Eigen::RowMajor> R_cc = R_v_c.transpose() * R_vv * R_v_c;
    Eigen::Matrix<T, 3, 1> t_cc = R_v_c.transpose() * (R_vv * t_v_c + t_vv - t_v_c);

    // homography matrix
    T d = T(-1.0 * t_v_c(2));
    Eigen::Matrix<T, 3, 1> n_vcs(T(0), T(0), T(1));
    Eigen::Matrix<T, 3, 1> n_cam = R_v_c.transpose() * n_vcs;
    Eigen::Matrix<T, 3, 3, Eigen::RowMajor> H = K * (R_cc + t_cc * n_cam.transpose() / d) * K_inv;

    // H reprojection error
    Eigen::Matrix<T, 3, 1> p1(T(x1_), T(y1_), T(1));
    Eigen::Matrix<T, 3, 1> p1_proj = H * p1;
    p1_proj /= p1_proj(2);
    residuals[0] = ceres::abs(T(x2_) - p1_proj(0));
    residuals[1] = ceres::abs(T(y2_) - p1_proj(1));
    
    return true;
  }

 private:
  const double x1_, y1_, x2_, y2_;
  Eigen::Matrix3d R_vv_, K_;
  Eigen::Vector3d t_vv_;
};

2.基于非路面光流的VO估计yaw/pitch

前提：利用空间采样点利用E优化vo（利用E的优化不限制光流在同一个平面），然后通过vo分解出pitch和yaw

在这里插入图片描述
首先通过光流构建前后帧特征点之间的匹配关系，然后构建极线约束求解Essential Matrix，分解E得到位移方向t的初值
在这里插入图片描述
用tcc和Rcc构建本征矩阵E，然后求sampson残差
在这里插入图片描述

//该残差构建需要已知的rpy_vo, xyz_vo, 他们由R_v_v和需要标定的R_v_c构造
Eigen::Matrix3f R_v_c = last_calib_Rvc;
Eigen::Matrix3f R_c_c = R_v_c.transpose() * R_v_v * R_v_c;
Eigen::Vector3f t_c_c = R_v_c.transpose() * (R_v_v * t_v_c + t_v_v - t_v_c);
Eigen::Vector3d rpy_vo = Intr_R2XYZ(R_c_c).cast<double>();
Eigen::Vector3d xyz_vo = t_c_c.cast<double>();

ceres::CostFunction *cost_function = RelativePoseCostFunctionRPY::Create(pt1, pt2);
problem.AddResidualBlock(cost_function, loss_func, rpy_vo.data(), xyz_vo.data());
                                                
class RelativePoseCostFunctionRPY {
 public:
  RelativePoseCostFunctionRPY(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2)
      : x1_(p1(0)), y1_(p1(1)), x2_(p2(0)), y2_(p2(1)) {}

  static ceres::CostFunction* Create(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2) 
  {
    return ( new ceres::AutoDiffCostFunction<RelativePoseCostFunctionRPY, 1, 3, 3>( new RelativePoseCostFunctionRPY(p1, p2)));
  }

  template <typename T>
  bool operator()(const T* const rpy, const T* const xyz_vo, T* residuals) const {
    Eigen::Matrix<T, 3, 1> rpy_data(rpy_vo[0], rpy_vo[1], rpy_vo[2]);
    Eigen::Matrix<T, 3, 3, Eigen::RowMajor> R = Intr_XYZ2R(rpy_data);
    // Matrix representation of the cross product t x R.
    Eigen::Matrix<T, 3, 3> t_x;
    t_x << T(0), -xyz_vo[2], xyz_vo[1], xyz_vo[2], T(0), -xyz_vo[0], -xyz_vo[1], xyz_vo[0],
        T(0);
    // Essential matrix.
    const Eigen::Matrix<T, 3, 3> E = t_x * R;
    // Homogeneous image coordinates.
    const Eigen::Matrix<T, 3, 1> p1_h(T(x1_), T(y1_), T(1));
    const Eigen::Matrix<T, 3, 1> p2_h(T(x2_), T(y2_), T(1));
    // Squared sampson error.
    const Eigen::Matrix<T, 3, 1> Ep1 = E * p1_h;
    const Eigen::Matrix<T, 3, 1> Etp2 = E.transpose() * p2_h;
    const T p2tEp1 = p2_h.transpose() * Ep1;
    residuals[0] = p2tEp1 * p2tEp1 /
                   (Ep1(0) * Ep1(0) + Ep1(1) * Ep1(1) + Etp2(0) * Etp2(0) +
                    Etp2(1) * Etp2(1));
    return true;
  }

 private:
  const double x1_, y1_, x2_, y2_;
};

利用最优化方法得到更精确的t的值，多个匹配特征点，tx和R构建sampson error优化，然后求解yaw和pitch
在这里插入图片描述
在这里插入图片描述

double x = xyz_vo(0), y = xyz_vo(1), z = xyz_vo(2);
double curr_pitch = -1.0 * std::atan2(-z, std::sqrt(x * x + y * y));
double curr_yaw = -1.0 * std::atan2(y, x);

3.基于非路面光流的直接估计rpy

这个方法是最普通的方法，直接在前后两帧中提前特征点进行光流跟踪，然后剔除外点。
首先已知车辆坐标系下的移动R_vv和t_vv，最后直接利用光流跟踪值优化R_v_c提取出相机标定参数rpy

//前提：直接估计rpy_v_c
cost_function = EpipolarErrorWithPriorMotion::Create(curr_points[i], last_points[i], R_vv, t_vv);
problem.AddResidualBlock(cost_function, loss_func, rpy_v_c.data(), xyz_v_c.data());

class EpipolarErrorWithPriorMotion {
 public:
  EpipolarErrorWithPriorMotion(const cv::Point2f &p1, const cv::Point2f &p2, const Eigen::Matrix3d &R_vv, const Eigen::Vector3d &t_vv)
      : x1_(p1.x), y1_(p1.y), x2_(p2.x), y2_(p2.y), R__vv_(R_vv), t_vv_(t_vv) {}

  static ceres::CostFunction *Create(const cv::Point2f &p1, const cv::Point2f &p2, const Eigen::Matrix3d &R, const Eigen::Vector3d &t) 
  {
    return ( new ceres::AutoDiffCostFunction<EpipolarErrorWithPriorMotion, 1, 3, 3>(new EpipolarErrorWithPriorMotion(p1, p2, R, t, sleep_us)));
  }

  template <typename T>
  bool operator()(const T *const rpy_v_c, const T *const xyz_v_c, T *residuals) const {
    // get extrinsic params
    Eigen::Matrix<T, 3, 1> rpy_data(rpy_v_c[0], rpy_v_c[1], rpy_v_c[2]);
    Eigen::Matrix<T, 3, 3, Eigen::RowMajor> R_v_c = Intr_XYZ2R(rpy_data);
    Eigen::Matrix<T, 3, 1> t_v_c(xyz_v_c[0], xyz_v_c[1], xyz_v_c[2]);

    // camera relative pose
    Eigen::Matrix<T, 3, 3, Eigen::RowMajor> R_cc = R_v_c.transpose() * R_vv * R_v_c;
    Eigen::Matrix<T, 3, 1> t_cc = R_v_c.transpose() * (R_vv * t_v_c + t_vv - t_v_c);

    // epipolar error
    Eigen::Matrix<T, 3, 3> t_x;
    t_x << T(0), -t_cc[2], t_cc[1], t_cc[2], T(0), -t_cc[0], -t_cc[1], t_cc[0], T(0);
    const Eigen::Matrix<T, 3, 3, Eigen::RowMajor> E = t_x * R_cc;

    // Homogeneous image coordinates.
    const Eigen::Matrix<T, 3, 1> p1_h(T(x1_), T(y1_), T(1));
    const Eigen::Matrix<T, 3, 1> p2_h(T(x2_), T(y2_), T(1));

    // Squared sampson error.
    const Eigen::Matrix<T, 3, 1> Ep1 = E * p1_h;
    const Eigen::Matrix<T, 3, 1> Etp2 = E.transpose() * p2_h;
    const T p2tEp1 = p2_h.transpose() * Ep1;
    residuals[0] = p2tEp1 * p2tEp1 /
                   (Ep1(0) * Ep1(0) + Ep1(1) * Ep1(1) + Etp2(0) * Etp2(0) +
                    Etp2(1) * Etp2(1));

    return true;
  }

 private
  const double x1_, y1_, x2_, y2_;
  Eigen::Matrix3d R_vv_;
  Eigen::Vector3d t_vv_;
};
dueen1123
关注
  • 8
    点赞
  • 踩
  • 15
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
基于双目视觉的自动驾驶技术
3D视觉工坊
03-04 2575
点击上方“3D视觉工坊”，选择“星标”干货第一时间送达本文由知乎作者黄浴授权转载，不得擅自二次转载。原文链接：https://zhuanlan.zhihu.com/p/109906574...
摄像头标定
qq_43506113的博客
11-07 666
        首先由标定板的N张图片的亚像素级角点检测可以得出相机内参矩阵和畸变参数： 步骤：拍摄多张标定板图片，然后提取亚像素级角点信息，并对应标定板的实际尺寸设置每个角点的像素坐标，由两个函数得出相机内参矩阵相机畸变参数， 相u坐标转化为像素坐标：[u,v,1]=K(内参)*[X,Y,Z]。 世界坐标系转化为相机坐标系：[X,Y,Z]=[R,T]*[Xw,Yw,Zw]。 经过张正友标...
相机标定原理和步骤
adas_l5的博客
07-24 1003
传统相机标定法需要使用尺寸已知的标定物，通过建立标定物上坐标已知的点与其图像点之间的对应，利用一定的算法获得相机模型的内外参数。摄像机标定过程，简单的可以简单的描述为通过标定板，如下图，可以得到n个对应的世界坐标三维点Xi和对应的图像坐标二维点xi，这些三维点到二维点的转换都可以通过上面提到的相机内参K ，相机外参 R 和t，以及畸变参数 D ，经过一系列的矩阵变换得到。（基础矩阵中分解得到的R、T是前后两帧相机的之间的位姿变化，即以相机第一帧时的位姿为初始值，计算得到的相机第二帧时的旋转、平移量。
imu与lidar外参手眼标定
xinxiangwangzhi_的博客
02-07 963
激光雷达手眼标定
基于SLAM的双目立体视觉实时自标定
JinLn的博客
04-21 4491
SLAM-Based Self-Calibration of a Binocular Stereo Vision Rig in Real-Time1.介绍2.基于SLAM的自校准流程3. 基于SLAM的三维场景点图构建3.1 单目视觉SLAM系统3.2 单目相机姿态跟踪3.3 构建3D点云地图4.外参校准4.1 匹配特征点4.2 初始外参估计4.3 联合优化4.4 验证校准结果5.实验5.1 实验...
自动驾驶——估计预瞄轨迹YawRate
qq_40464599的博客
09-04 1500
x + a2 * x^2 + a3 * x^3 ，那么现在有个需求，希望根据上述x和y的关系，去估计规划预瞄轨迹yawRate。首先CD = x，AC = y ,BD = AB = R,CD与BD相互垂直 ，AC与CD垂直。角BDA = 角BAD =90 – angle，故有角B = 2 * angle。又由于angle = arcsin(y/(x^ 2+y^ 2)^(0.5))
使用单目相机前后帧特征点匹配进行3D深度估计的方法
u014374826的博客
05-15 1066
使用单目相机前后帧特征点匹配进行3D深度估计的方法
手眼标定_全面细致的推导过程
小白学视觉
04-11 2305
点击上方“小白学视觉”，选择加"星标"或“置顶”重磅干货，第一时间送达 本文转自| 新机器视觉本文解决的问题：机械手搭载双目相机，手眼标定。本文有细致的推导过程，非常全...
VINS系统：自动驾驶的导航之星
07-13
- **图像和IMU预处理**：提取图像特征点，并使用光流法进行跟踪；同时对IMU数据进行预积分处理 。 - **初始化**：利用图像序列和IMU数据进行尺度、重力向量和速度的初始化 。 - **后端滑动窗口优化**：基于滑动窗口...
MATLAB相机标定结果在图像处理和计算机视觉中的应用：探索标定结果的价值
相机标定是确定相机内参和外参的过程，对于图像处理和计算机视觉应用至关重要。MATLAB提供了丰富的工具箱和函数，用于执行相机标定任务。 在MATLAB中，相机标定通常涉及以下步骤： - **获取图像：**使用相机拍摄一...
面向自动驾驶的定位方法综述
惊鸿一博
10-30 3943
摘要：定位作为自动驾驶中最基本的环节之一，目的在于明确车辆相对于全局的绝对位置或相对于交通参与者的相对位置，其准确性直接影响了自动驾驶系统的安全性。当前，自动驾驶技术正在对我们的出行产生越来越深远的影响，但是在复杂的真实道路环境中，面向自动驾驶的定位方法还不够成熟。本文对已有的定位方法进行了全面而系统的梳理，首先针对单个定位方式逐类别地进行了归纳，包含基于通信的、基于航位推算的、和基于特征匹配的方式三种类别，并重点分析对比了各种定位方式的基本原理和优缺点；然后调研和讨论了现阶段典型的融合定位方式；最后对当.
自动驾驶系统的传感器标定方法
CV_Autobot的博客
09-09 2384
作者|黄浴 编辑|汽车人原文链接：https://zhuanlan.zhihu.com/p/57028341点击下方卡片，关注“自动驾驶之心”公众号ADAS巨卷干货，即可获取点击进入→自动驾驶之心技术交流群后台回复【领域综述】获取自动驾驶全栈近80篇综述论文！传感器标定是自动驾驶的基本需求，一个车上装了多个/多种传感器，而它们之间的坐标关系是需要确定的。湾区自动驾驶创业公司ZooX的co...
【学习笔记】卫星通信NTN 3GPP标准化进展分析（六）- 参考标准
u011376987的博客
09-01 1155
本文总结了NTN标准化进程以及后续的研究计划，是学习NTN协议的入门。本文来自3GPP Joern Krause, 3GPP MCC。
java重点学习-spring
qq_40453972的博客
09-03 863
AOP称为面向切面编程，用于将那些与业务无关，但却对多个对象产生影响的公共行为和逻辑，抽取并封装为一个可重用的模块，这个模块被命名为“切面”(Aspect)，减少系统中的重复代码，降低了模块间的耦合度，同时提高了系统的可维护性。常见的AOP使用场景记录操作日志、●缓存处理Spring中内置的事务处理什么是AOP面向切面编程，用于将那些与业务无关，但却对多个对象产生影响的公共行为和逻辑，抽取公共模块复用，降低耦合你们项目中有没有使用到AOP。
iomuxc、pinctrl子系统、gpio子系统（学习总结）
最新发布
weixin_61385844的博客
09-04 923
IOMUXC是SOC设计中实现引脚复用的关键组件，它通过动态切换引脚功能，提高了IO资源的利用率，满足了不同外设和功能的IO需求。在配置和使用IOMUXC时，需要参考芯片使用手册和设备树文档，确保引脚功能的正确设置。
学习项目1
m0_62803606的博客
09-03 1607
这篇文章主要记录你最近入手的无线串口模块的配置情况，这是个很好的习惯，尤其是在你以后需要重复操作或排查问题时，会非常有帮助。- **DL_20无线串口模块**：这是一个常见的无线串口通信模块，通常基于nRF24L01+等无线通信芯片，支持点对点或多点通信，适合短距离的无线数据传输。是的，如果你按照步骤成功配置了两个模块并将它们连接到单机和电脑，当你在上位机软件（如SSCOM）上能稳定地接收到从单片机发送的数据（例如每秒输出的T值），就说明两个无线模块之间的通信已经配置成功。
假期学习-- iOS runtime的简单了解
m0_74217598的博客
09-04 955
（Runtime字典转模型，数据的接受取决于模型的属性）首先需要了解，属性定义在类里面，那么类里面就有一个属性列表，属性列表以数组的形式存在，根据属性列表就可以获得类里面的所有属性名，所以遍历属性列表，也就可以遍历模型中的所有属性名。所以RunTime字典转模型过程就很清晰了。
网络编程学习：TCP/IP协议
kkbbaaii的博客
09-01 1740
网络编程学习：TCP/IP协议
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值