// MIT License
//
// Copyright (c) 2021 Aditya Shridhar Hegde
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "mean_shift.hpp"

#include <NTL/ZZ.h>

#include <algorithm>
#include <numeric>
#include <random>
#include <stdexcept>

MeanShift::MeanShift(Scheme &scheme, long logn, long logq, long logq_boot,
                     long logp, long logt)
    : scheme(scheme),
      logn(logn),
      logq(logq),
      logq_boot(logq_boot),
      logp(logp),
      logt(logt) {
  slots = 1 << logn;
  refreshSeed();
}

MeanShift::MeanShift(Scheme &scheme, long logn, long logq, long logq_boot,
                     long logp, long logt, long seed)
    : scheme(scheme),
      logn(logn),
      logq(logq),
      logq_boot(logq_boot),
      logp(logp),
      logt(logt),
      seed(seed) {
  slots = 1 << logn;
}

void MeanShift::refreshSeed(long s) { seed = s; }

void MeanShift::refreshSeed() { seed = std::rand(); }

void MeanShift::leftRotateAndEqual(Ciphertext &x, long r) {
  while (r & (-r)) {
    scheme.leftRotateFastAndEqual(x, r & (-r));
    r -= (r & (-r));
  }
}

void MeanShift::rightRotateAndEqual(Ciphertext &x, long r) {
  while (r & (-r)) {
    scheme.rightRotateFastAndEqual(x, r & (-r));
    r -= (r & (-r));
  }
}

// x.logq := x.logq
void MeanShift::repeatSlotRight(Ciphertext &x, long rep, long step) {
  Ciphertext orig;
  Ciphertext temp;
  orig.copy(x);

  long rot = 1;
  for (long i = NumBits(rep) - 2; i >= 0; --i) {
    temp.copy(x);
    rightRotateAndEqual(temp, step * rot);
    scheme.addAndEqual(x, temp);
    rot *= 2;

    if (bit(rep, i)) {
      temp.copy(orig);
      rightRotateAndEqual(temp, rot * step);
      scheme.addAndEqual(x, temp);
      rot += 1;
    }
  }
}

// x.logq := x.logq
void MeanShift::sumSlotsLeft(Ciphertext &x, long num, long step) {
  Ciphertext orig;
  Ciphertext temp;
  orig.copy(x);

  long rot = 1;
  for (long i = NumBits(num) - 2; i >= 0; --i) {
    temp.copy(x);
    leftRotateAndEqual(temp, rot * step);
    scheme.addAndEqual(x, temp);
    rot *= 2;

    if (bit(num, i)) {
      temp.copy(orig);
      leftRotateAndEqual(temp, rot * step);
      scheme.addAndEqual(x, temp);
      rot += 1;
    }
  }
}

// x.logq := x.logq - logp
void MeanShift::clearSlotsRight(Ciphertext &x, long ret_size, long skip,
                                long max_slot) {
  std::vector<complex<double>> selector(slots, complex<double>{0, 0});

  for (long i = 0; i + ret_size <= max_slot; i += skip) {
    std::fill(selector.begin() + i, selector.begin() + i + ret_size,
              complex<double>{1, 0});
  }

  scheme.multByConstVecAndEqual(x, selector.data(), logp);
  scheme.reScaleByAndEqual(x, logp);
}

// res.logq = x.logq - (steps + 2) * logp
void MeanShift::inverse(Ciphertext &res, Ciphertext &x, double m, long steps) {
  Ciphertext scaled;
  scheme.multByConst(scaled, x, 2.0 / m, logp);
  scheme.reScaleByAndEqual(scaled, logp);

  SchemeAlgo schemeAlgo(scheme);
  schemeAlgo.inverse(res, scaled, scaled.logp, steps);

  scheme.multByConstAndEqual(res, 2.0 / m, res.logp);
  scheme.reScaleByAndEqual(res, logp);
}

// res[i].logq = x[i].logq - (inv_steps + t + 3) * logp
void MeanShift::minIdx(std::vector<Ciphertext> &res, std::vector<Ciphertext> &x,
                       long t, long inv_steps) {
  Ciphertext sum;
  res.resize(x.size());

  for (size_t i = 0; i < x.size(); ++i) {
    // res[i] := 1 - x[i]
    scheme.negate(res[i], x[i]);
    scheme.addConstAndEqual(res[i], 1.0, logp);

    // res[i] := res[i]^(2^t)
    for (long j = 0; j < t; ++j) {
      scheme.squareAndEqual(res[i]);
      scheme.reScaleByAndEqual(res[i], logp);
    }

    // sum := sum + res[i]
    if (i == 0)
      sum.copy(res[0]);
    else {
      scheme.addAndEqual(sum, res[i]);
    }
  }

  // inv := 1/sum
  Ciphertext inv;
  inverse(inv, sum, x.size(), inv_steps);

  // res[i] := res[i] / sum
  for (size_t i = 0; i < res.size(); ++i) {
    scheme.modDownToAndEqual(res[i], inv.logq);
    scheme.multAndEqual(res[i], inv);
    scheme.reScaleByAndEqual(res[i], logp);
  }
}

// res.logq := x.logq - (inv_steps + t + 4) logp
void MeanShift::minIdxSIMD(Ciphertext &res, Ciphertext &x, long dim,
                           long npoints, long t, long inv_steps,
                           long batchsize) {
  scheme.negate(res, x);
  scheme.addConstAndEqual(res, 1.0, logp);

  for (long j = 0; j < t; ++j) {
    scheme.squareAndEqual(res);
    scheme.reScaleByAndEqual(res, logp);
  }

  Ciphertext sum;
  sum.copy(res);
  sumSlotsLeft(sum, batchsize, npoints * dim);
  clearSlotsRight(sum, npoints * dim, slots, slots);
  repeatSlotRight(sum, batchsize, npoints * dim);

  Ciphertext inv;
  inverse(inv, sum, batchsize, inv_steps);

  scheme.modDownToAndEqual(res, inv.logq);
  scheme.multAndEqual(res, inv);
  scheme.reScaleByAndEqual(res, logp);
}

// res.logq = x.logq - logp
void MeanShift::l2NormSquared(Ciphertext &res, Ciphertext &x) {
  Ciphertext rot;
  scheme.square(res, x);
  scheme.reScaleByAndEqual(res, logp);
  rot.copy(res);
  for (long i = 1; i < slots; ++i) {
    scheme.leftRotateFastAndEqual(rot, 1);
    scheme.addAndEqual(res, rot);
  }
}

// res.logq := x.logq - 2 logp
void MeanShift::l2NormSquaredSIMD(Ciphertext &res, Ciphertext &x, long dim,
                                  long num_points) {
  scheme.square(res, x);
  scheme.reScaleByAndEqual(res, logp);
  sumSlotsLeft(res, dim, 1);
  clearSlotsRight(res, 1, dim, num_points * dim);
  repeatSlotRight(res, dim, 1);
}

// res.logq := x.logq - (degree + 1) * logp
void MeanShift::kernel(Ciphertext &res, Ciphertext &x, Ciphertext &y,
                       long degree) {
  // diff = (x - y)^2
  Ciphertext diff;
  scheme.sub(diff, x, y);

  l2NormSquared(res, diff);

  // res = (1 - res)^(2^degree)
  scheme.negateAndEqual(res);
  scheme.addConstAndEqual(res, 1.0, logp);
  for (long i = 0; i < degree; ++i) {
    scheme.squareAndEqual(res);
    scheme.reScaleByAndEqual(res, logp);
  }
}

// res.logp := x.logp - (degree + 2) logp
void MeanShift::kernelSIMD(Ciphertext &res, Ciphertext &x, Ciphertext &y,
                           long dim, long num_points, long degree) {
  // diff = (x - y)^2
  Ciphertext diff;
  scheme.sub(diff, x, y);

  l2NormSquaredSIMD(res, diff, dim, num_points);

  // res = (1 - res)^(2^degree)
  scheme.negateAndEqual(res);
  scheme.addConstAndEqual(res, 1.0, logp);
  for (long i = 0; i < degree; ++i) {
    scheme.squareAndEqual(res);
    scheme.reScaleByAndEqual(res, logp);
  }
}

void MeanShift::modeSeeking(std::vector<Ciphertext> &res,
                            std::vector<Ciphertext> &points, long d, long steps,
                            long kdegree, long inv_steps) {
  res.resize(d);

  // Choose dusts
  std::mt19937 gen(seed);
  std::uniform_int_distribution<> distrib(0, points.size() - 1);
  for (long i = 0; i < d; ++i) {
    res[i].copy(points[distrib(gen)]);
  }

  // Gradient ascent
  for (long iter = 0; iter < steps; ++iter) {
    // dust.logq := dust.logq - (kdegree + inv_steps + 4) * logp
    for (auto &dust : res) {
      // Bootstrap if modulus not large enough for one more iteration
      if (dust.logq < (kdegree + inv_steps + 4) * logp + 1) {
        scheme.bootstrapAndEqual(dust, logq_boot, logQ, logt);
      }

      Ciphertext sum, A, a, tpoint, temp;

      for (size_t k = 0; k < points.size(); ++k) {
        if (points[k].logq != dust.logq)
          scheme.modDownTo(tpoint, points[k], dust.logq);
        else
          tpoint.copy(points[k]);

        kernel(a, tpoint, dust, kdegree);

        scheme.modDownTo(temp, tpoint, a.logq);
        scheme.multAndEqual(temp, a);
        scheme.reScaleByAndEqual(temp, logp);

        if (k == 0) {
          sum.copy(a);
          A.copy(temp);
        } else {
          scheme.addAndEqual(sum, a);
          scheme.addAndEqual(A, temp);
        }
      }

      Ciphertext inv;
      inverse(inv, sum, points.size(), inv_steps);

      scheme.modDownToAndEqual(A, inv.logq);
      scheme.mult(dust, inv, A);
      scheme.reScaleByAndEqual(dust, logp);
    }
  }
}

