沉默杀手HomeArchivesTagsCategories
SLAM的常用函数
|SLAM工具|
Word count: 193|Reading time: 1 min

位姿 —-> 转换矩阵

1
2
3
4
5
6
7
8
9
10
Eigen::Matrix3d PoseToTrans(Eigen::Vector3d x)
{
    std::cout << x << std::endl;
    Eigen::Matrix3d trans;
    trans << cos(x(2)), -sin(x(2)), x(0),
        	 sin(x(2)), cos(x(2)), x(1),
        		 0,         0, 	    1;

    return trans;
}

转换矩阵 —-> 位姿

1
2
3
4
5
6
7
8
9
Eigen::Vector3d TransToPose(Eigen::Matrix3d trans)
{
    Eigen::Vector3d pose;
    pose(0) = trans(0, 2);
    pose(1) = trans(1, 2);
    pose(2) = atan2(trans(1, 0), trans(0, 0));

    return pose;
}

计算整个 pose-graph 的误差

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
double ComputeError(std::vector<Eigen::Vector3d>& Vertexs,
    std::vector<Edge>& Edges)
{
    double sumError = 0;
    for (int i = 0; i < Edges.size(); i++) {
        Edge tmpEdge = Edges[i];
        Eigen::Vector3d xi = Vertexs[tmpEdge.xi];
        Eigen::Vector3d xj = Vertexs[tmpEdge.xj];
        Eigen::Vector3d z  = tmpEdge.measurement;
        Eigen::Matrix3d infoMatrix = tmpEdge.infoMatrix;

        Eigen::Matrix3d Xi = PoseToTrans(xi);
        Eigen::Matrix3d Xj = PoseToTrans(xj);
        Eigen::Matrix3d Z = PoseToTrans(z);

        Eigen::Matrix3d Ei = Z.inverse() * Xi.inverse() * Xj;

        Eigen::Vector3d ei = TransToPose(Ei);

        sumError += ei.transpose() * infoMatrix * ei;
    }
    return sumError;
}