SLAM十四讲第五次作业-深蓝学院

其他 2019-03-11 14:11:20 阅读次数: 0

在这里插入图片描述

void computeAngle(const cv::Mat &image, vector<cv::KeyPoint> &keypoints) {
    int half_patch_size = 8;
    for(auto &kp : keypoints){
        // START YOUR CODE HERE (~7 lines)
        //judge if keypoint is on edge
        int x=cvRound(kp.pt.x);
        int y=cvRound(kp.pt.y);
        if( x-half_patch_size<0||x+half_patch_size>image.cols||
            y-half_patch_size<0||y+half_patch_size>image.rows)
            continue;  //结束当前循环，进入到下一次循环
        double m01=0,m10=0;   //定义变量的时候，要初始化，不然这里第一张图片所有kp.angle=0
        for(int i=-half_patch_size;i<half_patch_size;i++){    //-8<i<8,-8<j<8
            for(int j=-half_patch_size;j<half_patch_size;j++){
                m01 += j*image.at<uchar>(y+j,x+i);              //真实坐标(j,i)+(y,x)
                m10 += i*image.at<uchar>(y+j,x+i);              //获得单个像素值image.at<uchar>(y,x)
            }
        }
        kp.angle = atan(m01/m10)*180/pi;
        cout<<"m10 = "<<m01<<"   "<<"m01 = "<<m10<<"  "<<"kp.angle = "<<kp.angle<<endl;
        // END YOUR CODE HERE
    }
    return;
}

结果截图：
在这里插入图片描述
在这里插入图片描述

  • 为什么要有这一步？
    为了让描述子面对图像旋转时，仍然有效！
    先让p、q按照keypoint的角度进行旋转，使得图像旋转后，p与q的位置相对于keypoint不发生旋转，然后再进行像素大小比较，再进行keypoints匹配。
void computeORBDesc(const cv::Mat &image, vector<cv::KeyPoint> &keypoints, vector<DescType> &desc) {
    for (auto &kp: keypoints) {
        DescType d(256, false);
        for (int i = 0; i < 256; i++) {
            // START YOUR CODE HERE (~7 lines)
            auto cos_ = float(cos(kp.angle*pi/180)); //将角度转换成弧度再进行cos、sin的计算
            auto sin_ = float(sin(kp.angle*pi/180));
            //注意pattern中的数如何取
            cv::Point2f p_r(cos_*ORB_pattern[4*i]-sin_*ORB_pattern[4*i+1],
                    sin_*ORB_pattern[4*i]+cos_*ORB_pattern[4*i+1]);
            cv::Point2f q_r(cos_*ORB_pattern[4*i+2]-sin_*ORB_pattern[4*i+3],
                    sin_*ORB_pattern[4*i+2]+cos_*ORB_pattern[4*i+3]);

            cv::Point2f p(kp.pt+p_r); //获取p'与q'的真实坐标，才能获得其像素值
            cv::Point2f q(kp.pt+q_r);

            // if kp goes outside, set d.clear()
            if(p.x<0||p.y<0||p.x>image.cols||p.y>image.rows||
            q.x<0||q.y<0||q.x>image.cols||q.y>image.rows){
                d.clear();
                break;
            }
            //像素值比较
             d[i]=image.at<uchar>(p)>image.at<uchar>(q)?0:1; // 这里的“>”不可以替换成“-”，因为用“-”，结果是负数和正数都为真，是0的时候为假。
     // END YOUR CODE HERE
        }
        desc.push_back(d);
    }

    int bad = 0;
    for (auto &d: desc) {
        if (d.empty()) bad++;
    }
    cout << "bad/total: " << bad << "/" << desc.size() << endl;
    return;
}

在这里插入图片描述
在这里插入图片描述

void bfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches) {
    int d_max = 50;
    // START YOUR CODE HERE (~12 lines)
    // find matches between desc1 and desc2.
    for(int i=0;i<desc1.size();i++){
        if(desc1[i].empty())
            continue;
        int d_min=256 ,index=-1; //必须定义在这里，每次循环重新初始化
        for(int j=0;j<desc2.size();j++){ //这个for循环，取出最小的d_min
            if(desc2[j].empty())
                continue;
            int d=0; //必须定义在这里，每次循环重新初始化
            for(int k=0;k<256;k++){
                d += desc1[i][k]^desc2[j][k]; //异或：不同为1;
            }
            if(d<d_max){
                d_min=d;
                index=j;
            }
        }
        if(d_min<=d_max){
            cv::DMatch match(i,index,d_min);
            matches.push_back(match);
        }
    }
 // END YOUR CODE HERE

    for (auto &m: matches) {
        cout <<"queryIdx = "<< m.queryIdx << ", " << "trainIdx = "<<m.trainIdx << ", " << "distance = "<<m.distance << endl;
    }
    return;
}

