代码仓库

https://github.com/peitianyu/2d_slam_learning.git

简介

1
2
3
4
5
6
7
8
9
10
2维坐标变换相对简单,实际上主要可分为两部分,求解转换矩阵或者转换向量/求解转换后坐标
这里主要用两个函数表示
	- TransformFrom()
	- TransformAdd()
这里提供了两种思路
	- 根据公式直接代入,硬解: 对于2dslam,建议使用,比较简单
	- 齐次坐标使用Eigen库求解
	- 使用四元数更新(待添加)
位姿线性差分:
	- 对差分向量进行线性差分

测试

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
// 直接求解变换向量与变换后坐标
void TestDirectSolution()
{
    Eigen::Vector3f old_pose(1.0, 1.0, 1.0);
    Eigen::Vector3f new_pose(2.0, 2.0, 2.0);

    Eigen::Vector3f d_pose = Transform::TransformFrom(old_pose, new_pose); // 求解变换向量
    std::cout<<"d_pose: "<<d_pose.transpose()<<std::endl;

    std::cout<<"transformed pose: "<<Transform::TransformAdd(old_pose, d_pose).transpose()<<std::endl; // 求解变换后坐标
}

// 齐次坐标使用Eigen求解
void TestEigenSolution()
{
    Eigen::Vector3f old_pose(1.0, 1.0, 1.0);
    Eigen::Vector3f new_pose(2.0, 2.0, 2.0);

    Eigen::Matrix3f t_d_pose = Transform::TransformFromWithEigen(old_pose, new_pose); // 求解变换向量
    std::cout<<"t_d_pose: \n"<<t_d_pose<<std::endl;
    Eigen::Vector3f d_pose = Transform::T2V(t_d_pose);
    std::cout<<"d_pose: "<<d_pose.transpose()<<std::endl;

    std::cout<<"transformed pose: "<<Transform::TransformAddWithEigen(old_pose, t_d_pose).transpose()<<std::endl; // 求解变换后坐标
}

// 测试差分坐标
void TestDiffPose()
{
    Eigen::Vector3f old_pose(1.0, 1.0, 1.0);
    Eigen::Vector3f new_pose(2.0, 2.0, 2.0);

    Eigen::Vector3f d_pose = Transform::TransformFrom(old_pose, new_pose); 
    std::cout<<"d_pose: "<<d_pose.transpose()<<std::endl;

    std::cout<<"transformed pose: "<<Transform::TransformAdd(old_pose, d_pose/3.0).transpose()<<std::endl;

    std::cout<<"transformed pose: "<<Transform::TransformAdd(old_pose, 2.0*d_pose/3.0).transpose()<<std::endl;

    std::cout<<"transformed pose: "<<Transform::TransformAdd(old_pose, d_pose).transpose()<<std::endl;
}

// 测试差分坐标
void TestDiffPoseWithEigen()
{
    Eigen::Vector3f old_pose(1.0, 1.0, 1.0);
    Eigen::Vector3f new_pose(2.0, 2.0, 2.0);

    Eigen::Matrix3f t_d_pose = Transform::TransformFromWithEigen(old_pose, new_pose);
    std::cout<<"t_d_pose: \n"<<t_d_pose<<std::endl;
    Eigen::Vector3f d_pose = Transform::T2V(t_d_pose);
    std::cout<<"d_pose: "<<d_pose.transpose()<<std::endl;

    std::cout<<"transformed pose: "<<Transform::TransformAddWithEigen(old_pose, Transform::V2T(d_pose/3.0)).transpose()<<std::endl;

    std::cout<<"transformed pose: "<<Transform::TransformAddWithEigen(old_pose, Transform::V2T(2.0*d_pose/3.0)).transpose()<<std::endl;

    std::cout<<"transformed pose: "<<Transform::TransformAddWithEigen(old_pose, t_d_pose).transpose()<<std::endl;
}

结果

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
-------------TestDirectSolution--------------
d_pose:   1.38177 -0.301169         1
transformed pose: 2 2 2
-------------TestEigenSolution--------------
t_d_pose:
 0.540302 -0.841471   1.38177
 0.841471  0.540302 -0.301169
        0         0         1
d_pose:   1.38177 -0.301169         1
transformed pose: 2 2 2
-------------TestDiffPose--------------
d_pose:   1.38177 -0.301169         1
transformed pose: 1.33333 1.33333 1.33333
transformed pose: 1.66667 1.66667 1.66667
transformed pose: 2 2 2
-------------TestDiffPoseWithEigen--------------
t_d_pose:
 0.540302 -0.841471   1.38177
 0.841471  0.540302 -0.301169
        0         0         1
d_pose:   1.38177 -0.301169         1
transformed pose: 1.33333 1.33333 1.33333
transformed pose: 1.66667 1.66667 1.66667
transformed pose: 2 2 2