# SLAM trajectory global error calculation

## 1. umeyama algorithm

After the SLAM result is output, we need to measure it to determine the accuracy of the positioning. We use the following high-precision optitrack equipment to collect GroundTruth data from the SLAM device, combined with the SLAM positioning data, we can use the umeyama algorithm to align the two trajectories, and then further evaluate the accuracy error.

We record the groundTruth trajectory as and the slam trajectory as the square root error for these two trajectories conforming to similar transformations:

First we calculate the mean:

Then we calculate the variance:

Covariance:

Decompose it by SVD, denoted as, and have:

The degree of freedom of similarity transformation can be expressed as:

The above is the principle of the umeyama algorithm for similar transformations. Readers who are interested in the proof of the lemma can read Shinji Umeyama's paper "Least-Squares Estimation of Transformation Parameters Between Two Point Patterns".

The code is as follows:

inline matrix<3> kabsch(const std::vector<vector<3>> &src, const std::vector<vector<3>> &dst) {
matrix<3> cov = matrix<3>::Zero();
for (size_t i = 0; i <src.size(); ++i) {
cov += src[i] * dst[i].transpose();
}
cov = cov * (1.0/src.size());
Eigen::JacobiSVD<matrix<3>> svd(cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
const matrix<3> &U = svd.matrixU();
const matrix<3> &V = svd.matrixV();
matrix<3> E = matrix<3>::Identity();
if ((V * U.transpose()).determinant() >= 0.0) {
E(2, 2) = 1.0;
} else {
E(2, 2) = -1.0;
}

return V * E * U.transpose();
}

inline std::tuple<double, quaternion, vector<3>> umeyama(std::vector<vector<3>> gt, std::vector<vector<3>> in, bool fix_scale = false) {
vector<3> gt_avg = vector<3>::Zero();
vector<3> in_avg = vector<3>::Zero();
for (size_t i = 0; i <gt.size(); ++i) {
gt_avg += gt[i];
in_avg += in[i];
}
gt_avg/= (double)gt.size();
in_avg/= (double)in.size();

double gt_d2 = 0;
double in_d2 = 0;
for (size_t i = 0; i <gt.size(); ++i) {
gt[i] -= gt_avg;
in[i] -= in_avg;
gt_d2 += gt[i].squaredNorm();
in_d2 += in[i].squaredNorm();
}

double S = sqrt(in_d2/gt_d2);
if (fix_scale) {
S = 1;
}
matrix<3> R = kabsch(in, gt);
vector<3> T = S * gt_avg-R * in_avg;

quaternion q;
q = R;

return {S, q, T};
}

## 2. the trajectory alignment result

We use the evo tool to process directly. After installation, we need to align through the following command line and calculate the rmse error. Here we have to pay attention to the fact that the two trajectories must be aligned before processing. Readers can choose hardware time alignment or software alignment (Bsplines calibration, please refer to the previous article).

evo_ape tum groundTruth.txt slam.txt -va --plot

To use the evo tool for alignment, all data needs to be written into a txt file in the format of the tum dataset. The format of the tum data set is [timestamp tx ty tz qx qy qz qw], -va means displaying detailed calculation results, and --plot means displaying the results graphically.

We align the collected groundTruth data with the slam data globally, and get the following trajectory error comparison chart, relative trajectory or individual trajectory readers can search and learn by themselves:

The gray trajectory is groundTruth.txt, and the color trajectory is slam.txt. It can be seen from the figure that the two trajectories are basically aligned, and the global pose error (APE) rmse error is about 5 cm. Readers of umeyama pose results and other error standards can be obtained from the interface.

SLAM calibration series articles

Reference: https://cloud.tencent.com/developer/article/1802605 SLAM trajectory global error calculation-Cloud + Community-Tencent Cloud