UInt32 RigidBodyShapeCollection::MOSHFIT(std::vector<TimeSequence*>& output, double rateHz, double distribution_tolerance, double convergence_accuracy, const BinMemFactory& memfactory /*=BinMemFactoryDefault()*/) const
{
    // compute mean shape
    RigidBodyShape mean;
    UInt32 status = ComputeMeanShape(mean, distribution_tolerance, convergence_accuracy);
    if (status != RigidBodyResult::success)
        return status;

    // build output arrays and iterators
    std::vector<TSOccVector3Iter> outputiter;
    size_t num_shapes = NumShapes();
    size_t num_markers = shape[0].NumMarkers();
    TimeRange tr;
    tr.Frames = num_shapes;
    tr.Start = 0.0;
    tr.Rate = rateHz;
    size_t imarker;
    for (imarker = 0; imarker < num_markers; imarker++)
    {
        TimeSequence* ts = TSFactoryOccValue(3).New(tr, memfactory);
        output.push_back(ts);
        outputiter.push_back( TSOccVector3Iter(*ts) );
    }

    // iterate over all shapes
    for (std::vector<RigidBodyShape>::const_iterator ishape( shape.begin() ); ishape != shape.end(); ishape++)
    {
        // compute fit of mean to this shape
        RigidTransform3 T;
        mean.ComputeFitTo(T, *ishape);

        // transform mean and use result
        for (imarker = 0; imarker < num_markers; imarker++)
        {
            RigidTransform3::MulVec(outputiter[imarker].Value(), T, mean.Marker(imarker).position);
            outputiter[imarker].Occluded() = 0;
            outputiter[imarker].Next();
        }
    }

    return RigidBodyResult::success;
}
Beispiel #2
0
    void EnsembleForce::ComputeEnsembleForce(ParticleSubjectArray& shapes) {
        if (shapes.size() < 2) {
            return;
        }
        const int nPoints = shapes[0].GetNumberOfPoints();
        const int nSubjects = shapes.size();

        // Compute offset for origin-centered shapes
        VNLMatrix centers(nSubjects, 4);
        centers.fill(0);
        for (int i = 0; i < nSubjects; i++) {
            ParticleSubject& subject = shapes[i];
            fordim(k) {
                for (int j = 0; j < nPoints; j++) {
                    Particle& par = subject[j];
                    centers[i][k] += par.x[k];
                }
                centers[i][k] /= nPoints;
                for (int j = 0; j < nPoints; j++) {
                    Particle& par = subject[j];
                    par.x[k] -= centers[i][k];
                }
            }
        }
        
        // Compute xMeanShape
        ComputeMeanShape(shapes);
        for (int i = 0; i < shapes.size(); i++) {
            ParticleBSpline bspline;
            bspline.SetReferenceImage(m_ImageContext->GetLabel(i));
            bspline.EstimateTransform(shapes[i], m_MeanShape);
            FieldTransformType::Pointer fieldTransform = bspline.GetTransform();
            shapes[i].TransformX2Y(fieldTransform.GetPointer());
            CompositeTransformType::Pointer transform = CompositeTransformType::New();
            transform->AddTransform(fieldTransform);
            shapes[i].m_Transform = transform;
        }

        // recover coordinates of particles
        for (int i = 0; i < nSubjects; i++) {
            ParticleSubject& subject = shapes[i];
            fordim (k) {
                for (int j = 0; j < nPoints; j++) {
                    Particle& par = subject[j];
                    par.x[k] += centers[i][k];
                }
            }
        }

        // Compute yMeanShape
        fordim(k) {
            for (int j = 0; j < nPoints; j++) {
                for (int i = 0; i < nSubjects; i++) {
                    m_MeanShape[j].y[k] += shapes[i][j].y[k];
                }
                m_MeanShape[j].y[k] /= nSubjects;
            }
        }

        for (int i = 0; i < nSubjects; i++) {
            #pragma omp parallel for
            for (int j = 0; j < nPoints; j++) {
                FieldTransformType::InputPointType xPoint;
                FieldTransformType::JacobianType xJac;
                xJac.set_size(__Dim,__Dim);

                double f[__Dim] = { 0, };
                double *x = shapes[i][j].x;
                double *y = shapes[i][j].y;
                double *my = m_MeanShape[j].y;
                fordim(k) {
                    xPoint[k] = x[k];
                }
                shapes[i].m_Transform->GetNthTransform(0)->ComputeInverseJacobianWithRespectToPosition(xPoint, xJac);
//                shapes[i].m_Transform->ComputeJacobianWithRespectToPosition(xPoint, xJac);
                fordim(k) {
                    if (__Dim == 3) {
                        f[k] = xJac[0][k]*(y[0]-my[0]) + xJac[1][k]*(y[1]-my[1]) + xJac[2][k]*(y[2]-my[2]);
                    } else if (__Dim == 2) {
                        f[k] = xJac[0][k]*(y[0]-my[0]) + xJac[1][k]*(y[1]-my[1]);
                    }
                }
                shapes[i][j].SubForce(f, m_Coeff);
            }
        }
    }