alignment-calibration.cpp

Go to the documentation of this file.

#include "calibration.h"
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>

using namespace Eigen;

void calibration_misalignment(Vector3f &rotationVector, const Vector3f samples0[],
                              const Vector3f &reference0, const Vector3f samples1[],
                              const Vector3f &reference1, size_t n_samples)
{
    // Note that this implementation makes the assumption that the angular
    // misalignment is small.  Something based on QUEST would be needed to
    // account for errors larger than a few degrees.
    Matrix<double, Dynamic, 3> X(n_samples, 3);
    Matrix<double, Dynamic, 1> y(n_samples, 1);

    AngleAxisd reference(
        Quaterniond().setFromTwoVectors(reference0.cast<double>(), reference1.cast<double>()));
    for (size_t i = 0; i < n_samples; ++i) {
        AngleAxisd observation(Quaterniond().setFromTwoVectors(samples0[i].cast<double>(),
                                                               samples1[i].cast<double>()));

        X.row(i) = observation.axis();
        y[i] = reference.angle() - observation.angle();
    }

    // Run linear least squares over the result.
    Vector3d result;
    (X.transpose() * X).ldlt().solve(X.transpose() * y, &result);
    rotationVector = result.cast<float>();
}