视觉十四讲第三讲深蓝学院习题分析

最新推荐文章于 2022-05-10 12:37:14 发布

吃饲料的憨胖子

最新推荐文章于 2022-05-10 12:37:14 发布

阅读量983

点赞数

分类专栏：十四讲文章标签： slam

本文链接：https://blog.csdn.net/qq_33638170/article/details/108659478

版权

十四讲专栏收录该内容

1 篇文章 0 订阅

订阅专栏

已经学过一两遍这本书了，但都是囫囵吞枣，学的很差，现在重新返回学习，注重基础，哪里不会补哪里。希望和大家一起进步，不留疑惑。

群的性质

1.~~Z + 是群，群的定义为只有一种运算的集合，该群为整数集，加法。~~
2.~~N + 是群，群的定义为只有一种运算的集合，该群为自然数集，加法。~~

更正：判断是否为群不能只从定义来看，要利用他的几条性质“凤姐幺你”
1.对于Z+，是群
封闭性： z1+z2属于z；
结合律： (z1+z2)+z3=z1+(z2+z3)
幺元：a0=0, a+a0=a
逆： a+a逆=a0,注意这里的逆，不是矩阵那个取逆。是

对于一个数：n，n和其加法逆元(或称相反数)之和是加法单位（即零）。
对于n加法逆元表示为-n。
例：7的加法逆元是-7。-0.3的加法逆元是0.3。
若“+”符合结合律（(x+y)+z=x+(y+z)），则加法逆元的唯一的。（反证法：设x有相异的加法逆元x,x’：x=x+0=x+(x+x’)=(x+x)+x’=0+x’=x’）

『-1，1』在普通乘法下是群。
证：
1)封闭性：1×1=1 (-1)×(-1)=1 (-1)×1=-1 1×(-1)=-1
2)结合律：成立
3)单位元：1
4)逆元素：1的逆元是1，-1的逆元是-1

2.则自然数集除了0都没有负数，也就没有逆元素。

验证向量叉乘的李代数性质

~~美整明白要干啥~~
实际是因为李括号的结果等于叉乘，我们利用叉乘的性质来反推这个李括号是符合李代数的性质。

这里是关于叉乘的一些性质。
https://baike.baidu.com/item/%E5%8A%A0%E6%B3%95%E9%80%86%E5%85%83/1580329?fr=aladdin

这个是关于叉乘的性质
向量内积的性质：

a^2 ≥ 0；当a^2 = 0时，必有a = 0. （正定性）
a·b = b·a. （对称性）
(λa + μb)·c = λa·c + μb·c，对任意实数λ, μ成立. （线性）
cos∠(a,b) =a·b/(|a||b|).
|a·b| ≤ |a||b|，等号只在a与b共线时成立.

其中会用到双叉积拉格朗日公式：
https://blog.csdn.net/meteor_033/article/details/54848970
最后的证明如下：
在这里插入图片描述

推导 SE(3) 的指数映射
在这里插入图片描述

5 伴随
伴随这个要从后往前推，根据高博给的提示，计算，注意此时的R旋转矩阵为正交矩阵。
在这里插入图片描述

6.轨迹描绘和轨迹误差

cmake_minimum_required(VERSION 3.14)
project(thirdHw)

set(CMAKE_CXX_STANDARD 14)

find_package(Eigen3  REQUIRED)
find_package(Pangolin REQUIRED)
find_package( Sophus REQUIRED )
set(Sophus_LIBRARIES /usr/local/lib/libSophus.so)

include_directories(
        ${EIGEN3_INCLUDE_DIR}
        ${Pangolin_INCLUDE_DIRS}
        ${Sophus_INCLUDE_DIRS})

add_executable(thirdHw main.cpp)
target_link_libraries(thirdHw

        ${EIGEN3_LIBS}
        ${Pangolin_LIBRARIES}
        ${Sophus_LIBRARIES}

        )

