Merged in f5soh/librepilot/update_credits (pull request #529)

[librepilot.git] / ground / gcs / src / plugins / config / twostep.cpp

/**
 ******************************************************************************
 *
 * @file       twostep.cpp
 * @author     Jonathan Brandmeyer <jonathan.brandmeyer@gmail.com>
 *      Copyright (C) 2010.
 * @addtogroup GCSPlugins GCS Plugins
 * @{
 * @addtogroup Config plugin
 * @{
 * @brief Implements low-level calibration algorithms
 *****************************************************************************/
/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; Version 3 of the License, and no other
 * version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include <calibration.h>
#include <assertions.h>
#include <Eigen/Eigen2Support>

#include <cstdlib>

#undef PRINTF_DEBUGGING

#include <iostream>
#include <iomanip>

#if defined(__APPLE__) || defined(_WIN32)
// Qt bug work around
  #ifndef isnan
extern "C" int isnan(double);
  #endif
  #ifndef isinf
extern "C" int isinf(double);
  #endif
#endif

/*
 * Alas, this implementation is a fairly direct copy of the contents of the
 * following papers.  You don't have a chance at understanding it without
 * reading these first.  Any bugs should be presumed to be JB's fault rather
 * than Alonso and Shuster until proven otherwise.
 *
 * Reference [1]: "A New Algorithm for Attitude-independent magnetometer
 * calibration", Robert Alonso and Malcolmn D. Shuster, Flight Mechanics/
 * Estimation Theory Symposium, NASA Goddard, 1994, pp 513-527
 *
 * Reference [2]: Roberto Alonso and Malcolm D. Shuster, "Complete Linear
 * Attitude-Independent Magnetometer Calibration", Journal of the Astronautical
 * Sciences, Vol 50, No 4, 2002, pp 477-490
 *
 */

namespace {
Vector3f center(const Vector3f samples[],
                size_t n_samples)
{
    Vector3f summation(Vector3f::Zero());

    for (size_t i = 0; i < n_samples; ++i) {
        summation += samples[i];
    }
    return summation / float(n_samples);
}

void inv_fisher_information_matrix(Matrix3f & fisherInv,
                                   Matrix3f & fisher,
                                   const Vector3f samples[],
                                   size_t n_samples,
                                   const float noise)
{
    MatrixXf column_samples(3, n_samples);

    for (size_t i = 0; i < n_samples; ++i) {
        column_samples.col(i) = samples[i];
    }
    fisher = 4 / noise *
             column_samples * column_samples.transpose();
    // Compute the inverse by taking advantage of the results symmetricness
    fisher.ldlt().solve(Matrix3f::Identity(), &fisherInv);
}
// Eq 14 of the original reference.  Computes the gradiant of the cost function
// with respect to the bias offset.
// @param samples: The vector of measurement samples
// @param mags: The vector of sample delta magnitudes
// @param b: The estimate of the bias
// @param mu: The trace of the noise matrix (3*noise)
// @return the negative gradiant of the cost function with respect to the
// current value of the estimate, b
Vector3f neg_dJdb(const Vector3f samples[],
                  const float mags[],
                  size_t n_samples,
                  const Vector3f & b,
                  float mu,
                  float noise)
{
    Vector3f summation(Vector3f::Zero());
    float b_norm2 = b.squaredNorm();

    for (size_t i = 0; i < n_samples; ++i) {
        summation += (mags[i] - 2 * samples[i].dot(b) + b_norm2 - mu)
                     * 2 * (samples[i] - b);
    }

    return (1.0 / noise) * summation;
}
} // !namespace (anon)

Vector3f twostep_bias_only(const Vector3f samples[],
                           size_t n_samples,
                           const Vector3f & referenceField,
                           const float noise)
{
    // \tilde{H}
    Vector3f *centeredSamples = new Vector3f[n_samples];
    // z_k
    float sampleDeltaMag[n_samples];
    // eq 7 and 8 applied to samples
    Vector3f avg = center(samples, n_samples);
    float refSquaredNorm = referenceField.squaredNorm();
    float sampleDeltaMagCenter = 0;

    for (size_t i = 0; i < n_samples; ++i) {
        // eq 9 applied to samples
        centeredSamples[i]    = samples[i] - avg;
        // eqn 2a
        sampleDeltaMag[i]     = samples[i].squaredNorm() - refSquaredNorm;
        sampleDeltaMagCenter += sampleDeltaMag[i];
    }
    sampleDeltaMagCenter /= n_samples;

    Matrix3f P_bb;
    Matrix3f P_bb_inv;
    // Due to eq 12b
    inv_fisher_information_matrix(P_bb, P_bb_inv, centeredSamples, n_samples, noise);
    // Compute centered magnitudes
    float sampleDeltaMagCentered[n_samples];
    for (size_t i = 0; i < n_samples; ++i) {
        sampleDeltaMagCentered[i] = sampleDeltaMag[i] - sampleDeltaMagCenter;
    }

    // From eq 12a
    Vector3f estimate(Vector3f::Zero());
    for (size_t i = 0; i < n_samples; ++i) {
        estimate += sampleDeltaMagCentered[i] * centeredSamples[i];
    }
    estimate = P_bb * ((2 / noise) * estimate);

    // Newton-Raphson gradient descent to the optimal solution
    // eq 14a and 14b
    float mu = -3 * noise;
    for (int i = 0; i < 6; ++i) {
        Vector3f neg_gradiant = neg_dJdb(samples, sampleDeltaMag, n_samples,
                                         estimate, mu, noise);
        Matrix3f scale = P_bb_inv + 4 / noise * (avg - estimate) * (avg - estimate).transpose();
        Vector3f neg_increment;
        scale.ldlt().solve(neg_gradiant, &neg_increment);
        // Note that the negative has been done twice
        estimate += neg_increment;
    }
    delete[] centeredSamples;
    return estimate;
}

namespace {
// Private utility functions for the version of TWOSTEP that computes bias and
// scale factor vectors alone.

/** Compute the gradiant of the norm of b with respect to the estimate vector.
 */
Matrix<double, 6, 1>
dnormb_dtheta(const Matrix<double, 6, 1> & theta)
{
    return (Matrix<double, 6, 1>() << 2 * theta.coeff(0) / (1 + theta.coeff(3)),
            2 * theta.coeff(1) / (1 + theta.coeff(4)),
            2 * theta.coeff(2) / (1 + theta.coeff(5)),
            -pow(theta.coeff(0), 2) / pow(1 + theta.coeff(3), 2),
            -pow(theta.coeff(1), 2) / pow(1 + theta.coeff(4), 2),
            -pow(theta.coeff(2), 2) / pow(1 + theta.coeff(5), 2)).finished();
}

/**
 * Compute the gradiant of the cost function with respect to the optimal
 * estimate.
 */
Matrix<double, 6, 1>
dJdb(const Matrix<double, 6, 1> & centerL,
     double centerMag,
     const Matrix<double, 6, 1> & theta,
     const Matrix<double, 6, 1> & dbdtheta,
     double mu,
     double noise)
{
    // \frac{\delta}{\delta \theta'} |b(\theta')|^2
    Matrix<double, 6, 1> vectorPart = dbdtheta - centerL;

    // By equation 35
    double normbthetaSquared = 0;
    for (unsigned i = 0; i < 3; ++i) {
        normbthetaSquared += theta.coeff(i) * theta.coeff(i) / (1 + theta.coeff(3 + i));
    }
    double scalarPart = (centerMag - centerL.dot(theta) + normbthetaSquared
                         - mu) / noise;
    return scalarPart * vectorPart;
}

/** Reconstruct the scale factor and bias offset vectors from the transformed
 * estimate vector.
 */
Matrix<double, 1, 6>
theta_to_sane(const Matrix<double, 6, 1> & theta)
{
    return (Matrix<double, 6, 1>() <<
            theta.coeff(0) / sqrt(1 + theta.coeff(3)),
            theta.coeff(1) / sqrt(1 + theta.coeff(4)),
            theta.coeff(2) / sqrt(1 + theta.coeff(5)),
            -1 + sqrt(1 + theta.coeff(3)),
            -1 + sqrt(1 + theta.coeff(4)),
            -1 + sqrt(1 + theta.coeff(5))).finished();
}
} // !namespace (anon)

/**
 * Compute the scale factor and bias associated with a vector observer
 * according to the equation:
 * B_k = (I_{3x3} + D)^{-1} \times (O^T A_k H_k + b + \epsilon_k)
 * where k is the sample index,
 *       B_k is the kth measurement
 *       I_{3x3} is a 3x3 identity matrix
 *       D is a 3x3 diagonal matrix of scale factors
 *       O is the orthogonal alignment matrix
 *       A_k is the attitude at the kth sample
 *       b is the bias in the global reference frame
 *       \epsilon_k is the noise at the kth sample
 * This implementation makes the assumption that \epsilon is a constant white,
 * gaussian noise source that is common to all k.  The misalignment matrix O
 * is not computed, and the off-diagonal elements of D, corresponding to the
 * misalignment scale factors are not either.
 *
 * @param bias[out] The computed bias, in the global frame
 * @param scale[out] The computed scale factor, in the sensor frame
 * @param samples[in] An array of measurement samples.
 * @param n_samples The number of samples in the array.
 * @param referenceField The magnetic field vector at this location.
 * @param noise The one-sigma squared standard deviation of the observed noise
 * in the sensor.
 */
void twostep_bias_scale(Vector3f & bias,
                        Vector3f & scale,
                        const Vector3f samples[],
                        const size_t n_samples,
                        const Vector3f & referenceField,
                        const float noise)
{
    // Initial estimate for gradiant descent starts at eq 37a of ref 2.

    // Define L_k by eq 30 and 28 for k = 1 .. n_samples
    Matrix<double, Dynamic, 6> fullSamples(n_samples, 6);
    // \hbar{L} by eq 33, simplified by obesrving that the
    Matrix<double, 1, 6> centerSample = Matrix<double, 1, 6>::Zero();
    // Define the sample differences z_k by eq 23 a)
    double sampleDeltaMag[n_samples];
    // The center value \hbar{z}
    double sampleDeltaMagCenter = 0;
    double refSquaredNorm = referenceField.squaredNorm();

    for (size_t i = 0; i < n_samples; ++i) {
        fullSamples.row(i) << 2 * samples[i].transpose().cast<double>(),
            -(samples[i][0] * samples[i][0]),
            -(samples[i][1] * samples[i][1]),
            -(samples[i][2] * samples[i][2]);
        centerSample += fullSamples.row(i);

        sampleDeltaMag[i]     = samples[i].squaredNorm() - refSquaredNorm;
        sampleDeltaMagCenter += sampleDeltaMag[i];
    }
    sampleDeltaMagCenter /= n_samples;
    centerSample /= n_samples;

    // True for all k.
    // double mu = -3*noise;
    // The center value of mu, \bar{mu}
    // double centerMu = mu;
    // The center value of mu, \tilde{mu}
    // double centeredMu = 0;

    // Define \tilde{L}_k for k = 0 .. n_samples
    Matrix<double, Dynamic, 6> centeredSamples(n_samples, 6);
    // Define \tilde{z}_k for k = 0 .. n_samples
    double centeredMags[n_samples];
    // Compute the term under the summation of eq 37a
    Matrix<double, 6, 1> estimateSummation = Matrix<double, 6, 1>::Zero();
    for (size_t i = 0; i < n_samples; ++i) {
        centeredSamples.row(i) = fullSamples.row(i) - centerSample;
        centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter;
        estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose();
    }
    estimateSummation /= noise; // note: paper supplies 1/noise

    // By eq 37 b).  Note, paper supplies 1/noise here
    Matrix<double, 6, 6> P_theta_theta_inv = (1.0f / noise) *
                                             centeredSamples.transpose() * centeredSamples;
#ifdef PRINTF_DEBUGGING
    SelfAdjointEigenSolver<Matrix<double, 6, 6> > eig(P_theta_theta_inv);
    std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n";
    std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose()
              << "\n";
    std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n";
#endif

    // The Fisher information matrix has a small eigenvalue, with a
    // corresponding eigenvector of about [1e-2 1e-2 1e-2 0.55, 0.55,
    // 0.55].  This means that there is relatively little information
    // about the common gain on the scale factor, which has a
    // corresponding effect on the bias, too. The fixup is performed by
    // the first iteration of the second stage of TWOSTEP, as documented in
    // [1].
    Matrix<double, 6, 1> estimate;
    // By eq 37 a), \tilde{Fisher}^-1
    P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate);

#ifdef PRINTF_DEBUGGING
    Matrix<double, 6, 6> P_theta_theta;
    P_theta_theta_inv.ldlt().solve(Matrix<double, 6, 6>::Identity(), &P_theta_theta);
    SelfAdjointEigenSolver<Matrix<double, 6, 6> > eig2(P_theta_theta);
    std::cout << "eigenvalues: " << eig2.eigenvalues().transpose() << "\n";
    std::cout << "eigenvectors: \n" << eig2.eigenvectors() << "\n";
    std::cout << "estimate summation: \n" << estimateSummation.normalized()
              << "\n";
#endif

    // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradiant(theta)
    // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1}
    size_t count = 3;
    while (count-- > 0) {
        // eq 40
        Matrix<double, 1, 6> db_dtheta = dnormb_dtheta(estimate);

        Matrix<double, 6, 1> dJ_dtheta = dJdb(centerSample,
                                              sampleDeltaMagCenter,
                                              estimate,
                                              db_dtheta,
                                              -3 * noise,
                                              noise / n_samples);


        // Eq 39 (with double-negation on term inside parens)
        db_dtheta = centerSample - db_dtheta;
        Matrix<double, 6, 6> scale = P_theta_theta_inv +
                                     (double(n_samples) / noise) * db_dtheta.transpose() * db_dtheta;

        // eq 14b, mutatis mutandis (grumble, grumble)
        Matrix<double, 6, 1> increment;
        scale.ldlt().solve(dJ_dtheta, &increment);
        estimate -= increment;
    }

    // Transform the estimated parameters from [c | e] back into [b | d].
    for (size_t i = 0; i < 3; ++i) {
        scale.coeffRef(i) = -1 + sqrt(1 + estimate.coeff(3 + i));
        bias.coeffRef(i)  = estimate.coeff(i) / sqrt(1 + estimate.coeff(3 + i));
    }
}

