SLAM学习笔记

SLAM学习笔记

编译环境ubuntu20.04，vs code

先cmake文件

cmake_minimum_required(VERSION 2.8)
project(image)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++17")
find_package(OpenCV REQUIRED)
find_package( Pangolin REQUIRED)
find_package( Sophus REQUIRED)

include_directories("/usr/include/eigen3")
include_directories(${Pangolin_INCLUDE_DIRS})

add_executable(img image.cpp)
add_executable(distort undistort.cpp)
add_executable(stereo stereoVision.cpp)
add_executable(joint jointMap.cpp)



target_link_libraries(img ${OpenCV_LIBS})

target_link_libraries(distort ${OpenCV_LIBS})

target_link_libraries(stereo ${OpenCV_LIBS} ${Pangolin_LIBRARIES})

target_link_libraries(joint ${OpenCV_LIBS} ${Pangolin_LIBRARIES})
target_link_libraries(joint Sophus::Sophus)

然后是双目视觉

#include 
#include 
#include 
#include 
#include 
using namespace std;
using namespace Eigen;

string left_file = "/home/martin/桌面/code/image/left.png";
string right_file = "/home/martin/桌面/code/image/right.png";
void showPointCloud(const vector> &pointcloud);

int main(int argc,char** argv)
{
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;              //相机内参

    double b = 0.573;                                                             //基线宽度

    cv::Mat left = cv::imread(left_file , 0);                                     //读图
    cv::Mat right = cv::imread(right_file, 0);
    cv::Ptr sgbm = cv::StereoSGBM::create(                        
        0,96,9,8*9*9,32*9*9,1,63,10,100,2);
                                          
    cv::Mat disparity_sgbm,disparity;
    sgbm->compute(left,right,disparity_sgbm);                                     //计算、转换图像类型和画幅
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0/16.0f);

    vector> pointcloud;

    for (size_t i = 0; i < left.rows; i++)                                        //生成深度点云
    {
        for (size_t j = 0; j < left.cols; j++)
        {
            if(disparity.at(i,j) <= 0.0 || disparity.at(i,j) >= 96.0)//在宽高范围内获取像素值
            continue;

            Vector4d point (0,0,0, left.at(i,j) / 255.0);                  //三维坐标带个颜色

            double x = (j-cx)/fx;                                                 //书上的双目模型
            double y = (i-cy)/fy;
            double depth = fx * b /(disparity.at(i,j));
            point[0] = x*depth;
            point[1] = y*depth;
            point[2] = depth;

            pointcloud.push_back(point);
        }
        
    }
    
    cv::imshow("disparity",disparity / 96.0);
    cv::waitKey(0);

    showPointCloud(pointcloud);
    return 0;

}
//pangolin正常调用生成点云图像----------------------------------------------------------------------
void showPointCloud(const vector> &pointcloud)
{
    if (pointcloud.empty()) 
    {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    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)
    );

    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);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) 
        {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::Finishframe();
        usleep(5000);   
    }
    return;    
}

最后是点云地图

#include 
#include 
#include 
#include 
#include 
#include "sophus/se3.hpp"
using namespace std;
using namespace Eigen;

//声明同前两次一致
typedef Eigen::Matrix Vector6d;
typedef vector> TrajectoryType;
//显示点云的函数
void showPointCloud(const vector> &pointcloud);

int main (int argc,char** argv)
{
    vector colorImgs, depthImgs;                       //创建两个放置彩色图和深度图的容器
    TrajectoryType poses;                                       //相机姿态

    ifstream fin("/home/martin/桌面/code/image/pose.txt");       //我改成了pose的绝对路径
    if(!fin)
    {
        cerr<<"路径错误"<d;                               //c++11新的for遍历，还挺好用的
        Sophus::SE3d pose (Eigen::Quaterniond(data[6],data[3],data[4],data[5]),
                           Eigen::Vector3d(data[0],data[1],data[2]));
        poses.push_back(pose);
        
    }

    double cx = 325.5, cy = 253.5, fx = 518.0, fy = 519.0;      //相机内参
    double depthScale = 1000.0;                           
    vector> pointcloud;
    pointcloud.reserve(10000000);

    for (size_t i = 0; i < 5; i++)
    {
        cout<<"图象转换中： "<(j)[k];//读取深度
                if(d==0) continue;                              //深度为零就是没有测到
                Eigen::Vector3d point;
                point[2] = double(d) / depthScale;
                point[0] = (k-cx)*point[2]/fx;
                point[1] = (j-cy)*point[2]/fy;
                Eigen::Vector3d pointworld = T * point;

                Vector6d p;
                p.head<3>() = pointworld;
                p[5] = color.data[j * color.step + k * color.channels()];      //blue
                p[4] = color.data[j * color.step + k * color.channels()+1];    //green
                p[3] = color.data[j * color.step + k * color.channels()+2];    //red
                pointcloud.push_back(p);                                       //拼接
                
            }
            
        }
        
    }

    cout<<"点云共有"<> &pointcloud)
{
    if (pointcloud.empty()) 
    {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    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)
    );

    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);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) 
        {
            glColor3f(p[3]/255.0, p[4]/255.0, p[5]/255.0);   //这里两句
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::Finishframe();
        usleep(5000);   
    }
    return;    
}

c++东西确实多，还得继续学。