#include <iostream>

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <boost/timer.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <vector>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
#define NDEBUG
#include <assert.h>
using namespace std;

// path to trajectory file
string trajectory_file = "/home/han/Documents/homework/SLAM第三讲作业资料/L3/code/groundtruth.txt";
string trajectory_file1 = "/home/han/Documents/homework/SLAM第三讲作业资料/L3/code/estimated.txt";
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses,curposes;
double data[7];
// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);
void ReadFile(string trajectory_file,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &poses);

int main(int argc, char **argv) {
//标准的定义容器方法,aligned_allocator管理C++中的各种数据类型的内存方法是一样的，
//可以不需要着重写出来。但是在Eigen管理内存和C++11中的方法是不一样的，
// 所以需要单独强调元素的内存分配和管理。

    /// implement pose reading code
    // start your code here (5~10 lines)
    //read file,save to vector poses
    ReadFile(trajectory_file,poses);
    ReadFile(trajectory_file1,curposes);
    //assert宏的原型定义在<assert.h>中，其作用是如果它的条件返回错误，则终止程序执行，
    assert(!poses.empty() && !curposes.empty());
    assert(poses.size() == curposes.size());
    // end your code here
    double rmse=0;
    int i=0;
    for(i;i<min(poses.size(),curposes.size());i++)
    {
        //向量的平方范数由squaredNorm()获得，等价于向量对自身做点积，也等同于所有元素额平方和。
        // Eigen也提供了norm()范数，返回的是squaredNorm()的根。这些操作也适用于矩阵。
        // 如果想使用其他元素级的范数，使用lpNorm<p>()方法，
        // 当求无穷范数时，模板参数p可以取特殊值Infinity，得到的是所有元素的最大绝对值。
        //参考：https://blog.csdn.net/u012936940/article/details/79842944
        double e = (poses[i].inverse()*curposes[i]).log().norm();
        rmse+=pow(e,2);
    }
    rmse=sqrt(rmse/poses.size());
    cout << "RMSE = " << rmse << endl;
    // draw trajectory in pangolin
    //how to draw in a window?
    DrawTrajectory(poses,curposes);

    return 0;
}
void ReadFile(string trajectory_file,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &poses)
{
    ifstream fin(trajectory_file);
    if(!fin)
        cout<<"no have files";
    while(!fin.eof())
    {
        double timestemp;
        fin>>timestemp;

        for(double & d:data)
            fin>>d;
        poses.push_back(
                Sophus::SE3( Eigen::Quaterniond(data[6], data[3], data[4], data[5]),
                             Eigen:: Vector3d(data[0], data[1], data[2])));

    }

}

效果图：
黑色的是curpose 出来的线条
在这里插入图片描述

疑问，
上题中给的公式中e 含有V，反对称矩阵，为什么代码中没有体现？
Twc的平移部分直接就是轨迹，为什么计算中还需要旋转部分？

通过代码，学到的C+知识。
1.C+中log,与EIgen中log函数不同，这里用的eigen
C+
引入#include
以e为底：log(n)
以10为底：log10(n)
以m为底：log(n)/log(m)
2.迭代器
for (std::vector::iterator iter = ivec.begin(); iter != ivec.end(); ++iter)
iter = 0; // set element to which iter refers to 0
print_int_vector(ivec);
参考链接：https://blog.csdn.net/zhanh1218/article/details/33340959
3.C++返回vector/将vector作为参数传递
/
输入一个数，返回这个数后面的十个数字序列
注意参数的这个 & 符号不能省略
*/
void getSequence(int num,vector& sequence){
for(int i=0;i<10;i++){
sequence.push_back(i+num);
}
}
参考：https://blog.csdn.net/neverever01/article/details/80744148
4,norm 函数
此处用的L2泛数
参考：https://blog.csdn.net/a493823882/article/details/80569888（理论）
https://blog.csdn.net/u012936940/article/details/79842944（EIgen使用）