void MeanShift::refreshDusts(Ciphertext &dusts, long dim, long npoints,
                             long d) {
  Ciphertext temp1, temp2;

  // The cost of bootstrapping is proportional to the number of underlying
  // plaintext slots. Bootstrapping constitutes one of the most expensive
  // operations in the algorithm.
  //
  // Currently dusts are of the form (d1, d1, ..., d1, d2, d2, ..., dd).
  // We want convert it to (d1, d2, d3, ..., dd) to reduce the number of
  // plaintext slots.
  std::vector<complex<double>> selector(slots, complex<double>{0, 0});
  for (long i = 0; i < d; ++i) {
    std::fill(selector.begin() + i * npoints * dim,
              selector.begin() + i * npoints * dim + dim,
              complex<double>{1, 0});
    scheme.multByConstVec(temp1, dusts, selector.data(), logp);
    scheme.reScaleByAndEqual(temp1, logp);
    std::fill(selector.begin() + i * npoints * dim,
              selector.begin() + i * npoints * dim + dim,
              complex<double>{0, 0});

    if (i == 0) {
      temp2.copy(temp1);
    } else {
      leftRotateAndEqual(temp1, i * dim * (npoints - 1));
      scheme.addAndEqual(temp2, temp1);
    }
  }

  long step = 1 << ((long)std::ceil(std::log2(dim * d)));
  repeatSlotRight(temp2, slots / step, step);
  auto temp = temp2.n;
  temp2.n = step;

  scheme.bootstrapAndEqual(temp2, logq_boot, logQ, logt);
  temp2.n = temp;

  for (long i = 0; i < d; ++i) {
    std::fill(selector.begin() + i * dim, selector.begin() + (i + 1) * dim,
              complex<double>{1, 0});
    scheme.multByConstVec(temp1, temp2, selector.data(), logp);
    scheme.reScaleByAndEqual(temp1, logp);
    std::fill(selector.begin() + i * dim, selector.begin() + (i + 1) * dim,
              complex<double>{0, 0});

    if (i == 0) {
      dusts.copy(temp1);
    } else {
      rightRotateAndEqual(temp1, i * dim * (npoints - 1));
      scheme.addAndEqual(dusts, temp1);
    }
  }
}

void MeanShift::modeSeekingSIMD(Ciphertext &dusts,
                                std::vector<Ciphertext> &points, long dim,
                                long npoints, long d, long steps, long kdegree,
                                long inv_steps) {
  Ciphertext temp;

  // Choose dusts
  std::mt19937 gen(seed);
  // Can also choose a ciphertext at random and then a point in the ciphertext
  // at random. However, this approach allows easy verification when computed
  // in plaintext (check plaintext_clustering).
  std::uniform_int_distribution<> distrib(0, npoints * points.size() - 1);
  std::vector<complex<double>> selector(slots, complex<double>{0, 0});

  // Assume d < slots / (dim * npoints)
  for (long i = 0; i < d; ++i) {
    long r = distrib(gen);
    long pi = r % npoints;
    long ci = r / npoints;

    std::fill(selector.begin() + pi * dim, selector.begin() + (pi + 1) * dim,
              complex<double>{1, 0});
    scheme.multByConstVec(temp, points[ci], selector.data(), logp);
    scheme.reScaleByAndEqual(temp, logp);
    std::fill(selector.begin() + pi * dim, selector.begin() + (pi + 1) * dim,
              complex<double>{0, 0});

    long beg = i * npoints;
    if (beg < pi) {
      leftRotateAndEqual(temp, (pi - beg) * dim);
    } else if (beg > pi) {
      rightRotateAndEqual(temp, (beg - pi) * dim);
    }

    if (i == 0)
      dusts.copy(temp);
    else
      scheme.addAndEqual(dusts, temp);
  }

  // dusts will be repeated in loop below. The current dust format is expected
  // at the start of each iteration.

  // Gradient ascent
  // Now, dusts.logq = points.logq - logp

  Ciphertext grad, sum, A, temp2, tpoint;

  for (long iter = 0; iter < steps; ++iter) {
    // Bootstrap if modulus not large enough for one more iteration
    if (dusts.logq - (kdegree + inv_steps + 6) * logp < logq_boot) {
      refreshDusts(dusts, dim, npoints, d);
    }

    if (dusts.logq - (kdegree + inv_steps + 6) * logp < logq_boot) {
      throw std::runtime_error(
          "Modulus not large enough for mode seeking after bootstrap.");
    }

    // In the beginning of each iteration, only the first `dim` slots of dusts
    // have the right value and everything else has 0.
    repeatSlotRight(dusts, npoints, dim);

    for (size_t i = 0; i < points.size(); ++i) {
      scheme.modDownTo(tpoint, points[i], dusts.logq);
      kernelSIMD(grad, dusts, tpoint, dim, npoints * d, kdegree);

      scheme.modDownTo(temp2, tpoint, grad.logq);
      scheme.mult(temp, grad, temp2);
      scheme.reScaleByAndEqual(temp, logp);
      sumSlotsLeft(temp, npoints, dim);
      if (i == 0) {
        A.copy(temp);
      } else {
        scheme.addAndEqual(A, temp);
      }

      sumSlotsLeft(grad, npoints, dim);
      if (i == 0) {
        sum.copy(grad);
      } else {
        scheme.addAndEqual(sum, grad);
      }
    }

    Ciphertext inv;
    inverse(inv, sum, npoints * points.size(), inv_steps);

    scheme.modDownToAndEqual(A, inv.logq);
    scheme.mult(dusts, inv, A);
    scheme.reScaleByAndEqual(dusts, logp);
    clearSlotsRight(dusts, dim, dim * npoints, d * npoints * dim);
  }

  repeatSlotRight(dusts, npoints, dim);
}

// z := max(kdegree + 2, inv_steps + minidx_t + 5)
// res.logq = dusts.logq - z * logp
void MeanShift::pointLabeling(std::vector<std::vector<Ciphertext>> &res,
                              std::vector<Ciphertext> &dusts,
                              std::vector<Ciphertext> &points, long kdegree,
                              long inv_steps, long minidx_t) {
  std::vector<Ciphertext> nbhd(dusts.size());
  Ciphertext temp;

  for (size_t i = 0; i < dusts.size(); ++i) {
    for (size_t j = 0; j < dusts.size(); ++j) {
      kernel(temp, dusts[i], dusts[j], kdegree);

      if (j == 0) {
        nbhd[i].copy(temp);
      } else {
        scheme.addAndEqual(nbhd[i], temp);
      }
    }
  }

  std::vector<Ciphertext> Cbar;
  std::vector<Ciphertext> norms(dusts.size());

  res.resize(points.size());

  for (size_t i = 0; i < points.size(); ++i) {
    for (size_t j = 0; j < dusts.size(); ++j) {
      scheme.sub(temp, points[i], dusts[j]);
      l2NormSquared(norms[j], temp);
    }

    minIdx(Cbar, norms, minidx_t, inv_steps);
    res[i].resize(dusts.size());

    for (size_t j = 0; j < dusts.size(); ++j) {
      // make compatible for multiplication
      if (Cbar[j].logq < nbhd[j].logq) {
        // this will be run once for all points
        scheme.modDownToAndEqual(nbhd[j], Cbar[j].logq);
      } else if (Cbar[j].logq > nbhd[j].logq) {
        scheme.modDownToAndEqual(Cbar[j], nbhd[j].logq);
      }

      scheme.mult(res[i][j], Cbar[j], nbhd[j]);
      scheme.reScaleByAndEqual(res[i][j], logp);
    }
  }
}

// res.logq := dusts.logq - max(kdegree + 5, inv_steps + minidx_t + 7) logp
void MeanShift::pointLabelingSIMD(std::vector<Ciphertext> &res,
                                  Ciphertext &dusts,
                                  std::vector<Ciphertext> &points, long dim,
                                  long npoints, long d, long kdegree,
                                  long inv_steps, long minidx_t) {
  Ciphertext temp1, temp2, nbhd;

  // Currently dusts are of the form (d1, d1, ..., d1, d2, d2, ..., dd).
  // We want to now convert it to
  // (d1, d2, d3, ..., dd, 0, .., 0, d1, d2, d3, ... (repeated d times), dd, 0,
  // .., 0) so that we can easily compute pairwise kernel on the dusts.
  std::vector<complex<double>> selector(slots, complex<double>{0, 0});
  for (long i = 0; i < d; ++i) {
    std::fill(selector.begin() + i * npoints * dim,
              selector.begin() + i * npoints * dim + dim,
              complex<double>{1, 0});
    scheme.multByConstVec(temp1, dusts, selector.data(), logp);
    scheme.reScaleByAndEqual(temp1, logp);
    std::fill(selector.begin() + i * npoints * dim,
              selector.begin() + i * npoints * dim + dim,
              complex<double>{0, 0});

    if (i == 0) {
      temp2.copy(temp1);
    } else {
      leftRotateAndEqual(temp1, i * dim * (npoints - 1));
      scheme.addAndEqual(temp2, temp1);
    }
  }

  // `temp2` will now contain the dusts in the required format.
  repeatSlotRight(temp2, d, npoints * dim);

  // Compute `nbhd` which is the sum of the kernel value of a dust to every
  // other dust. (Refer paper for details)
  scheme.modDownBy(temp1, dusts, logp);
  kernelSIMD(nbhd, temp2, temp1, dim, npoints * d, kdegree);
  sumSlotsLeft(nbhd, d, dim);
  clearSlotsRight(nbhd, dim, npoints * dim, d * npoints * dim);
  repeatSlotRight(nbhd, npoints, dim);

  // Compute labels for each point ciphertext and store result in `res`.
  res.resize(points.size());
  for (size_t i = 0; i < points.size(); ++i) {
    scheme.sub(temp1, dusts, points[i]);
    l2NormSquaredSIMD(temp2, temp1, dim, npoints * d);
    minIdxSIMD(temp1, temp2, dim, npoints, minidx_t, inv_steps, d);

    if (nbhd.logq < temp1.logq)
      scheme.modDownToAndEqual(temp1, nbhd.logq);
    else if (nbhd.logq > temp1.logq)
      scheme.modDownToAndEqual(nbhd, temp1.logq);

    scheme.mult(res[i], temp1, nbhd);
    scheme.reScaleByAndEqual(res[i], logp);
  }
}

