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;
}
Sphere Sphere::fit_sphere(const MatrixX3f& points)
{
const VectorXf& x = points.col(0);
const VectorXf& y = points.col(1);
const VectorXf& z = points.col(2);
VectorXf point_means = points.colwise().mean();
VectorXf x_mean_free = x.array() - point_means(0);
VectorXf y_mean_free = y.array() - point_means(1);
VectorXf z_mean_free = z.array() - point_means(2);
Matrix3f A;
A << (x.cwiseProduct(x_mean_free)).mean(), 2*(x.cwiseProduct(y_mean_free)).mean(), 2*(x.cwiseProduct(z_mean_free)).mean(),
0, (y.cwiseProduct(y_mean_free)).mean(), 2*(y.cwiseProduct(z_mean_free)).mean(),
0, 0, (z.cwiseProduct(z_mean_free)).mean();
Matrix3f A_T = A.transpose();
A += A_T;
Vector3f b;
VectorXf sq_sum = x.array().pow(2)+y.array().pow(2)+z.array().pow(2);
b << (sq_sum.cwiseProduct(x_mean_free)).mean(),
(sq_sum.cwiseProduct(y_mean_free)).mean(),
(sq_sum.cwiseProduct(z_mean_free)).mean();
Vector3f center = A.ldlt().solve(b);
MatrixX3f tmp(points.rows(),3);
tmp.col(0) = x.array() - center(0);
tmp.col(1) = y.array() - center(1);
tmp.col(2) = z.array() - center(2);
float r = sqrt(tmp.array().pow(2).rowwise().sum().mean());
return Sphere(center, r);
}
