示例#1
0
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;
}
示例#2
0
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);
}