void MeanShift::clusterSIMD(std::vector<Ciphertext> &res,
                            std::vector<Ciphertext> &points, long dim,
                            long npoints, long num_dusts, long iterations,
                            long mode_kdegree, long label_kdegree,
                            long mode_inv_steps, long label_inv_steps,
                            long minidx_t) {
  Ciphertext dusts;
  modeSeekingSIMD(dusts, points, dim, npoints, num_dusts, iterations,
                  mode_kdegree, mode_inv_steps);

  // `dusts` has a lower modulus
  // Check if modulus suffices for point labeling
  if (dusts.logq < logq_boot)
    throw std::runtime_error(
        "Modulus lower than logl after mode seeking. Cannot bootstrap.");

  if (dusts.logq <
      std::max(label_kdegree + 5, label_inv_steps + minidx_t + 7) * logp) {
    refreshDusts(dusts, dim, npoints, num_dusts);
    repeatSlotRight(dusts, npoints, dim);
  }

  if (dusts.logq <
      std::max(label_kdegree + 5, label_inv_steps + minidx_t + 7) * logp)
    throw std::runtime_error(
        "Modulus not large enough for point labelling after bootstrap.");

  if (dusts.logq <
      std::max(label_kdegree + 5, label_inv_steps + minidx_t + 7) * logp)
    throw std::runtime_error(
        "Modulus not large enough for point labelling after bootstrap.");

  // `pointLabelingSIMD` expects `dusts` and `points` to have same modulus.
  std::vector<Ciphertext> mod_points(points.size());
  for (size_t i = 0; i < points.size(); ++i) {
    scheme.modDownTo(mod_points[i], points[i], dusts.logq);
  }

  pointLabelingSIMD(res, dusts, mod_points, dim, npoints, num_dusts,
                    label_kdegree, label_inv_steps, minidx_t);
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"	// Precompiled headers
//
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/CSchemeArchiveBase.h>
#include <mrpt/serialization/CSerializable.h>

#include <Eigen/Dense>
#include <iomanip>
#include <limits>

using namespace std;
using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::poses;

IMPLEMENTS_SERIALIZABLE(CPose3DQuat, CSerializable, mrpt::poses)

/** Constructor from a CPose3D */
CPose3DQuat::CPose3DQuat(const CPose3D& p)
{
	x() = p.x();
	y() = p.y();
	z() = p.z();
	p.getAsQuaternion(m_quat);
}

/** Constructor from a 4x4 homogeneous transformation matrix.
 */
CPose3DQuat::CPose3DQuat(const CMatrixDouble44& M)
	: m_quat(UNINITIALIZED_QUATERNION)
{
	m_coords[0] = M(0, 3);
	m_coords[1] = M(1, 3);
	m_coords[2] = M(2, 3);
	CPose3D p(M);
	p.getAsQuaternion(m_quat);
}

/** Returns the corresponding 4x4 homogeneous transformation matrix for the
 * point(translation) or pose (translation+orientation).
 * \sa getInverseHomogeneousMatrix
 */
void CPose3DQuat::getHomogeneousMatrix(CMatrixDouble44& out_HM) const
{
	m_quat.rotationMatrixNoResize(out_HM);
	out_HM(0, 3) = m_coords[0];
	out_HM(1, 3) = m_coords[1];
	out_HM(2, 3) = m_coords[2];
	out_HM(3, 0) = out_HM(3, 1) = out_HM(3, 2) = 0;
	out_HM(3, 3) = 1;
}

/** Returns a 1x7 vector with [x y z qr qx qy qz] */
void CPose3DQuat::asVector(vector_t& v) const
{
	v[0] = m_coords[0];
	v[1] = m_coords[1];
	v[2] = m_coords[2];
	v[3] = m_quat[0];
	v[4] = m_quat[1];
	v[5] = m_quat[2];
	v[6] = m_quat[3];
}

/**  Makes "this = A (+) B"; this method is slightly more efficient than "this=
 * A + B;" since it avoids the temporary object.
 *  \note A or B can be "this" without problems.
 */
void CPose3DQuat::composeFrom(const CPose3DQuat& A, const CPose3DQuat& B)
{
	// The 3D point:
	double gx, gy, gz;
	A.m_quat.rotatePoint(
		B.m_coords[0], B.m_coords[1], B.m_coords[2], gx, gy, gz);
	this->m_coords[0] = A.m_coords[0] + gx;
	this->m_coords[1] = A.m_coords[1] + gy;
	this->m_coords[2] = A.m_coords[2] + gz;

	// The 3D rotation:
	this->m_quat.crossProduct(A.m_quat, B.m_quat);
}

/**  Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
 * than "this= A - B;" since it avoids the temporary object.
 *  \note A or B can be "this" without problems.
 * \sa composeFrom
 */
void CPose3DQuat::inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B)
{
	// The 3D point:
	const CQuaternionDouble B_conj(
		B.m_quat.r(), -B.m_quat.x(), -B.m_quat.y(), -B.m_quat.z());
	B_conj.rotatePoint(
		A.m_coords[0] - B.m_coords[0], A.m_coords[1] - B.m_coords[1],
		A.m_coords[2] - B.m_coords[2], this->m_coords[0], this->m_coords[1],
		this->m_coords[2]);
	// The 3D rotation:
	this->m_quat.crossProduct(B_conj, A.m_quat);
}

/**  Computes the 3D point G such as \f$ G = this \oplus L \f$.
 * \sa inverseComposeFrom
 */
void CPose3DQuat::composePoint(
	const double lx, const double ly, const double lz, double& gx, double& gy,
	double& gz, mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint,
	mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose) const
{
	if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
	{
		const double qx2 = square(m_quat.x());
		const double qy2 = square(m_quat.y());
		const double qz2 = square(m_quat.z());

		// Jacob: df/dpoint
		if (out_jacobian_df_dpoint)
		{
			// 3x3:  df_{qr} / da

			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 3] = {
				1 - 2 * (qy2 + qz2),
				2 * (m_quat.x() * m_quat.y() - m_quat.r() * m_quat.z()),
				2 * (m_quat.r() * m_quat.y() + m_quat.x() * m_quat.z()),

				2 * (m_quat.r() * m_quat.z() + m_quat.x() * m_quat.y()),
				1 - 2 * (qx2 + qz2),
				2 * (m_quat.y() * m_quat.z() - m_quat.r() * m_quat.x()),

				2 * (m_quat.x() * m_quat.z() - m_quat.r() * m_quat.y()),
				2 * (m_quat.r() * m_quat.x() + m_quat.y() * m_quat.z()),
				1 - 2 * (qx2 + qy2)};
			out_jacobian_df_dpoint->loadFromArray(vals);
		}

		// Jacob: df/dpose
		if (out_jacobian_df_dpose)
		{
			// 3x7:  df_{qr} / dp
			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals1[3 * 7] = {
				1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0};
			out_jacobian_df_dpose->loadFromArray(vals1);

			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 4] = {
				2 * (-m_quat.z() * ly + m_quat.y() * lz),
				2 * (m_quat.y() * ly + m_quat.z() * lz),
				2 * (-2 * m_quat.y() * lx + m_quat.x() * ly + m_quat.r() * lz),
				2 * (-2 * m_quat.z() * lx - m_quat.r() * ly + m_quat.x() * lz),

				2 * (m_quat.z() * lx - m_quat.x() * lz),
				2 * (m_quat.y() * lx - 2 * m_quat.x() * ly - m_quat.r() * lz),
				2 * (m_quat.x() * lx + m_quat.z() * lz),
				2 * (m_quat.r() * lx - 2 * m_quat.z() * ly + m_quat.y() * lz),

				2 * (-m_quat.y() * lx + m_quat.x() * ly),
				2 * (m_quat.z() * lx + m_quat.r() * ly - 2 * m_quat.x() * lz),
				2 * (-m_quat.r() * lx + m_quat.z() * ly - 2 * m_quat.y() * lz),
				2 * (m_quat.x() * lx + m_quat.y() * ly)};

			CMatrixDouble44 norm_jacob(UNINITIALIZED_MATRIX);
			this->quat().normalizationJacobian(norm_jacob);

			out_jacobian_df_dpose->asEigen().block<3, 4>(0, 3) =
				(CMatrixFixed<double, 3, 4>(vals) * norm_jacob).eval();
		}
	}

	// function itself:
	m_quat.rotatePoint(lx, ly, lz, gx, gy, gz);
	gx += m_coords[0];
	gy += m_coords[1];
	gz += m_coords[2];
}

/**  Computes the 3D point G such as \f$ L = G \ominus this \f$.
 * \sa composeFrom
 */
void CPose3DQuat::inverseComposePoint(
	const double gx, const double gy, const double gz, double& lx, double& ly,
	double& lz, mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint,
	mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose) const
{
	if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
	{
		const double qx2 = square(m_quat.x());
		const double qy2 = square(m_quat.y());
		const double qz2 = square(m_quat.z());

		// Jacob: df/dpoint
		if (out_jacobian_df_dpoint)
		{
			// 3x3:  df_{m_quat.r()} / da
			//		inv_df_da =
			//		[ - 2*qy^2 - 2*qz^2 + 1,     2*qx*qy - 2*qr*qz,     2*qr*qy
			//+
			// 2*qx*qz]
			//		[     2*qr*qz + 2*qx*qy, - 2*qx^2 - 2*qz^2 + 1,     2*qy*qz
			//-
			// 2*qr*qx]
			//		[     2*qx*qz - 2*qr*qy,     2*qr*qx + 2*qy*qz, - 2*qx^2 -
			// 2*qy^2 + 1]
			//

			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 3] = {
				1 - 2 * (qy2 + qz2),
				2 * (m_quat.x() * m_quat.y() + m_quat.r() * m_quat.z()),
				2 * (-m_quat.r() * m_quat.y() + m_quat.x() * m_quat.z()),

				2 * (-m_quat.r() * m_quat.z() + m_quat.x() * m_quat.y()),
				1 - 2 * (qx2 + qz2),
				2 * (m_quat.y() * m_quat.z() + m_quat.r() * m_quat.x()),

				2 * (m_quat.x() * m_quat.z() + m_quat.r() * m_quat.y()),
				2 * (-m_quat.r() * m_quat.x() + m_quat.y() * m_quat.z()),
				1 - 2 * (qx2 + qy2)};
			out_jacobian_df_dpoint->loadFromArray(vals);
		}

		// Jacob: df/dpose
		if (out_jacobian_df_dpose)
		{
			// 3x7:  df_{m_quat.r()} / dp
			//		inv_df_dp =
			//[ 2*qy^2 + 2*qz^2 - 1, - 2*qr*qz - 2*qx*qy,   2*qr*qy - 2*qx*qz,
			// 2*qz*(ay - y) - 2*qy*(az - z),                 2*qy*(ay - y) +
			// 2*qz*(az - z), 2*qx*(ay - y) - 4*qy*(ax - x) - 2*qr*(az - z),
			// 2*qr*(ay - y) - 4*qz*(ax - x) + 2*qx*(az - z)]
			//[   2*qr*qz - 2*qx*qy, 2*qx^2 + 2*qz^2 - 1, - 2*qr*qx - 2*qy*qz,
			// 2*qx*(az - z) - 2*qz*(ax - x), 2*qy*(ax - x) - 4*qx*(ay - y) +
			// 2*qr*(az - z),                 2*qx*(ax - x) + 2*qz*(az - z),
			// 2*qy*(az - z) - 4*qz*(ay - y) - 2*qr*(ax - x)]
			//[ - 2*qr*qy - 2*qx*qz,   2*qr*qx - 2*qy*qz, 2*qx^2 + 2*qy^2 - 1,
			// 2*qy*(ax - x) - 2*qx*(ay - y), 2*qz*(ax - x) - 2*qr*(ay - y) -
			// 4*qx*(az - z), 2*qr*(ax - x) + 2*qz*(ay - y) - 4*qy*(az - z),
			// 2*qx*(ax - x) + 2*qy*(ay - y)]
			//
			const double qr = m_quat.r();
			const double qx = m_quat.x();
			const double qy = m_quat.y();
			const double qz = m_quat.z();

			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals1[3 * 7] = {
				2 * qy2 + 2 * qz2 - 1,
				-2 * qr * qz - 2 * qx * qy,
				2 * qr * qy - 2 * qx * qz,
				0,
				0,
				0,
				0,

				2 * qr * qz - 2 * qx * qy,
				2 * qx2 + 2 * qz2 - 1,
				-2 * qr * qx - 2 * qy * qz,
				0,
				0,
				0,
				0,

				-2 * qr * qy - 2 * qx * qz,
				2 * qr * qx - 2 * qy * qz,
				2 * qx2 + 2 * qy2 - 1,
				0,
				0,
				0,
				0,
			};

			out_jacobian_df_dpose->loadFromArray(vals1);

			const double Ax = 2 * (gx - m_coords[0]);
			const double Ay = 2 * (gy - m_coords[1]);
			const double Az = 2 * (gz - m_coords[2]);

			alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 4] = {
				-qy * Az + qz * Ay,
				qy * Ay + qz * Az,
				qx * Ay - 2 * qy * Ax - qr * Az,
				qx * Az + qr * Ay - 2 * qz * Ax,

				qx * Az - qz * Ax,
				qy * Ax - 2 * qx * Ay + qr * Az,
				qx * Ax + qz * Az,
				qy * Az - 2 * qz * Ay - qr * Ax,

				qy * Ax - qx * Ay,
				qz * Ax - qr * Ay - 2 * qx * Az,
				qr * Ax + qz * Ay - 2 * qy * Az,
				qx * Ax + qy * Ay};

			CMatrixDouble44 norm_jacob(UNINITIALIZED_MATRIX);
			this->quat().normalizationJacobian(norm_jacob);

			out_jacobian_df_dpose->insertMatrix(
				0, 3, (CMatrixFixed<double, 3, 4>(vals) * norm_jacob).eval());
		}
	}

	// function itself:
	m_quat.inverseRotatePoint(
		gx - m_coords[0], gy - m_coords[1], gz - m_coords[2], lx, ly, lz);
}