结果截图：
在这里插入图片描述
在这里插入图片描述
答：
1.因为ORB的描述子采用的是0和1表示。
2.取更大有很多误匹配，取更小匹配的点较少。
3.0.27秒，FLANN可以减少时间。
在这里插入图片描述

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sophus/so3.h>
using namespace Eigen;
using namespace std;

int main(int argc, char **argv) {

    // 给定Essential矩阵
    Matrix3d E;
    E << -0.0203618550523477, -0.4007110038118445, -0.03324074249824097,
            0.3939270778216369, -0.03506401846698079, 0.5857110303721015,
            -0.006788487241438284, -0.5815434272915686, -0.01438258684486258;

    // 待计算的R,t
   Matrix3d R;
   Vector3d t;

    // SVD and fix singular values
    Eigen::JacobiSVD<Matrix3d> svd(E,ComputeFullU|ComputeFullV); //svd分解
    Vector3d sigma = svd.singularValues(); //svd分解出来的sigma是3×1的向量
    Matrix3d SIGMA; //将向量sigma调整成矩阵SIGMA
    SIGMA<<(sigma(0,0)+sigma(1,0))/2,0,0,
            0,(sigma(0,0)+sigma(1,0))/2,0,
            0,0,0;
    cout<<"SIGMA = \n"<<SIGMA<<endl;

    // set t1, t2, R1, R2 
   Matrix3d t_wedge1;
   Matrix3d t_wedge2;
   Matrix3d R1;
   Matrix3d R2;

   Matrix3d R_z1 = AngleAxisd(M_PI/2,Vector3d(0,0,1)).toRotationMatrix(); //定义旋转矩阵,沿 Z 轴旋转 90 度
   Matrix3d R_z2 = AngleAxisd(-M_PI/2,Vector3d(0,0,1)).toRotationMatrix(); //定义旋转矩阵沿 Z 轴旋转 -90 度
   Matrix3d U=svd.matrixU(); //u的值
   Matrix3d V=svd.matrixV(); //v的值

   t_wedge1=U*R_z1*SIGMA*U.transpose(); //t1的值
   t_wedge2=U*R_z2*SIGMA*U.transpose(); //t2的值
   R1=U*R_z1.transpose()*V.transpose();
   R2=U*R_z2.transpose()*V.transpose();

    cout << "R1 =\n" << R1 << endl;
    cout << "R2 =\n" << R2 << endl;
    cout << "t1 =\n" << Sophus::SO3::vee(t_wedge1) << endl;
    cout << "t2 =\n" << Sophus::SO3::vee(t_wedge2) << endl;

    // check t^R=E up to scale
    Matrix3d tR = t_wedge1 * R1;
    cout << "E = t^R =\n" << tR << endl;

    return 0;
}

结果截图：
在这里插入图片描述

在这里插入图片描述

//
// Created by xiang on 12/21/17.
//
#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <iomanip>
#include "sophus/se3.h"

using namespace std;
using namespace Eigen;

typedef vector<Vector3d, Eigen::aligned_allocator<Vector3d>> VecVector3d;
typedef vector<Vector2d, Eigen::aligned_allocator<Vector3d>> VecVector2d;
typedef Matrix<double, 6, 1> Vector6d;

string p3d_file = "./p3d.txt";
string p2d_file = "./p2d.txt";

