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; }
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); } } }