/*---------------------------------------------------------------
	*=
  ---------------------------------------------------------------*/
void CPose3DQuat::operator*=(const double s)
{
	m_coords[0] *= s;
	m_coords[1] *= s;
	m_coords[2] *= s;
	m_quat[0] *= s;
	m_quat[1] *= s;
	m_quat[2] *= s;
	m_quat[3] *= s;
}

uint8_t CPose3DQuat::serializeGetVersion() const { return 0; }
void CPose3DQuat::serializeTo(mrpt::serialization::CArchive& out) const
{
	out << m_coords[0] << m_coords[1] << m_coords[2] << m_quat[0] << m_quat[1]
		<< m_quat[2] << m_quat[3];
}
void CPose3DQuat::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		{
			in >> m_coords[0] >> m_coords[1] >> m_coords[2] >> m_quat[0] >>
				m_quat[1] >> m_quat[2] >> m_quat[3];
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}
/** Serialize CSerializable Object to CSchemeArchiveBase derived object*/
void CPose3DQuat::serializeTo(
	mrpt::serialization::CSchemeArchiveBase& out) const
{
	SCHEMA_SERIALIZE_DATATYPE_VERSION(1);
	CPoint3D point(m_coords[0], m_coords[1], m_coords[2]);
	out["point"] = point;
	out["orientation"]["r"] = m_quat[0];
	out["orientation"]["x"] = m_quat[1];
	out["orientation"]["y"] = m_quat[2];
	out["orientation"]["z"] = m_quat[3];
}
/** Serialize CSchemeArchiveBase derived object to CSerializable Object*/
void CPose3DQuat::serializeFrom(mrpt::serialization::CSchemeArchiveBase& in)
{
	uint8_t version;
	SCHEMA_DESERIALIZE_DATATYPE_VERSION();
	switch (version)
	{
		case 1:
		{
			CPoint3D point;
			in["point"].readTo(point);
			m_coords[0] = point.x();
			m_coords[1] = point.y();
			m_coords[2] = point.z();
			m_quat[0] = static_cast<double>(in["orientation"]["r"]);
			m_quat[1] = static_cast<double>(in["orientation"]["x"]);
			m_quat[2] = static_cast<double>(in["orientation"]["y"]);
			m_quat[3] = static_cast<double>(in["orientation"]["z"]);
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	}
}
/*---------------------------------------------------------------
		sphericalCoordinates
---------------------------------------------------------------*/
void CPose3DQuat::sphericalCoordinates(
	const TPoint3D& point, double& out_range, double& out_yaw,
	double& out_pitch,
	mrpt::math::CMatrixFixed<double, 3, 3>* out_jacob_dryp_dpoint,
	mrpt::math::CMatrixFixed<double, 3, 7>* out_jacob_dryp_dpose) const
{
	const bool comp_jacobs =
		out_jacob_dryp_dpoint != nullptr || out_jacob_dryp_dpose != nullptr;

	// Pass to coordinates as seen from this 6D pose:
	CMatrixFixed<double, 3, 3> jacob_dinv_dpoint,
		*ptr_ja1 = comp_jacobs ? &jacob_dinv_dpoint : nullptr;
	CMatrixFixed<double, 3, 7> jacob_dinv_dpose,
		*ptr_ja2 = comp_jacobs ? &jacob_dinv_dpose : nullptr;

	TPoint3D local;
	this->inverseComposePoint(
		point.x, point.y, point.z, local.x, local.y, local.z, ptr_ja1, ptr_ja2);

	// Range:
	out_range = local.norm();

	// Yaw:
	if (local.y != 0 || local.x != 0) out_yaw = atan2(local.y, local.x);
	else
		out_yaw = 0;

	// Pitch:
	if (out_range != 0) out_pitch = -asin(local.z / out_range);
	else
		out_pitch = 0;

	// Jacobians are:
	//  dryp_dpoint = dryp_dlocalpoint  * dinv_dpoint
	//  dryp_dpose  = dryp_dlocalpoint  * dinv_dpose
	if (comp_jacobs)
	{
		if (out_range == 0)
			THROW_EXCEPTION("Jacobians are undefined for range=0");

		/* MATLAB:
			syms H h_range h_yaw h_pitch real;
			syms xi_ yi_ zi_ real;
			h_range = sqrt(xi_^2+yi_^2+zi_^2);
			h_yaw   = atan(yi_/xi_);
			% h_pitch = -asin(zi_/ sqrt( xi_^2 + yi_^2 + zi_^2 ) );
			h_pitch = -atan(zi_, sqrt( xi_^2 + yi_^2) );
			H=[ h_range ; h_yaw ; h_pitch ];
			jacob_fesf_xyz=jacobian(H,[xi_ yi_ zi_])
		*/
		const double _r = 1.0 / out_range;
		const double x2 = square(local.x);
		const double y2 = square(local.y);

		const double t2 = std::sqrt(x2 + y2);
		const double _K = 1.0 / (t2 * square(out_range));

		double vals[3 * 3] = {
			local.x * _r,
			local.y * _r,
			local.z * _r,
			-local.y / (x2 * (y2 / x2 + 1)),
			1.0 / (local.x * (y2 / x2 + 1)),
			0,
			(local.x * local.z) * _K,
			(local.y * local.z) * _K,
			-t2 / square(out_range)};

		const CMatrixDouble33 dryp_dlocalpoint(vals);
		if (out_jacob_dryp_dpoint)
			*out_jacob_dryp_dpoint = dryp_dlocalpoint * jacob_dinv_dpoint;
		if (out_jacob_dryp_dpose)
			*out_jacob_dryp_dpose = dryp_dlocalpoint * jacob_dinv_dpose;
	}
}

/**  Textual output stream function.
 */
std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose3DQuat& p)
{
	const std::streamsize old_pre = o.precision();
	const ios_base::fmtflags old_flags = o.flags();
	o << "(x,y,z,qr,qx,qy,qz)=(" << std::fixed << std::setprecision(4)
	  << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
	  << p.quat()[0] << "," << p.quat()[1] << "," << p.quat()[2] << ","
	  << p.quat()[3] << ")";
	o.flags(old_flags);
	o.precision(old_pre);
	return o;
}

/** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
 * than a pose with all its arguments multiplied by "-1") */
CPose3DQuat mrpt::poses::operator-(const CPose3DQuat& p)
{
	CPose3DQuat ret = p;
	ret.inverse();
	return ret;
}

/** Convert this pose into its inverse, saving the result in itself. \sa
 * operator- */
void CPose3DQuat::inverse()
{
	// Invert translation:
	this->inverseComposePoint(0, 0, 0, m_coords[0], m_coords[1], m_coords[2]);

	// Invert rotation: [qr qx qy qz] ==> [qr -qx -qy -qz]
	m_quat[1] = -m_quat[1];
	m_quat[2] = -m_quat[2];
	m_quat[3] = -m_quat[3];
}

void CPose3DQuat::setToNaN()
{
	for (int i = 0; i < 3; i++)
		m_coords[i] = std::numeric_limits<double>::quiet_NaN();

	for (int i = 0; i < 4; i++)
		quat()[i] = std::numeric_limits<double>::quiet_NaN();
}

bool mrpt::poses::operator==(const CPose3DQuat& p1, const CPose3DQuat& p2)
{
	return p1.quat() == p2.quat() && p1.x() == p2.x() && p1.y() == p2.y() &&
		p1.z() == p2.z();
}

bool mrpt::poses::operator!=(const CPose3DQuat& p1, const CPose3DQuat& p2)
{
	return !(p1 == p2);
}

CPoint3D mrpt::poses::operator-(const CPoint3D& G, const CPose3DQuat& p)
{
	CPoint3D L;
	p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
	return L;
}

TPoint3D mrpt::poses::operator-(const TPoint3D& G, const CPose3DQuat& p)
{
	mrpt::math::TPoint3D L;
	p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
	return L;
}

TPose3DQuat CPose3DQuat::asTPose() const
{
	return TPose3DQuat(
		x(), y(), z(), m_quat.r(), m_quat.x(), m_quat.y(), m_quat.z());
}

void CPose3DQuat::fromString(const std::string& s)
{
	mrpt::math::CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION_FMT(
			"Malformed expression in ::fromString, s=\"%s\"", s.c_str());
	ASSERTMSG_(m.rows() == 1 && m.cols() == 7, "Expected vector length=7");
	m_coords[0] = m(0, 0);
	m_coords[1] = m(0, 1);
	m_coords[2] = m(0, 2);
	m_quat[0] = m(0, 3);
	m_quat[1] = m(0, 4);
	m_quat[2] = m(0, 5);
	m_quat[3] = m(0, 6);
}

void CPose3DQuat::fromStringRaw(const std::string& s)
{
	this->fromString("[" + s + "]");
}


#ifndef _CELERITE2_INTERNAL_HPP_DEFINED_
#define _CELERITE2_INTERNAL_HPP_DEFINED_

#include <Eigen/Core>

