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