int main(int argc, char **argv) {

    VecVector2d p2d;
    VecVector3d p3d;
    Matrix3d K;
    double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
    K << fx, 0, cx, 0, fy, cy, 0, 0, 1;

    // load points in to p3d and p2d 
    // START YOUR CODE HERE
    ifstream fin_p3d(p3d_file);
    ifstream fin_p2d(p2d_file);
    if(!fin_p2d.is_open() && !fin_p3d.is_open()){

        cout<<"txt file can not open ! "<<endl;
        return 1;
    }
    double data[3]={0};
    while(!fin_p3d.eof()){ //eof:end of file
        for(int i=0;i<3;i++)
        {
            fin_p3d>>data[i];
        }
        Vector3d item(data[0],data[1],data[2]);
        p3d.push_back(item);
    }
    while (!fin_p2d.eof()){
        for(int i=0;i<2;i++){
            fin_p2d>>data[i];
        }
        Vector2d item(data[0],data[1]);
        p2d.push_back(item);
    }
    // END YOUR CODE HERE
    assert(p3d.size() == p2d.size()); //assert:假定两个文件记录的相等

    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    Sophus::SE3 T_esti; // estimated pose
    for (int iter = 0; iter < iterations; iter++) {

        Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();
        Vector6d b = Vector6d::Zero();
        cost = 0;
        // compute cost
        for (int i = 0; i < nPoints; i++) {
            // compute cost for p3d[I] and p2d[I]
            // START YOUR CODE HERE
            Vector4d P_ = T_esti.matrix()*Vector4d(p3d[i](0,0),(p3d[i](1,0)),p3d[i](2,0),1);
            Vector3d u = K*Vector3d(P_(0,0),P_(1,0),P_(2,0));
            Vector2d e = p2d[i]-Vector2d(u(0,0)/u(2,0),u(1,0)/u(2,0)); //e = p2d - K*T*p3d
            cost += e.squaredNorm()/2; //cost即最小二乘的表达式 cost += e^2/2
           // END YOUR CODE HERE

     // compute jacobian,注意jacobian的负号
     //这里是T_esti是SE3的定义，不是se3的定义，无需将jacobin的前3列与后3列对调。
            Matrix<double, 2, 6> J;
            // START YOUR CODE HERE
            double x = P_(0,0);
            double y = P_(1,0);
            double z = P_(2,0);
            double x2=x*x;
            double y2=y*y;
            double z2=z*z;

            J(0,0) = fx/z;
            J(0,1) = 0;
            J(0,2) = -fx*x/z2;
            J(0,3) = -fx*x*y/z2;
            J(0,4) = fx+fx*x2/z2;
            J(0,5) = -fx*y/z;

            J(1,0) = 0;
            J(1,1) = fy/z;
            J(1,2) = -fy*y/z2;
            J(1,3) = -fy-fy*y2/z2;
            J(1,4) = fy*x*y/z2;
            J(1,5) = fy*x/z;
            J=-J; //jacobin的负号不要忘了，观测值-预测值

     // END YOUR CODE HERE

            H += J.transpose() * J;
            b += -J.transpose() * e;
        }

 // solve dx 
        Vector6d dx;

        // START YOUR CODE HERE 
        dx=H.ldlt().solve(b); //调用QR分解求解
        // END YOUR CODE HERE

        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= lastCost) {
            // cost increase, update is not good
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // update your estimation
        // START YOUR CODE HERE 
        T_esti = Sophus::SE3::exp(dx)*T_esti; //左乘更新
        // END YOUR CODE HERE
        
        lastCost = cost;

        cout << "iteration " << iter << " cost=" << cout.precision(12) << cost << endl;
    }

    cout << "estimated pose: \n" << T_esti.matrix() << endl;
    return 0;
}

结果截图：

在这里插入图片描述

//用ICP实现轨迹对齐
//把两条轨迹的平移部分看作点集,然后求点集之间的 ICP,得到两组点之间的变换T_eg

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <cmath>
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

using namespace std;
using namespace Eigen;
using namespace cv;
void ReadData(string FileName ,
              vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &poses_e,
              vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &poses_g,
              vector<Point3f> &t1,
              vector<Point3f> &t2
);
void icp_svd (
        const vector<Point3f>& pts1,
        const vector<Point3f>& pts2,
        Matrix3d & R,Vector3d& t);
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_e,
                    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_g);

/*******************************main函数*************************************/
int main(int argc,char **argv){
    string TrajectoryFile = "./compare.txt";
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_e;
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_g;
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_g_; //poses_g_=T*poses_g
    Eigen::Matrix3d R;
    Eigen::Vector3d t;
    vector<Point3f> t_e,t_g;

    ReadData( TrajectoryFile,poses_e, poses_g, t_e, t_g);
    icp_svd(t_e,t_g,R,t);
    Sophus::SE3 T_eg(R,t);
    for(auto SE3_g:poses_g){
        SE3_g =T_eg*SE3_g; // T_e[i]=T_eg*T_g[i]
        poses_g_.push_back(SE3_g);
    }
   DrawTrajectory(poses_e,poses_g_);
}
/*************读取文件中的位姿******************/
void ReadData(string FileName ,
        vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &poses_e,
        vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &poses_g,
        vector<Point3f> &t_e,
        vector<Point3f> &t_g
        ){
    string line;
    double time1,tx_1,ty_1,tz_1,qx_1,qy_1,qz_1,qw_1;
    double time2,tx_2,ty_2,tz_2,qx_2,qy_2,qz_2,qw_2;
    ifstream fin(FileName);
    if(!fin.is_open()){
        cout<<"compare.txt file can not open!"<<endl;
        return ;
    }
    while(getline(fin,line)){
        istringstream record(line);
        record>>time1 >> tx_1 >> ty_1 >> tz_1 >> qx_1 >> qy_1 >> qz_1 >> qw_1
              >>time2 >> tx_2 >> ty_2 >> tz_2 >> qx_2 >> qy_2 >> qz_2 >> qw_2;
        t_e.push_back(Point3d(tx_1,ty_1,tz_1)); //将t取出，为了进行用icp进行计算
        t_g.push_back(Point3d(tx_2,ty_2,tz_2));

        Eigen::Vector3d point_t1(tx_1, ty_1, tz_1);
        Eigen::Vector3d point_t2(tx_2, ty_2, tz_2);

        Eigen::Quaterniond q1 = Eigen::Quaterniond(qw_1, qx_1, qy_1, qz_1).normalized(); //四元数的顺序要注意
        Eigen::Quaterniond q2 = Eigen::Quaterniond(qw_2, qx_2, qy_2, qz_2).normalized();

        Sophus::SE3 SE3_qt1(q1, point_t1);
        Sophus::SE3 SE3_qt2(q2, point_t2);

        poses_e.push_back(SE3_qt1);
        poses_g.push_back(SE3_qt2);
    }

}