namespace celerite2 {
namespace core {

#define UNUSED(x) (void)(x)

#define CAST_BASE(TYPE, VAR) Eigen::MatrixBase<TYPE> &VAR = const_cast<Eigen::MatrixBase<TYPE> &>(VAR##_out)

#define CAST_VEC(TYPE, VAR, ROWS)                                                                                \
  CAST_BASE(TYPE, VAR);                                                                                \
  VAR.derived().resize(ROWS)

#define CAST_MAT(TYPE, VAR, ROWS, COLS)                                                                                \
  CAST_BASE(TYPE, VAR);                                                                                \
  VAR.derived().resize(ROWS, COLS)

const int THE_WORKSPACE_VARIABLE_MUST_BE_ROW_MAJOR = 0;
#define ASSERT_ROW_MAJOR(TYPE) EIGEN_STATIC_ASSERT((TYPE::ColsAtCompileTime == 1) || TYPE::IsRowMajor, THE_WORKSPACE_VARIABLE_MUST_BE_ROW_MAJOR)

namespace internal {

template <bool do_update = true>
struct update_workspace {
  template <typename A, typename B>
  static void apply(Eigen::Index n, const Eigen::MatrixBase<A> &a, Eigen::MatrixBase<B> const &b_out) {
    CAST_BASE(B, b);
    b.row(n) = a;
  }
};

template <>
struct update_workspace<false> {
  template <typename A, typename B>
  static void apply(Eigen::Index n, const Eigen::MatrixBase<A> &a, Eigen::MatrixBase<B> const &b_out) {
    UNUSED(n);
    UNUSED(a);
    UNUSED(b_out);
  }
};

template <bool is_solve = false>
struct update_f {
  template <typename A, typename B, typename C, typename D>
  static void apply(const Eigen::MatrixBase<A> &a, const Eigen::MatrixBase<B> &b, const Eigen::MatrixBase<C> &c, Eigen::MatrixBase<D> const &d_out) {
    CAST_BASE(D, d);
    d.noalias() += a * b;
    UNUSED(c);
  }

  template <typename A, typename B, typename C, typename D, typename E, typename F, typename G>
  static void reverse(const Eigen::MatrixBase<A> &a, const Eigen::MatrixBase<B> &b, const Eigen::MatrixBase<C> &c, const Eigen::MatrixBase<D> &d,
                      Eigen::MatrixBase<E> const &e_out, Eigen::MatrixBase<F> const &f_out, Eigen::MatrixBase<G> const &g_out) {
    CAST_BASE(E, e);
    CAST_BASE(F, f);
    e.noalias() += b * d.transpose();
    f.noalias() += a * d;
    UNUSED(c);
    UNUSED(g_out);
  }
};

template <>
struct update_f<true> {
  template <typename A, typename B, typename C, typename D>
  static void apply(const Eigen::MatrixBase<A> &a, const Eigen::MatrixBase<B> &b, const Eigen::MatrixBase<C> &c, Eigen::MatrixBase<D> const &d_out) {
    CAST_BASE(D, d);
    d.noalias() += a * c;
    UNUSED(b);
  }

  template <typename A, typename B, typename C, typename D, typename E, typename F, typename G>
  static void reverse(const Eigen::MatrixBase<A> &a, const Eigen::MatrixBase<B> &b, const Eigen::MatrixBase<C> &c, const Eigen::MatrixBase<D> &d,
                      Eigen::MatrixBase<E> const &e_out, Eigen::MatrixBase<F> const &f_out, Eigen::MatrixBase<G> const &g_out) {
    CAST_BASE(E, e);
    CAST_BASE(G, g);
    e.noalias() += c * d.transpose();
    g.noalias() += a * d;
    UNUSED(b);
    UNUSED(f_out);
  }
};

template <bool is_solve = false>
struct update_z {
  template <typename A, typename B>
  static void apply(const Eigen::MatrixBase<A> &a, Eigen::MatrixBase<B> const &b_out) {
    CAST_BASE(B, b);
    b.noalias() += a;
  }
};

template <>
struct update_z<true> {
  template <typename A, typename B>
  static void apply(const Eigen::MatrixBase<A> &a, Eigen::MatrixBase<B> const &b_out) {
    CAST_BASE(B, b);
    b.noalias() -= a;
  }
};

template <bool is_solve, bool do_update = true, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut,
          typename Work>
void forward(const Eigen::MatrixBase<Input> &t,                // (N,)
             const Eigen::MatrixBase<Coeffs> &c,               // (J,)
             const Eigen::MatrixBase<LowRank> &U,              // (N, J)
             const Eigen::MatrixBase<LowRank> &V,              // (N, J)
             const Eigen::MatrixBase<RightHandSide> &Y,        // (N, Nrhs)
             Eigen::MatrixBase<RightHandSideOut> const &Z_out, // (N, Nrhs)
             Eigen::MatrixBase<Work> const &F_out              // (N, J * Nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  typedef typename LowRank::Scalar Scalar;
  typedef typename Eigen::internal::plain_row_type<RightHandSide>::type RowVector;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;
  typedef typename Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, RightHandSide::ColsAtCompileTime> Inner;

  Eigen::Index N = U.rows(), J = U.cols(), nrhs = Y.cols();
  CAST_BASE(RightHandSideOut, Z); // Must already be the right shape
  CAST_BASE(Work, F);
  if (do_update) {
    F.derived().resize(N, J * nrhs);
    F.row(0).setZero();
  }

  CoeffVector p(J);
  Inner Fn(J, nrhs);
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Fn.data(), 1, J * nrhs);

  // This will track the previous row allowing for inplace operations
  RowVector tmp = Y.row(0);

  Fn.setZero();
  for (Eigen::Index n = 1; n < N; ++n) {
    p = exp(c.array() * (t(n - 1) - t(n)));
    update_f<is_solve>::apply(V.row(n - 1).transpose(), tmp, Z.row(n - 1), Fn);
    tmp = Y.row(n);
    update_workspace<do_update>::apply(n, ptr, F);
    Fn = p.asDiagonal() * Fn;
    update_z<is_solve>::apply(U.row(n) * Fn, Z.row(n));
  }
}

template <bool is_solve, bool do_update = true, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut,
          typename Work>
void backward(const Eigen::MatrixBase<Input> &t,                // (N,)
              const Eigen::MatrixBase<Coeffs> &c,               // (J,)
              const Eigen::MatrixBase<LowRank> &U,              // (N, J)
              const Eigen::MatrixBase<LowRank> &V,              // (N, J)
              const Eigen::MatrixBase<RightHandSide> &Y,        // (N, Nrhs)
              Eigen::MatrixBase<RightHandSideOut> const &Z_out, // (N, Nrhs)
              Eigen::MatrixBase<Work> const &F_out              // (N, J * Nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  typedef typename LowRank::Scalar Scalar;
  typedef typename Eigen::internal::plain_row_type<RightHandSide>::type RowVector;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;
  typedef typename Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, RightHandSide::ColsAtCompileTime> Inner;

  Eigen::Index N = U.rows(), J = U.cols(), nrhs = Y.cols();
  CAST_BASE(RightHandSideOut, Z); // Must already be the right shape
  CAST_BASE(Work, F);
  if (do_update) {
    F.derived().resize(N, J * nrhs);
    F.row(N - 1).setZero();
  }

  CoeffVector p(J);
  Inner Fn(J, nrhs);
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Fn.data(), 1, J * nrhs);

  // This will track the previous row allowing for inplace operations
  RowVector tmp = Y.row(N - 1);

  Fn.setZero();
  for (Eigen::Index n = N - 2; n >= 0; --n) {
    p = exp(c.array() * (t(n) - t(n + 1)));
    update_f<is_solve>::apply(U.row(n + 1).transpose(), tmp, Z.row(n + 1), Fn);
    tmp = Y.row(n);
    update_workspace<do_update>::apply(n, ptr, F);
    Fn = p.asDiagonal() * Fn;
    update_z<is_solve>::apply(V.row(n) * Fn, Z.row(n));
  }
}

template <bool is_solve, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOptional, typename Work,
          typename RightHandSideInternal, typename InputOut, typename CoeffsOut, typename LowRankOut, typename RightHandSideOut>
void forward_rev(const Eigen::MatrixBase<Input> &t,                      // (N,)
                 const Eigen::MatrixBase<Coeffs> &c,                     // (J,)
                 const Eigen::MatrixBase<LowRank> &U,                    // (N, J)
                 const Eigen::MatrixBase<LowRank> &V,                    // (N, J)
                 const Eigen::MatrixBase<RightHandSideOptional> &Y,      // (N, Nrhs)
                 const Eigen::MatrixBase<RightHandSide> &Z,              // (N, Nrhs)
                 const Eigen::MatrixBase<Work> &F,                       // (N, J * Nrhs)
                 Eigen::MatrixBase<RightHandSideInternal> const &bZ_out, // (N, Nrhs)
                 Eigen::MatrixBase<InputOut> const &bt_out,              // (N,)
                 Eigen::MatrixBase<CoeffsOut> const &bc_out,             // (J,)
                 Eigen::MatrixBase<LowRankOut> const &bU_out,            // (N, J)
                 Eigen::MatrixBase<LowRankOut> const &bV_out,            // (N, J)
                 Eigen::MatrixBase<RightHandSideOut> const &bY_out       // (N, Nrhs)  -  Must be the right shape already (and zeroed)
) {
  ASSERT_ROW_MAJOR(Work);

  typedef typename LowRank::Scalar Scalar;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;
  typedef typename Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, RightHandSide::ColsAtCompileTime> Inner;

  Eigen::Index N = U.rows(), J = U.cols(), nrhs = Y.cols();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bV, N, J);
  CAST_BASE(RightHandSideOut, bY);
  CAST_BASE(RightHandSideInternal, bZ);

  Scalar dt, factor;
  CoeffVector p(J), bp(J);
  Inner Fn(J, nrhs), bF(J, nrhs);
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Fn.data(), 1, J * nrhs);
  bF.setZero();
  for (Eigen::Index n = N - 1; n >= 1; --n) {
    dt  = t(n - 1) - t(n);
    p   = exp(c.array() * dt);
    ptr = F.row(n);

    // Reverse: update_z<is_solve>::apply(U.row(n) * Fn, Z.row(n));
    update_z<is_solve>::apply(bZ.row(n) * (p.asDiagonal() * Fn).transpose(), bU.row(n));
    update_z<is_solve>::apply(U.row(n).transpose() * bZ.row(n), bF);

    // Reverse: Fn = P.row(n - 1).asDiagonal() * Fn;
    bp.array() = (Fn * bF.transpose()).diagonal().array() * p.array();
    bc.noalias() += dt * bp;
    factor = (c.array() * bp.array()).sum();
    bt(n) -= factor;
    bt(n - 1) += factor;
    bF = p.asDiagonal() * bF;

    // Reverse: update_f<is_solve>::apply(V.row(n - 1).transpose(), Y.row(n - 1), Z.row(n - 1), Fn);
    update_f<is_solve>::reverse(V.row(n - 1), Y.row(n - 1), Z.row(n - 1), bF, bV.row(n - 1), bY.row(n - 1), bZ.row(n - 1));
  }
}

template <bool is_solve, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename Work, typename RightHandSideInternal,
          typename InputOut, typename CoeffsOut, typename LowRankOut, typename RightHandSideOut>
