void bug_1225()
{
Matrix3d m1, m2;
m1.setRandom();
m1 = m1*m1.transpose();
m2 = m1.triangularView<Upper>();
SelfAdjointEigenSolver<Matrix3d> eig1(m1);
SelfAdjointEigenSolver<Matrix3d> eig2(m2.selfadjointView<Upper>());
VERIFY_IS_APPROX(eig1.eigenvalues(), eig2.eigenvalues());
}
/**
* 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));
}
}
