深蓝学院视觉SLAM第六次习题(下)

这篇博客介绍了视觉SLAM中直接法的应用,包括误差计算、雅可比矩阵和多层图像金字塔。还讨论了直接法与光流的比较,提出光流的inverse和compositional概念的无效性,并分析了直接法的加速策略。此外,博主通过计算光流来估算视差,并与给定图像进行比较,但遇到了不理想的结果,可能是代码错误导致的。
摘要由CSDN通过智能技术生成

2.直接法

误差为
在这里插入图片描述
雅可比矩阵为
在这里插入图片描述
首先要计算移动过的点的坐标,之后求解雅可比矩阵

void DirectPoseEstimationSingleLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3 &T21
) {
   

    // parameters
    int half_patch_size = 4;
    int iterations = 100;

    double cost = 0, lastCost = 0;
    int nGood = 0;  // good projections
    VecVector2d goodProjection;

    for (int iter = 0; iter < iterations; iter++) {
   
        nGood = 0;
        goodProjection.clear();

        // Define Hessian and bias
        Matrix6d H = Matrix6d::Zero();  // 6x6 Hessian
        Vector6d b = Vector6d::Zero();  // 6x1 bias

        for (size_t i = 0; i < px_ref.size(); i++) {
   

            // compute the projection in the second image
            // TODO START YOUR CODE HERE
	    double X_ref=depth_ref[i]*(px_ref[i][0]-cx)/fx;
	    double Y_ref=depth_ref[i]*(px_ref[i][1]-cy)/fy;
	    double Z_ref=depth_ref[i];
	    Matrix4d T_21=T21.matrix();
	    double X_cur=T_21(0,0)*X_ref+T_21(0,1)*Y_ref+T_21(0,2)*Z_ref+T_21(0,3);
	    double Y_cur=T_21(1,0)*X_ref+T_21(1,1)*Y_ref+T_21(1,2)*Z_ref+T_21(1,3);
	    double Z_cur=T_21(2,0)*X_ref+T_21(2,1)*Y_ref+T_21(2,2)*Z_ref+T_21(2,3);
	  
            float u =0, v = 0;
	    u=(
  • 0
    点赞
  • 12
    收藏
    觉得还不错? 一键收藏
  • 3
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值