void backward_rev(const Eigen::MatrixBase<Input> &t,                      // (N,)
                  const Eigen::MatrixBase<Coeffs> &c,                     // (J,)
                  const Eigen::MatrixBase<LowRank> &U,                    // (N, J)
                  const Eigen::MatrixBase<LowRank> &V,                    // (N, J)
                  const Eigen::MatrixBase<RightHandSide> &Y,              // (N, Nrhs)
                  const Eigen::MatrixBase<RightHandSide> &Z,              // (N, Nrhs)
                  const Eigen::MatrixBase<Work> &F,                       // (N, J * Nrhs)
                  Eigen::MatrixBase<RightHandSideInternal> const &bZ_out, // (N, Nrhs)
                  Eigen::MatrixBase<InputOut> const &bt_out,              // (N,)
                  Eigen::MatrixBase<CoeffsOut> const &bc_out,             // (J,)
                  Eigen::MatrixBase<LowRankOut> const &bU_out,            // (N, J)
                  Eigen::MatrixBase<LowRankOut> const &bV_out,            // (N, J)
                  Eigen::MatrixBase<RightHandSideOut> const &bY_out       // (N, Nrhs)  -  Must be the right shape already (and zeroed)
) {
  ASSERT_ROW_MAJOR(Work);

  typedef typename LowRank::Scalar Scalar;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;
  typedef typename Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, RightHandSide::ColsAtCompileTime> Inner;

  Eigen::Index N = U.rows(), J = U.cols(), nrhs = Y.cols();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bV, N, J);
  CAST_BASE(RightHandSideOut, bY);
  CAST_BASE(RightHandSideInternal, bZ);

  Scalar dt, factor;
  CoeffVector p(J), bp(J);
  Inner Fn(J, nrhs), bF(J, nrhs);
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Fn.data(), 1, J * nrhs);
  bF.setZero();
  for (Eigen::Index n = 0; n <= N - 2; ++n) {
    dt  = t(n) - t(n + 1);
    p   = exp(c.array() * dt);
    ptr = F.row(n);

    // Reverse: update_z<is_solve>::apply(V.row(n) * Fn, Z.row(n));
    update_z<is_solve>::apply(bZ.row(n) * (p.asDiagonal() * Fn).transpose(), bV.row(n));
    update_z<is_solve>::apply(V.row(n).transpose() * bZ.row(n), bF);

    // Reverse: Fn = P.row(n).asDiagonal() * Fn;
    bp.array() = (Fn * bF.transpose()).diagonal().array() * p.array();
    bc.noalias() += dt * bp;
    factor = (c.array() * bp.array()).sum();
    bt(n + 1) -= factor;
    bt(n) += factor;
    bF = p.asDiagonal() * bF;

    // Reverse: update_f<is_solve>::apply(U.row(n + 1).transpose(), Y.row(n + 1), Z.row(n + 1), Fn);
    update_f<is_solve>::reverse(U.row(n + 1), Y.row(n + 1), Z.row(n + 1), bF, bU.row(n + 1), bY.row(n + 1), bZ.row(n + 1));
  }
}

} // namespace internal

} // namespace core
} // namespace celerite2

#endif // _CELERITE2_INTERNAL_HPP_DEFINED_


/*
 * Copyright (C) 2017 Martin Lambers <marlam@marlam.de>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "webcam-head-tracker.hpp"

#include <chrono>
#include <cstdlib>
#include <cstdio>
#include <cmath>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/video/tracking.hpp>

#include <dlib/opencv.h>
#include <dlib/image_processing.h>

/* Helpers for timing */

class timer
{
public:
    std::chrono::steady_clock::time_point t;
    void setNow() { t = std::chrono::steady_clock::now(); }
};

float duration(const timer& start, const timer& end)
{
    std::chrono::duration<long long, std::micro> time_span
        = std::chrono::duration_cast<std::chrono::duration<long long, std::micro>>(end.t - start.t);
    return time_span.count() / 1e3f;
}

/* Double Exponential Smoothing
 * This implements double exponential smoothing-based prediction
 * as described in Sec. 2 of "Double Exponential Smoothing: An alternative to
 * Kalman Filter-Based Predictive Tracking" by Joseph J. LaViola Jr.
 */

class DoubleExponentialSmoothing
{
private:
    bool _isInitialized;
    double _lastVecS[3], _lastVecS2[3];
    double _lastQuatS[4], _lastQuatS2[4];

    inline void copy3(double* result, const double* value)
    {
        result[0] = value[0];
        result[1] = value[1];
        result[2] = value[2];
    }

    inline void copy4(double* result, const double* value)
    {
        result[0] = value[0];
        result[1] = value[1];
        result[2] = value[2];
        result[3] = value[3];
    }

    inline double mix(double alpha, double x, double y)
    {
        return alpha * y + (1.0 - alpha) * x;
    }

    inline void mix3(double* result, double alpha, const double* v, const double* w)
    {
        result[0] = mix(alpha, v[0], w[0]);
        result[1] = mix(alpha, v[1], w[1]);
        result[2] = mix(alpha, v[2], w[2]);
    }

    inline void mix4(double* result, double alpha, const double* v, const double* w)
    {
        result[0] = mix(alpha, v[0], w[0]);
        result[1] = mix(alpha, v[1], w[1]);
        result[2] = mix(alpha, v[2], w[2]);
        result[3] = mix(alpha, v[3], w[3]);
    }

    inline double dot4(const double* v, const double* w)
    {
        return (v[0] * w[0] + v[1] * w[1] + v[2] * w[2] + v[3] * w[3]);
    }

    inline void normalize4(double* v)
    {
        double s = std::sqrt(dot4(v, v));
        v[0] /= s;
        v[1] /= s;
        v[2] /= s;
        v[3] /= s;
    }

    inline void slerp(double* result, double alpha, const double* q, const double* r)
    {
        double w[4] = { r[0], r[1], r[2], r[3] };
        double cosHalfAngle = dot4(q, r);
        if (cosHalfAngle < 0.0) {
            // quat(x, y, z, w) and quat(-x, -y, -z, -w) represent the same rotation
            w[0] = -w[0]; w[1] = -w[1]; w[2] = -w[2]; w[3] = -w[3];
            cosHalfAngle = -cosHalfAngle;
        }
        double tmpQ, tmpW;
        if (std::fabs(cosHalfAngle) >= 1.0) {
            // angle is zero => rotations are identical
            tmpQ = 1.0;
            tmpW = 0.0;
        } else {
            double halfAngle = acos(cosHalfAngle);
            double sinHalfAngle = sqrt(1.0 - cosHalfAngle * cosHalfAngle);
            if (std::fabs(sinHalfAngle) < 0.001) {
                // angle is 180 degrees => result is not clear
                tmpQ = 0.5;
                tmpW = 0.5;
            } else {
                tmpQ = std::sin((1.0 - alpha) * halfAngle) / sinHalfAngle;
                tmpW = sin(alpha * halfAngle) / sinHalfAngle;
            }
        }
        result[0] = q[0] * tmpQ + w[0] * tmpW;
        result[1] = q[1] * tmpQ + w[1] * tmpW;
        result[2] = q[2] * tmpQ + w[2] * tmpW;
        result[3] = q[3] * tmpQ + w[3] * tmpW;
    }

public:
    DoubleExponentialSmoothing() : _isInitialized(false) {}

    void step(const double* vec, const double* quat,
            double alpha, double tau, double* estimatedVec, double* estimatedQuat)
    {
        if (!_isInitialized) {
            copy3(_lastVecS, vec);
            copy3(_lastVecS2, vec);
            copy4(_lastQuatS, quat);
            copy4(_lastQuatS2, quat);
            _isInitialized = true;
        }
        double vecS[3], vecS2[3], quatS[4], quatS2[4];
        // Eq. (1), (2)
        mix3(vecS, alpha, _lastVecS, vec);
        mix3(vecS2, alpha, _lastVecS2, vecS);
        mix4(quatS, alpha, _lastQuatS, quat);
        mix4(quatS2, alpha, _lastQuatS2, quatS);
        copy3(_lastVecS, vecS);
        copy3(_lastVecS2, vecS2);
        copy4(_lastQuatS, quatS);
        copy4(_lastQuatS2, quatS2);
        // Eq. (6) for floor(tau) and ceil(tau)
        double floorTau = std::floor(tau);
        double betaFloorTau = 2.0 + alpha * floorTau / (1.0 - alpha);
        double estimatedVecFloorTau[3], estimatedQuatFloorTau[4];
        mix3(estimatedVecFloorTau, betaFloorTau, vecS2, vecS);
        mix4(estimatedQuatFloorTau, betaFloorTau, quatS2, quatS);
        normalize4(estimatedQuatFloorTau);
        double ceilTau = std::ceil(tau);
        double betaCeilTau = 2.0 + alpha * ceilTau / (1.0 - alpha);
        double estimatedVecCeilTau[3], estimatedQuatCeilTau[4];
        mix3(estimatedVecCeilTau, betaCeilTau, vecS2, vecS);
        mix4(estimatedQuatCeilTau, betaCeilTau, quatS2, quatS);
        normalize4(estimatedQuatCeilTau);
        // mix results for floor(tau) and ceil(tau)
        mix3(estimatedVec, tau - floorTau, estimatedVecCeilTau, estimatedVecFloorTau);
        slerp(estimatedQuat, tau - floorTau, estimatedQuatCeilTau, estimatedQuatFloorTau);
    }
};

/* WebcamHeadTracker */

WebcamHeadTracker::WebcamHeadTracker(unsigned int debugOptions) :
    _debugOptions(debugOptions),
    _isReady(false),
    _capture(NULL),
    _frame(NULL),
    _w(0), _h(0),
    _fps(0.0f),
    _fx(0.0f), _fy(0.0f),
    _cx(0.0f), _cy(0.0f),
    _k1(0.0f), _k2(0.0f), _p1(0.0f), _p2(0.0f), _k3(0.0f),
    _faceCascade(NULL),
    _faceModel(NULL),
    _filter(Filter_Double_Exponential),
    _kalmanFilter(NULL),
    _despFilter(NULL),
    _headPosition { 0.0f, 0.0f, 0.5f },
    _headOrientation { 0.0f, 0.0f, 0.0f, 0.0f }
{
}

WebcamHeadTracker::~WebcamHeadTracker()
{
    delete _capture;
    delete _frame;
    delete _faceCascade;
    delete _faceModel;
    delete _kalmanFilter;
    delete _despFilter;
}

bool WebcamHeadTracker::initWebcam()
{
    if (_capture)
        return isReady();
    _capture = new cv::VideoCapture(0);
    if (_capture && _capture->isOpened()) {
        _capture->set(cv::CAP_PROP_FRAME_WIDTH, 640);
        _capture->set(cv::CAP_PROP_FRAME_HEIGHT, 480);
        _frame = new cv::Mat;
        _w = _capture->get(cv::CAP_PROP_FRAME_WIDTH);
        _h = _capture->get(cv::CAP_PROP_FRAME_HEIGHT);
        _fps = _capture->get(cv::CAP_PROP_FPS);
        if (_fps <= 0.0f)
            _fps = 30.0f;
        const char* intrinsics;
        float fx, fy, cx, cy;
        if ((intrinsics = std::getenv("WEBCAM_INTRINSIC_PARAMETERS"))
                && std::sscanf(intrinsics, "%g,%g,%g,%g", &fx, &fy, &cx, &cy) == 4) {
            _fx = fx;
            _fy = fy;
            _cx = cx;
            _cy = cy;
        } else {
            _fx = 0.9f * _w;
            _fy = _fx;
            _cx = _w / 2.0f;
            _cy = _h / 2.0f;
        }
        const char* distCoeffs;
        float k1, k2, p1, p2, k3;
        if ((distCoeffs = std::getenv("WEBCAM_DISTORTION_COEFFICIENTS"))
                && std::sscanf(distCoeffs, "%g,%g,%g,%g,%g", &k1, &k2, &p1, &p2, &k3) == 5) {
            _k1 = k1;
            _k2 = k2;
            _p1 = p1;
            _p2 = p2;
            _k3 = k3;
        }
        return true;
    } else {
        return false;
    }
}

