Tips for implementing the math

The code uses the Eigen library for performing matrix and vector operations necessary for calculating the vectors and covariance matrix. Refer to the quick reference guide to find methods for various operations.

Operations on the pose and error vector

End-effector poses and measurement errors in code are stored as a vector with 7 components. The first three components store XYZ positions and the remaining four store the four components of the orientation quaternion.

\[\begin{split}\begin{bmatrix} x \\ y \\ z \\ q_x \\ q_y \\ q_z \\ q_w \end{bmatrix}\end{split}\]

double x, y, z, q_x, q_y, q_z, q_w;

Eigen::Vector<double, 7> vector {x, y, z, q_x, q_y, q_z, q_w};

Vector math like additions, subtractions, multiplication/division with scalar values can be done directly using normal C++ operators like +, *, /.

The library supports element-wise operations on Eigen::Array objects but not on Eigen::Vector types. To perform element-wise operations on Eigen::Vector objects, you can use the array() method to obtain an Eigen::Array wrapper to apply operations on. The entire list of available operations can be found here.

For example, performing an element-wise square operation on a vector using the square() method looks like this:

\[\begin{split}\begin{bmatrix} x^2 \\ y^2 \\ z^2 \\ {q_x}^2 \\ {q_y}^2 \\ {q_z}^2 \\ {q_w}^2 \end{bmatrix}\end{split}\]

auto vector_squared = Eigen::Vector<double, 7>(vector.array().square());

The returned value from square() method is not an Eigen::Vector object. It either needs to be casted back to that type or used as a parameter for initializing a new object of that type.

For finding the transpose of the vector, simply use the transpose() method.

\[\begin{split}\begin{bmatrix} x \\ y \\ z \\ q_x \\ q_y \\ q_z \\ q_w \end{bmatrix}^T\end{split}\]

auto vector_transpose = vector.transpose();

Using the list of errors from measurements

When capturing the measurements for verification, the errors between the estimated end-effector poses (measured by the camera after calibration) and the reference poses (from robot’s forward kinematics) are already calculated for you and are stored in a list as an std::vector object. The size of this vector is the number of measurements taken.

\[errors = \{ e_1, e_2, e_3, \ ... \ e_n \}\]

std::vector<Eigen::Vector<double, 7>> error_vecs;

auto number_of_measurements = error_vecs.size();

A single error vector when indexed from this list would be:

\[\begin{split}e_i = \begin{bmatrix} x_i \\ y_i \\ z_i \\ {q_x}_i \\ {q_y}_i \\ {q_z}_i \\ {q_w}_i \end{bmatrix}\end{split}\]

auto e_i = error_vecs.at(i);

For accessing all the error vectors from the list, a simple range-based for loop will suffice.

for (const auto& e_i: error_vecs) {

    // Do something with e_i

}

Formulas for reference

Sample Mean

\[\bar{e} = \frac{1}{n} \sum_{i=1}^{n} e_i\]

\(n\) is the number of measurements.

Sample Covariance

\[S = \frac{1}{n - 1} \sum_{i=1}^{n} (e_i - \bar{e}) (e_i - \bar{e})^T\]

\(e_i - \bar{e}\) is the deviation of an error from sample mean error.

Sum of Squared Errors

\[\begin{split}SSE = \sum_{i=1}^{n} \begin{bmatrix} x_i^2 \\ y_i^2 \\ z_i^2 \\ {q_x}_i^2 \\ {q_y}_i^2 \\ {q_z}_i^2 \\ {q_w}_i^2 \end{bmatrix}\end{split}\]

The square operation on the error vector \(e_i\) is element-wise.