namespace {
// Private functions specific to the scale factor and orthogonal calibration
// version of TWOSTEP

/**
 * Reconstruct the symmetric E matrix from the last 6 elements of the estimate
 * vector
 */
Matrix3d E_theta(const Matrix<double, 9, 1> & theta)
{
    // By equation 49b
    return (Matrix3d() <<
            theta.coeff(3), theta.coeff(6), theta.coeff(7),
            theta.coeff(6), theta.coeff(4), theta.coeff(8),
            theta.coeff(7), theta.coeff(8), theta.coeff(5)).finished();
}

/**
 * Compute the gradient of the squared norm of b with respect to theta.  Note
 * that b itself is just a function of theta.  Therefore, this function
 * computes \frac{\delta,\delta\theta'}\left|b(\theta')\right|
 * From eq 55 of [2].
 */
Matrix<double, 9, 1>
dnormb_dtheta(const Matrix<double, 9, 1> & theta)
{
    // Re-form the matrix E from the vector of theta'
    Matrix3d E = E_theta(theta);
    // Compute (I + E)^-1 * c
    Vector3d IE_inv_c;

    E.ldlt().solve(theta.end<3>(), &IE_inv_c);

    return (Matrix<double, 9, 1>() << 2 * IE_inv_c,
            -IE_inv_c.coeff(0) * IE_inv_c.coeff(0),
            -IE_inv_c.coeff(1) * IE_inv_c.coeff(1),
            -IE_inv_c.coeff(2) * IE_inv_c.coeff(2),

            -2 * IE_inv_c.coeff(0) * IE_inv_c.coeff(1),
            -2 * IE_inv_c.coeff(0) * IE_inv_c.coeff(2),
            -2 * IE_inv_c.coeff(1) * IE_inv_c.coeff(2)).finished();
}

// The gradient of the cost function with respect to theta, at a particular
// value of theta.
// @param centerL: The center of the samples theta'
// @param centerMag: The center of the magnitude data
// @param theta: The estimate of the bias and scale factor
// @return the gradient of the cost function with respect to the
// current value of the estimate, theta'
Matrix<double, 9, 1>
dJ_dtheta(const Matrix<double, 9, 1> & centerL,
          double centerMag,
          const Matrix<double, 9, 1> & theta,
          const Matrix<double, 9, 1> & dbdtheta,
          double mu,
          double noise)
{
    // \frac{\delta}{\delta \theta'} |b(\theta')|^2
    Matrix<double, 9, 1> vectorPart = dbdtheta - centerL;

    // |b(\theta')|^2
    Matrix3d E = E_theta(theta);
    Vector3d rhs;
    (Matrix3d::Identity() + E).ldlt().solve(theta.start<3>(), &rhs);
    double normbthetaSquared = theta.start<3>().dot(rhs);

    double scalarPart = (centerMag - centerL.dot(theta) + normbthetaSquared
                         - mu) / noise;
    return scalarPart * vectorPart;
}
} // !namespace (anonymous)