#define STRINGIFY(s) STRINGIFY_HELPER(s)
#define STRINGIFY_HELPER(s) #s

const char* WebcamHeadTracker::filePathFrontalFaceXml()
{
#ifdef HAARCASCADE_FRONTALFACE_ALT_XML
    static const char s[] = STRINGIFY(HAARCASCADE_FRONTALFACE_ALT_XML);
#else
    static const char s[] = "";
#endif
    return s;
}

const char* WebcamHeadTracker::filePathFaceLandmarksDat()
{
#ifdef SHAPE_PREDICTOR_68_FACE_LANDMARKS_DAT
    static const char s[] = STRINGIFY(SHAPE_PREDICTOR_68_FACE_LANDMARKS_DAT);
#else
    static const char s[] = "";
#endif
    return s;
}

bool WebcamHeadTracker::initPoseEstimator(const char* frontalFaceXml, const char* faceLandmarksDat)
{
    if (isReady())
        return true;

    _faceCascade = new cv::CascadeClassifier;
    if (!_faceCascade->load(frontalFaceXml)) {
        delete _faceCascade;
        _faceCascade = NULL;
        return false;
    }

    _faceModel = new dlib::shape_predictor;
    try {
        dlib::deserialize(faceLandmarksDat) >> *_faceModel;
    }
    catch (std::exception& e) {
        delete _faceCascade;
        _faceCascade = NULL;
        delete _faceModel;
        _faceModel = NULL;
        return false;
    }

    // See http://docs.opencv.org/trunk/dc/d2c/tutorial_real_time_pose.html
    // for information on this!
    _kalmanFilter = new cv::KalmanFilter;
    _kalmanFilter->init(18, 6, 0, CV_64F);
    cv::setIdentity(_kalmanFilter->processNoiseCov, cv::Scalar::all(1e-3));
    cv::setIdentity(_kalmanFilter->measurementNoiseCov, cv::Scalar::all(1e-1));
    cv::setIdentity(_kalmanFilter->errorCovPost, cv::Scalar::all(1));
    float dt = 1.0f / _fps;
    _kalmanFilter->transitionMatrix.at<double>(0, 3) = dt;
    _kalmanFilter->transitionMatrix.at<double>(1, 4) = dt;
    _kalmanFilter->transitionMatrix.at<double>(2, 5) = dt;
    _kalmanFilter->transitionMatrix.at<double>(3, 6) = dt;
    _kalmanFilter->transitionMatrix.at<double>(4, 7) = dt;
    _kalmanFilter->transitionMatrix.at<double>(5, 8) = dt;
    _kalmanFilter->transitionMatrix.at<double>(0, 6) = 0.5f * dt * dt;
    _kalmanFilter->transitionMatrix.at<double>(1, 7) = 0.5f * dt * dt;
    _kalmanFilter->transitionMatrix.at<double>(2, 8) = 0.5f * dt * dt;
    _kalmanFilter->transitionMatrix.at<double>(9, 12) = dt;
    _kalmanFilter->transitionMatrix.at<double>(10, 13) = dt;
    _kalmanFilter->transitionMatrix.at<double>(11, 14) = dt;
    _kalmanFilter->transitionMatrix.at<double>(12, 15) = dt;
    _kalmanFilter->transitionMatrix.at<double>(13, 16) = dt;
    _kalmanFilter->transitionMatrix.at<double>(14, 17) = dt;
    _kalmanFilter->transitionMatrix.at<double>(9, 15) = 0.5f * dt * dt;
    _kalmanFilter->transitionMatrix.at<double>(10, 16) = 0.5f * dt * dt;
    _kalmanFilter->transitionMatrix.at<double>(11, 17) = 0.5f * dt * dt;
    _kalmanFilter->measurementMatrix.at<double>(0, 0) = 1;
    _kalmanFilter->measurementMatrix.at<double>(1, 1) = 1;
    _kalmanFilter->measurementMatrix.at<double>(2, 2) = 1;
    _kalmanFilter->measurementMatrix.at<double>(3, 9) = 1;
    _kalmanFilter->measurementMatrix.at<double>(4, 10) = 1;
    _kalmanFilter->measurementMatrix.at<double>(5, 11) = 1;

    _despFilter = new DoubleExponentialSmoothing;

    _isReady = true;
    return true;
}

void WebcamHeadTracker::setFocalLengthsInPixels(float fx, float fy)
{
    _fx = fx;
    _fy = fy;
}

void WebcamHeadTracker::setPrincipalPointInPixels(float cx, float cy)
{
    _cx = cx;
    _cy = cy;
}

void WebcamHeadTracker::setDistortionCoefficients(float k1, float k2, float p1, float p2, float k3)
{
    _k1 = k1;
    _k2 = k2;
    _p1 = p1;
    _p2 = p2;
    _k3 = k3;
}

void WebcamHeadTracker::setFilter(enum Filter filter)
{
    _filter = filter;
}

void WebcamHeadTracker::getNewFrame()
{
    timer t0, t1;
    t0.setNow();
    *_capture >> *_frame;
    t1.setNow();
    if (_debugOptions & Debug_Timing) {
        fprintf(stderr, "WHT: acquiring webcam frame:  %4.1f ms\n", duration(t0, t1));
    }
}

static void rodriguesToQuaternion(const double* r, double* q)
{
    // Note: the OpenCV use of Rodrigues rotation vectors seems to differ from
    // other uses. They multiply the unit rotation axis with the rotation angle
    // instead of with the tangens of half the angle. This is all crap. Everyone
    // should always use quaternions ;)
    double angle = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]);
    double axis[3] = { r[0] / angle, r[1] / angle, r[2] / angle};
    double sinHalfAngle = std::sin(angle / 2.0f);
    q[0] = axis[0] * sinHalfAngle;
    q[1] = axis[1] * sinHalfAngle;
    q[2] = axis[2] * sinHalfAngle;
    q[3] = std::cos(angle / 2.0f);
}

static void quaternionToRodrigues(const double* q, double* r)
{
    double halfAngle = std::acos(q[3]);
    double sinHalfAngle = std::sin(halfAngle);
    double angle = halfAngle * 2.0;
    double factor = angle / sinHalfAngle;
    r[0] = factor * q[0];
    r[1] = factor * q[1];
    r[2] = factor * q[2];
}

static void quaternionToEuler(const double* q, double* euler)
{
    double singularityTest = q[0] * q[1] + q[2] * q[3];
    if (singularityTest > 0.4999) {
        // north pole
        euler[0] = 2.0 * std::atan2(q[0], q[3]);
        euler[1] = M_PI_2;
        euler[2] = 0.0;
    } else if (singularityTest < -0.4999) {
        // south pole
        euler[0] = -2.0 * std::atan2(q[0], q[3]);
        euler[1] = -M_PI_2;
        euler[2] = 0.0f;
    } else {
        euler[0] = std::atan2(2.0 * (q[3] * q[0] + q[1] * q[2]), 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1]));
        euler[1] = std::asin(2.0 * (q[3] * q[1] - q[0] * q[2]));
        euler[2] = std::atan2(2.0 * (q[3] * q[2] + q[0] * q[1]), 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]));
    }
}

static void eulerToQuaternion(const double* euler, double* q)
{
    double x2 = euler[0] / 2.0;
    double y2 = euler[1] / 2.0;
    double z2 = euler[2] / 2.0;
    double sx2 = std::sin(x2);
    double cx2 = std::cos(x2);
    double sy2 = std::sin(y2);
    double cy2 = std::cos(y2);
    double sz2 = std::sin(z2);
    double cz2 = std::cos(z2);
    q[0] = sx2 * cy2 * cz2 - cx2 * sy2 * sz2;
    q[1] = cx2 * sy2 * cz2 + sx2 * cy2 * sz2;
    q[2] = cx2 * cy2 * sz2 - sx2 * sy2 * cz2;
    q[3] = cx2 * cy2 * cz2 + sx2 * sy2 * sz2;
}

