SLAM学习1三维刚体运动，实践

一、Eigen库配置

在Ubuntu中输入以下命令下载Eigen

sudo apt-get install libeigen3-dev

下载后可以在/usr/include/eigen3/下查看到Eigen库的头文件，也可以输入如下命令查看

sudo updatedb
locate eigen3

二.pangolin配置 

1、pangolin依赖及工具安装

基于Ubuntu18.04版本，在终端中输入以下命令

sudo apt update
sudo apt upgrade
sudo apt install libglew-dev cmake libboost-dev libboost-thread-dev libboost-filesystem-dev libeigen3-dev -y 

2、pangolin库源文件下载

在下载库源文件之前，我们按照视觉SLAM十四讲的源代码位置，通过下面的指令进入pangolin库文件夹，进行源文件的下载

cd slambook2/3rdparty/Pangolin
git clone https://github.com/stevenlovegrove/Pangolin.git #github地址
git clone https://gitee.com/krisnat/pangolin.git #gitee地址

3、pangolin库安装

先进入下载好的源文件目录，然后通过以下命令安装pangolin

cd slambook2/3rdparty/Pangolin/Pangolin
mkdir build && cd build
cmake ..
sudo make -j4
sudo make install

# -j后面的数字表示make过程使用的cpu线程数，数字越大，速度越快，可以根据自己的电脑配置情况进行调整

4、测试

运行ch3目录下examples中的示例程序

cd examples
mkdir build && cd build
cmake ..
make

三、实践

补充（以书本为例）：

1.下载书中的代码

git clone --recursive https://github.com/gaoxiang12/slambook.git

2.以第三讲中的ch3/useEigen为编译示例，可不用自带的cmakelist，尝试自己写一下，先将ch3/useEigen/eigenMatrix.cpp移到一个新建的文件夹try下,再编写一个对应的cmakelist.txt，可参考以下文件

cmake_minimum_required(VERSION 3.10)  # 指定 CMake 最低版本
project(useEigen)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-O3")
 
# 添加Eigen头文件
include_directories("/usr/include/eigen3")
add_executable(eigenMatrix eigenMatrix.cpp)

3.在当前文件夹下创建一个build文件

mkdir build

 4.先进入build文件再编译生成可执行文件

cmake ..

make

 5.可查看可执行文件后再执行(以我的为例)

ls
./eigenMatrix

1.坐标系转换

#include <iostream>
#include <Eigen/Dense>
#include <cmath>

int main() {
    // 旋转角度 (单位：弧度)
    double theta = M_PI / 4.0;  // 旋转45度

    // 构造绕Z轴的旋转矩阵
    Eigen::Matrix3d R;
    R << cos(theta), -sin(theta), 0,
         sin(theta), cos(theta),  0,
         0,            0,          1;
              
    // 定义平移向量
    Eigen::Vector3d translation(1, 2, 3);  // 平移量 (1, 2, 3)

    // 原始坐标
    Eigen::Vector3d p(1, 0, 0);

    // 构造齐次坐标变换矩阵 (4x4)
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();  // 单位矩阵
    T.block<3, 3>(0, 0) = R;                          // 填充旋转部分
    T.block<3, 1>(0, 3) = translation;                 // 填充平移部分

    // 将原始坐标扩展为齐次坐标 (4x1)
    Eigen::Vector4d p_homogeneous(p(0), p(1), p(2), 1.0);

    // 进行变换
    Eigen::Vector4d p_transformed = T * p_homogeneous;

    // 输出变换后的坐标
    std::cout << "变换后的坐标: " << p_transformed.transpose() << std::endl;

    return 0;
}

2.显示运动轨迹与相机位姿

#include <pangolin/pangolin.h>
#include <Eigen/Dense>
#include <vector>
#include <iostream>

using namespace std;
using namespace Eigen;

// 定义相机位姿结构体
struct CameraPose {
    Vector3d position;   // 相机的位置
    Matrix3d rotation;   // 相机的旋转矩阵
};

// 创建一个简单的旋转矩阵，绕Z轴旋转
Matrix3d getRotationMatrix(double angle) {
    Matrix3d rot = Matrix3d::Identity();
    rot(0, 0) = cos(angle);
    rot(0, 1) = -sin(angle);
    rot(1, 0) = sin(angle);
    rot(1, 1) = cos(angle);
    return rot;
}

// 绘制相机轨迹
void drawTrajectory(const vector<CameraPose>& poses) {
    glColor3f(1.0f, 0.0f, 0.0f);  // 设置轨迹颜色为红色
    glBegin(GL_LINE_STRIP);  // 使用线条绘制轨迹
    for (const auto& pose : poses) {
        glVertex3f(pose.position.x(), pose.position.y(), pose.position.z());
    }
    glEnd();
}

// 绘制相机位置
void drawCamera(const CameraPose& pose) {
    // 设置相机的位置和方向
    glPushMatrix();
    glTranslatef(pose.position.x(), pose.position.y(), pose.position.z());
    
    // 画一个小立方体来表示相机
    glColor3f(0.0f, 1.0f, 0.0f);  // 设置相机颜色为绿色
    glScalef(0.1f, 0.1f, 0.1f);  // 缩小立方体尺寸
    glutSolidCube(1.0);
    
    glPopMatrix();
}

int main() {
    // 设置相机的轨迹和位姿
    vector<CameraPose> poses;
    for (int i = 0; i < 10; ++i) {
        CameraPose pose;
        pose.position = Vector3d(i * 0.5, sin(i * 0.5), cos(i * 0.5));  // 模拟位置
        pose.rotation = getRotationMatrix(i * 0.1);  // 模拟旋转（绕Z轴旋转）
        poses.push_back(pose);
    }

    // 初始化Pangolin窗口
    pangolin::CreateWindowAndBind("Camera Trajectory Visualization", 1024, 768);
    glEnable(GL_DEPTH_TEST);

    // 设置投影矩阵和视图矩阵
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 512, 0.1, 1000), 
        pangolin::ModelViewLookAt(3.0, -3.0, 3.0, 0.0, 0.0, 0.0, pangolin::AxisY)
    );
    pangolin::Handler3D handler(s_cam);
    pangolin::View& d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, 0.0, 1.0, (float)1024 / (float)768)
        .SetHandler(&handler);

    // 进入显示循环
    while (!pangolin::ShouldQuit()) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        
        // 让相机移动
        s_cam.Activate(s_cam);

        // 绘制相机轨迹
        drawTrajectory(poses);

        // 绘制每个相机的位置
        for (const auto& pose : poses) {
            drawCamera(pose);
        }

        pangolin::FinishFrame();
        usleep(5000);  // 控制更新频率
    }

    return 0;
}