/**
 * Compute the scale factor and bias associated with a vector observer
 * according to the equation:
 * B_k = (I_{3x3} + D)^{-1} \times (O^T A_k H_k + b + \epsilon_k)
 * where k is the sample index,
 *       B_k is the kth measurement
 *       I_{3x3} is a 3x3 identity matrix
 *       D is a 3x3 symmetric matrix of scale factors
 *       O is the orthogonal alignment matrix
 *       A_k is the attitude at the kth sample
 *       b is the bias in the global reference frame
 *       \epsilon_k is the noise at the kth sample
 *
 * After computing the scale factor and bias matrices, the optimal estimate is
 * given by
 * \tilde{B}_k = (I_{3x3} + D)B_k - b
 *
 * This implementation makes the assumption that \epsilon is a constant white,
 * gaussian noise source that is common to all k.  The misalignment matrix O
 * is not computed.
 *
 * @param bias[out] The computed bias, in the global frame
 * @param scale[out] The computed scale factor matrix, in the sensor frame.
 * @param samples[in] An array of measurement samples.
 * @param n_samples The number of samples in the array.
 * @param referenceField The magnetic field vector at this location.
 * @param noise The one-sigma squared standard deviation of the observed noise
 * in the sensor.
 */
void twostep_bias_scale(Vector3f & bias,
                        Matrix3f & scale,
                        const Vector3f samples[],
                        const size_t n_samples,
                        const Vector3f & referenceField,
                        const float noise)
{
    // Define L_k by eq 51 for k = 1 .. n_samples
    Matrix<double, Dynamic, 9> fullSamples(n_samples, 9);
    // \hbar{L} by eq 52, simplified by observing that the common noise term
    // makes this a simple average.
    Matrix<double, 1, 9> centerSample = Matrix<double, 1, 9>::Zero();
    // Define the sample differences z_k by eq 23 a)
    double sampleDeltaMag[n_samples];
    // The center value \hbar{z}
    double sampleDeltaMagCenter = 0;
    // The squared norm of the reference vector
    double refSquaredNorm = referenceField.squaredNorm();

    for (size_t i = 0; i < n_samples; ++i) {
        fullSamples.row(i) << 2 * samples[i].transpose().cast<double>(),
            -(samples[i][0] * samples[i][0]),
            -(samples[i][1] * samples[i][1]),
            -(samples[i][2] * samples[i][2]),
            -2 * (samples[i][0] * samples[i][1]),
            -2 * (samples[i][0] * samples[i][2]),
            -2 * (samples[i][1] * samples[i][2]);

        centerSample += fullSamples.row(i);

        sampleDeltaMag[i]     = samples[i].squaredNorm() - refSquaredNorm;
        sampleDeltaMagCenter += sampleDeltaMag[i];
    }
    sampleDeltaMagCenter /= n_samples;
    centerSample /= n_samples;

    // Define \tilde{L}_k for k = 0 .. n_samples
    Matrix<double, Dynamic, 9> centeredSamples(n_samples, 9);
    // Define \tilde{z}_k for k = 0 .. n_samples
    double centeredMags[n_samples];
    // Compute the term under the summation of eq 57a
    Matrix<double, 9, 1> estimateSummation = Matrix<double, 9, 1>::Zero();
    for (size_t i = 0; i < n_samples; ++i) {
        centeredSamples.row(i) = fullSamples.row(i) - centerSample;
        centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter;
        estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose();
    }
    estimateSummation /= noise;

    // By eq 57b
    Matrix<double, 9, 9> P_theta_theta_inv = (1.0f / noise) *
                                             centeredSamples.transpose() * centeredSamples;

#ifdef PRINTF_DEBUGGING
    SelfAdjointEigenSolver<Matrix<double, 9, 9> > eig(P_theta_theta_inv);
    std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n";
    std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose()
              << "\n";
    std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n";
#endif

    // The current value of the estimate.  Initialized to \tilde{\theta}^*
    Matrix<double, 9, 1> estimate;
    // By eq 57a
    P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate);

    // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradient(theta)
    // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1}
    size_t count = 0;
    double eta   = 10000;
    while (count++ < 200 && eta > 1e-8) {
        static bool warned = false;
        if (hasNaN(estimate)) {
            std::cout << "WARNING: found NaN at time " << count << "\n";
            warned = true;
        }
#if 0
        SelfAdjointEigenSolver<Matrix3d> eig_E(E_theta(estimate));
        Vector3d S = eig_E.eigenvalues();
        Vector3d W; W << -1 + sqrt(1 + S.coeff(0)),
            -1 + sqrt(1 + S.coeff(1)),
            -1 + sqrt(1 + S.coeff(2));
        scale = (eig_E.eigenvectors() * W.asDiagonal() *
                 eig_E.eigenvectors().transpose()).cast<float>();

        (Matrix3f::Identity() + scale).ldlt().solve(
            estimate.start<3>().cast<float>(), &bias);
        std::cout << "\n\nestimated bias: " << bias.transpose()
                  << "\nestimated scale:\n" << scale;
#endif

        Matrix<double, 1, 9> db_dtheta = dnormb_dtheta(estimate);

        Matrix<double, 9, 1> dJ_dtheta = ::dJ_dtheta(centerSample,
                                                     sampleDeltaMagCenter,
                                                     estimate,
                                                     db_dtheta,
                                                     -3 * noise,
                                                     noise / n_samples);

        // Eq 59, with reused storage on db_dtheta
        db_dtheta = centerSample - db_dtheta;
        Matrix<double, 9, 9> scale = P_theta_theta_inv +
                                     (double(n_samples) / noise) * db_dtheta.transpose() * db_dtheta;

        // eq 14b, mutatis mutandis (grumble, grumble)
        Matrix<double, 9, 1> increment;
        scale.ldlt().solve(dJ_dtheta, &increment);
        estimate -= increment;
        eta = increment.dot(scale * increment);
        std::cout << "eta: " << eta << "\n";
    }
    std::cout << "terminated at eta = " << eta
              << " after " << count << " iterations\n";

    if (!isnan(eta) && !isinf(eta)) {
        // Transform the estimated parameters from [c | E] back into [b | D].
        // See eq 63-65
        SelfAdjointEigenSolver<Matrix3d> eig_E(E_theta(estimate));
        Vector3d S = eig_E.eigenvalues();
        Vector3d W; W << -1 + sqrt(1 + S.coeff(0)),
            -1 + sqrt(1 + S.coeff(1)),
            -1 + sqrt(1 + S.coeff(2));
        scale = (eig_E.eigenvectors() * W.asDiagonal() *
                 eig_E.eigenvectors().transpose()).cast<float>();

        (Matrix3f::Identity() + scale).ldlt().solve(
            estimate.start<3>().cast<float>(), &bias);
    } else {
        // return nonsense data.  The eigensolver can fall ingo
        // an infinite loop otherwise.
        // TODO: Add error code return
        scale = Matrix3f::Ones() * std::numeric_limits<float>::quiet_NaN();
        bias  = Vector3f::Zero();
    }
}