0

SLAM的常用函数

 2 years ago
source link: https://charon-cheung.github.io/2022/09/18/SLAM%E5%B7%A5%E5%85%B7/SLAM%E7%9A%84%E5%B8%B8%E7%94%A8%E5%87%BD%E6%95%B0/#%E8%AE%A1%E7%AE%97%E6%95%B4%E4%B8%AA-pose-graph-%E7%9A%84%E8%AF%AF%E5%B7%AE
Go to the source link to view the article. You can view the picture content, updated content and better typesetting reading experience. If the link is broken, please click the button below to view the snapshot at that time.
neoserver,ios ssh client
SLAM的常用函数
2022-09-18|SLAM工具|
Word count: 193|Reading time: 1 min

位姿 —-> 转换矩阵

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

转换矩阵 —-> 位姿

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 的误差

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

report

Recommend

About Joyk


Aggregate valuable and interesting links.
Joyk means Joy of geeK