bool WebcamHeadTracker::computeHeadPose()
{
    if (!_faceCascade)
        return false;

    timer t0, t1, t2, t3, t4;

    /* Face detection */
    t0.setNow();
    const int minFaceSize = 80;
    std::vector<cv::Rect> faces;
    _faceCascade->detectMultiScale(*_frame, faces, 1.1, 2,
            cv::CASCADE_SCALE_IMAGE | cv::CASCADE_FIND_BIGGEST_OBJECT,
            cv::Size(minFaceSize, minFaceSize));
    if (faces.size() < 1)
        return false;
    cv::Rect faceRect = faces[0];
    t1.setNow();

    /* Face landmark detection */
#if 1
    // A temporary workaround for a Dlib/OpenCV incompatibility:
    IplImage iplImg = cvIplImage(*_frame);
    dlib::cv_image<dlib::bgr_pixel> dlibFrame(&iplImg);
#else
    dlib::cv_image<dlib::bgr_pixel> dlibFrame(*_frame); // does not copy data
#endif
    dlib::rectangle dlibRect(faceRect.x, faceRect.y,
            faceRect.x + faceRect.width - 1, faceRect.y + faceRect.height - 1);
    dlib::full_object_detection shape = (*_faceModel)(dlibFrame, dlibRect);
    if (shape.num_parts() != 68)
        return false;
    std::vector<cv::Point2f> landmarks;
    landmarks.resize(68);
    for (int i = 0; i < 68; i++) {
        dlib::point p = shape.part(i);
        landmarks[i] = cv::Point(p.x(), p.y());
    }
    t2.setNow();

    /* Extract a subset of landmarks for which we have good guesses for
     * average positions from <https://en.wikipedia.org/wiki/Human_head>.
     * Everything must be in mm to be consistent with OpenCV assumptions.
     * We use landmarks that tend not to change too much with varying
     * facial expressions. */
    const cv::Point3f landmarkLeftEctocanthi  (-60.0f,   0.0f,   0.0f);
    const cv::Point3f landmarkRightEctocanthi (+60.0f,   0.0f,   0.0f);
    const cv::Point3f landmarkSellion         (  0.0f,   5.0f, -20.0f);
    const cv::Point3f landmarkSubnasale       (  0.0f, -42.0f, -30.0f);
    const cv::Point3f landmarkStomion         (  0.0f, -67.0f, -32.0f);
    const cv::Point3f landmarkLeftTragion     (-70.0f,   0.0f,  99.9f);
    const cv::Point3f landmarkRightTragion    (+70.0f,   0.0f,  99.9f);
    const std::vector<cv::Point3f> modelLandmarks( {
            landmarkLeftEctocanthi,
            landmarkLeftEctocanthi,
            landmarkRightEctocanthi,
            landmarkRightEctocanthi,
            landmarkSellion,
            landmarkSubnasale,
            landmarkStomion,
            landmarkLeftTragion,
            landmarkRightTragion
            });
    const int landmarkLeftEctocanthiIndex = 36;
    const int landmarkRightEctocanthiIndex = 45;
    const int landmarkSellionIndex = 27;
    const int landmarkSubnasaleIndex = 33;
    const int landmarkStomionIndex = 51;
    const int landmarkLeftTragionIndex = 0;
    const int landmarkRightTragionIndex = 16;
    const std::vector<cv::Point2f> imageLandmarks( {
            landmarks[landmarkLeftEctocanthiIndex],
            landmarks[landmarkLeftEctocanthiIndex],
            landmarks[landmarkRightEctocanthiIndex],
            landmarks[landmarkRightEctocanthiIndex],
            landmarks[landmarkSellionIndex],
            landmarks[landmarkSubnasaleIndex],
            landmarks[landmarkStomionIndex],
            landmarks[landmarkLeftTragionIndex],
            landmarks[landmarkRightTragionIndex]
            });
    cv::Matx33f cameraMatrix;
    cameraMatrix(0, 0) = _fx;
    cameraMatrix(0, 1) = 0.0f;
    cameraMatrix(0, 2) = _cx;
    cameraMatrix(1, 0) = 0.0f;
    cameraMatrix(1, 1) = _fy;
    cameraMatrix(1, 2) = _cy;
    cameraMatrix(2, 0) = 0.0f;
    cameraMatrix(2, 1) = 0.0f;
    cameraMatrix(2, 2) = 1.0f;
    cv::Mat distCoeffs(1, 5, CV_32F);
    distCoeffs.at<float>(0) = _k1;
    distCoeffs.at<float>(1) = _k2;
    distCoeffs.at<float>(2) = _p1;
    distCoeffs.at<float>(3) = _p2;
    distCoeffs.at<float>(4) = _k3;
    cv::Mat rvec(1, 3, CV_64F);
    rvec.at<double>(0) = M_PI; // 180 deg around x axis: null rotation in OpenCV orientation
    rvec.at<double>(1) = 0.0f;
    rvec.at<double>(2) = 0.0f;
    cv::Mat tvec(1, 3, CV_64F);
    tvec.at<double>(0) = 0.0f;
    tvec.at<double>(1) = 0.0f;
    tvec.at<double>(2) = 500.0f;
    // in my tests, using the CV_P3P solver with 4 points was less stable than using the iterative solver with 7
    //cv::solvePnP(modelLandmarks, imageLandmarks, cameraMatrix, distCoeffs, rvec, tvec, false, CV_P3P);
    cv::solvePnP(modelLandmarks, imageLandmarks, cameraMatrix, distCoeffs, rvec, tvec, true, cv::SOLVEPNP_ITERATIVE);
    double observedVec[3] = { tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2) };
    double observedQuat[4];
    rodriguesToQuaternion(&(rvec.at<double>(0)), observedQuat);
    t3.setNow();

    /* Feed the new measurement to the filter and save result */
    double estimatedVec[3] = { 0, 0, 0 };
    double estimatedQuat[4] = { 0, 0, 0, 0 };
    switch (_filter) {
    case Filter_None:
        {
            estimatedVec[0] = observedVec[0];
            estimatedVec[1] = observedVec[1];
            estimatedVec[2] = observedVec[2];
            estimatedQuat[0] = observedQuat[0];
            estimatedQuat[1] = observedQuat[1];
            estimatedQuat[2] = observedQuat[2];
            estimatedQuat[3] = observedQuat[3];
        }
        break;
    case Filter_Kalman:
        {
            double observedEulerAngles[3];
            quaternionToEuler(observedQuat, observedEulerAngles);
            // See http://docs.opencv.org/trunk/dc/d2c/tutorial_real_time_pose.html
            // for information on this!
            cv::Mat measurement(6, 1, CV_64F);
            measurement.at<double>(0) = observedVec[0];
            measurement.at<double>(1) = observedVec[1];
            measurement.at<double>(2) = observedVec[2];
            measurement.at<double>(3) = observedEulerAngles[0];
            measurement.at<double>(4) = observedEulerAngles[1];
            measurement.at<double>(5) = observedEulerAngles[2];
            cv::Mat prediction = _kalmanFilter->predict();
            cv::Mat estimation = _kalmanFilter->correct(measurement);
            estimatedVec[0] = estimation.at<double>(0);
            estimatedVec[1] = estimation.at<double>(1);
            estimatedVec[2] = estimation.at<double>(2);
            eulerToQuaternion(&(estimation.at<double>(9)), estimatedQuat);
        }
        break;
    case Filter_Double_Exponential:
        {
            _despFilter->step(observedVec, observedQuat,
                    0.2, 0.7,
                    estimatedVec, estimatedQuat);
        }
        break;
    }
    t4.setNow();

    /* Convert the internal representation to the external representation */
    // convert position
    _headPosition[0] = -estimatedVec[0] / 1000.0;
    _headPosition[1] = -estimatedVec[1] / 1000.0;
    _headPosition[2] =  estimatedVec[2] / 1000.0;
    // convert orientation (rotate 180 deg around x)
    _headOrientation[0] =  estimatedQuat[3];
    _headOrientation[1] = -estimatedQuat[2];
    _headOrientation[2] =  estimatedQuat[1];
    _headOrientation[3] = -estimatedQuat[0];

    /* Debug output */
    if (_debugOptions & Debug_Timing) {
        fprintf(stderr, "WHT: face detection:          %4.1f ms\n", duration(t0, t1));
        fprintf(stderr, "WHT: face landmark detection: %4.1f ms\n", duration(t1, t2));
        fprintf(stderr, "WHT: face model matching:     %4.1f ms\n", duration(t2, t3));
        fprintf(stderr, "WHT: filtering:               %4.1f ms\n", duration(t4, t3));
    }
    if (_debugOptions & Debug_Window) {
        // render face rectangle
        cv::rectangle(*_frame, faceRect, cv::Scalar(0, 0, 255));
        // render face model
        for (int i = 1; i <= 16; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 18; i <= 21; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 23; i <= 26; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 28; i <= 30; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 31; i <= 35; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        cv::line(*_frame, landmarks[30], landmarks[35], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 37; i <= 41; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        cv::line(*_frame, landmarks[36], landmarks[41], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 43; i <= 47; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        cv::line(*_frame, landmarks[42], landmarks[47], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 49; i <= 59; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        cv::line(*_frame, landmarks[48], landmarks[49], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 61; i <= 67; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        cv::line(*_frame, landmarks[60], landmarks[67], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 0; i < 68; i++)
            cv::circle(*_frame, landmarks[i], 2.5f, cv::Scalar(0, 0, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkLeftEctocanthiIndex],  3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkRightEctocanthiIndex], 3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkSellionIndex],         3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkSubnasaleIndex],       3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkStomionIndex],         3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkLeftTragionIndex],     3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkRightTragionIndex],    3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        // render projected face model landmarks
        std::vector<cv::Point2f> projectedModelLandmarks;
        cv::projectPoints(modelLandmarks, rvec, tvec, cameraMatrix, distCoeffs, projectedModelLandmarks);
        cv::line(*_frame, projectedModelLandmarks[7], projectedModelLandmarks[0], cv::Scalar(255, 0, 0));
        cv::line(*_frame, projectedModelLandmarks[0], projectedModelLandmarks[4], cv::Scalar(255, 0, 0));
        cv::line(*_frame, projectedModelLandmarks[4], projectedModelLandmarks[2], cv::Scalar(255, 0, 0));
        cv::line(*_frame, projectedModelLandmarks[2], projectedModelLandmarks[8], cv::Scalar(255, 0, 0));
        cv::line(*_frame, projectedModelLandmarks[4], projectedModelLandmarks[5], cv::Scalar(255, 0, 0));
        cv::line(*_frame, projectedModelLandmarks[5], projectedModelLandmarks[6], cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[0], 3.0f, cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[2], 3.0f, cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[4], 3.0f, cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[5], 3.0f, cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[6], 3.0f, cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[7], 3.0f, cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[8], 3.0f, cv::Scalar(255, 0, 0));
        // render projected filtered model
        std::vector<cv::Point2f> projectedFilteredModelLandmarks;
        tvec.at<double>(0) = estimatedVec[0];
        tvec.at<double>(1) = estimatedVec[1];
        tvec.at<double>(2) = estimatedVec[2];
        quaternionToRodrigues(estimatedQuat, &(rvec.at<double>(0)));
        cv::projectPoints(modelLandmarks, rvec, tvec, cameraMatrix, distCoeffs, projectedFilteredModelLandmarks);
        cv::line(*_frame, projectedFilteredModelLandmarks[7], projectedFilteredModelLandmarks[0], cv::Scalar(255, 255, 0));
        cv::line(*_frame, projectedFilteredModelLandmarks[0], projectedFilteredModelLandmarks[4], cv::Scalar(255, 255, 0));
        cv::line(*_frame, projectedFilteredModelLandmarks[4], projectedFilteredModelLandmarks[2], cv::Scalar(255, 255, 0));
        cv::line(*_frame, projectedFilteredModelLandmarks[2], projectedFilteredModelLandmarks[8], cv::Scalar(255, 255, 0));
        cv::line(*_frame, projectedFilteredModelLandmarks[4], projectedFilteredModelLandmarks[5], cv::Scalar(255, 255, 0));
        cv::line(*_frame, projectedFilteredModelLandmarks[5], projectedFilteredModelLandmarks[6], cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[0], 3.0f, cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[2], 3.0f, cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[4], 3.0f, cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[5], 3.0f, cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[6], 3.0f, cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[7], 3.0f, cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[8], 3.0f, cv::Scalar(255, 255, 0));
        // show
        cv::imshow("webcam head tracker", *_frame);
        int key = cv::waitKey(1);
        if (key == 27 || key == 'q')
            _isReady = false;
        if (key == 'f')
            _filter = (_filter == Filter_None ? Filter_Kalman
                    : _filter == Filter_Kalman ? Filter_Double_Exponential
                    : Filter_None);
    }
    return true;
}

void WebcamHeadTracker::getHeadPosition(float* headPosition) const
{
    headPosition[0] = _headPosition[0];
    headPosition[1] = _headPosition[1];
    headPosition[2] = _headPosition[2];
}

void WebcamHeadTracker::getHeadOrientation(float* headOrientation) const
{
    headOrientation[0] = _headOrientation[0];
    headOrientation[1] = _headOrientation[1];
    headOrientation[2] = _headOrientation[2];
    headOrientation[3] = _headOrientation[3];
}