void icp_svd (
        const vector<Point3f>& pts1,
        const vector<Point3f>& pts2,
        Matrix3d& R, Vector3d& t) {
    Point3f p1, p2; // center of mass
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = Point3f( Vec3f(p1) / N);
    p2 = Point3f( Vec3f(p2) / N);
    vector<Point3f> q1 ( N ), q2 ( N ); // remove the center
    for ( int i=0; i<N; i++ )
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W="<<W<<endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;

    R = U* ( V.transpose() ); //p1=R_12*p_2,注意R的意义，p2到p1的旋转关系
    t = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R * Eigen::Vector3d ( p2.x, p2.y, p2.z );
}

/*****************************绘制轨迹*******************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_e,
                    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_g) {
    if (poses_g.empty() || poses_e.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768); //创建一个窗口
    glEnable(GL_DEPTH_TEST); //启动深度测试
    glEnable(GL_BLEND); //启动混合
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); //混合函数glBlendFunc( GLenum sfactor , GLenum dfactor );sfactor 源混合因子dfactor 目标混合因子

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0) //对应的是gluLookAt,摄像机位置,参考点位置,up vector(上向量)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));


    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < poses_e.size() - 1; i++) {
            glColor3f(1 - (float) i / poses_e.size(), 0.0f, (float) i / poses_e.size());
            glBegin(GL_LINES);
            auto p1 = poses_e[i], p2 = poses_e[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        for (size_t j = 0; j < poses_g.size() - 1; j++) {
            glColor3f(1 - (float) j / poses_g.size(), 0.0f, (float) j / poses_g.size());
            glBegin(GL_LINES);
            auto p1 = poses_g[j], p2 = poses_g[j + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        pangolin::FinishFrame();
    }

}

结果截图：
在这里插入图片描述

猜你喜欢

转载自blog.csdn.net/weixin_41074793/article/details/85133424
今日推荐
技术解析 GPT-4o：即时语音交互的突破与 GenAI 发展策略
开源大模型与闭源大模型
微信小程序授权登录获取用户的openid
亿级流量系统架构设计与实战
人工智能时代的程序设计教学与课程设计
纽交所技术问题致伯克希尔 (BRK.A) 显示跌近 100%
探索 api.maynor1024.live：一站式 AI 服务平台
AI一键去衣技术：窥见深度学习在图像处理领域的革命(最后有彩蛋)
艾体宝案例 | 使用Redis和Spring Ai构建rag应用程序
Apple M1 vs 高通8Gen2 vs Apple A12Z各方面比较
【升职加薪必备架构图】Springboot学习路线汇总_springboot四层架构流程图
与Apollo共创生态：Apollo7周年大会自动驾驶生态利剑出鞘
周排行
面试爱奇艺，竟然挂在第5轮……
scala方法和函数的区别
NYIST--2018大一新生第一次周赛题解
java如何通过client客戶端http实现get/ post请求传递json参数到restful 服务接口
RabbitMQ 队列类型
2018-2019-1 20165311 20165329 20165334 实验一 开发环境的熟悉
iOS打包工具 配置相应的文件路径 一键打包到指定位置
【每日一题】替换空格
【转载】FPGA配置方式
旅行青蛙
每日归档
更多
2024-06-03(52)
2024-06-02(4)
2024-06-01(60)
2024-05-31(47)
2024-05-30(4)
2024-05-29(65)
2024-05-28(2)
2024-05-27(56)
2024-05-26(6)
2024-05-25(68)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022