void BranchList::findBranchesInCenterline(Eigen::MatrixXd positions) { positions = sortMatrix(2,positions); Eigen::MatrixXd positionsNotUsed = positions; // int minIndex; int index; int splitIndex; Eigen::MatrixXd::Index startIndex; BranchPtr branchToSplit; while (positionsNotUsed.cols() > 0) { if (!mBranches.empty()) { double minDistance = 1000; for (int i = 0; i < mBranches.size(); i++) { std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distances; distances = dsearchn(positionsNotUsed, mBranches[i]->getPositions()); double d = distances.second.minCoeff(&index); if (d < minDistance) { minDistance = d; branchToSplit = mBranches[i]; startIndex = index; if (minDistance < 2) break; } } std::pair<Eigen::MatrixXd::Index, double> dsearchResult = dsearch(positionsNotUsed.col(startIndex) , branchToSplit->getPositions()); splitIndex = dsearchResult.first; } else //if this is the first branch. Select the top position (Trachea). startIndex = positionsNotUsed.cols() - 1; std::pair<Eigen::MatrixXd,Eigen::MatrixXd > connectedPointsResult = findConnectedPointsInCT(startIndex , positionsNotUsed); Eigen::MatrixXd branchPositions = connectedPointsResult.first; positionsNotUsed = connectedPointsResult.second; if (branchPositions.cols() >= 5) //only include brances of length >= 5 points { BranchPtr newBranch = BranchPtr(new Branch()); newBranch->setPositions(branchPositions); mBranches.push_back(newBranch); if (mBranches.size() > 1) // do not try to split another branch when the first branch is processed { if ((splitIndex + 1 >= 5) && (branchToSplit->getPositions().cols() - splitIndex - 1 >= 5)) //do not split branch if the new branch is close to the edge of the branch //if the new branch is not close to one of the edges of the //connected existing branch: Split the existing branch { BranchPtr newBranchFromSplit = BranchPtr(new Branch()); Eigen::MatrixXd branchToSplitPositions = branchToSplit->getPositions(); newBranchFromSplit->setPositions(branchToSplitPositions.rightCols(branchToSplitPositions.cols() - splitIndex - 1)); branchToSplit->setPositions(branchToSplitPositions.leftCols(splitIndex + 1)); mBranches.push_back(newBranchFromSplit); newBranchFromSplit->setParentBranch(branchToSplit); newBranch->setParentBranch(branchToSplit); newBranchFromSplit->setChildBranches(branchToSplit->getChildBranches()); branchVector branchToSplitChildren = branchToSplit->getChildBranches(); for (int i = 0; i < branchToSplitChildren.size(); i++) branchToSplitChildren[i]->setParentBranch(newBranchFromSplit); branchToSplit->deleteChildBranches(); branchToSplit->addChildBranch(newBranchFromSplit); branchToSplit->addChildBranch(newBranch); } else if (splitIndex + 1 < 5) // If the new branch is close to the start of the existing // branch: Connect it to the same position start as the // existing branch { newBranch->setParentBranch(branchToSplit->getParentBranch()); branchToSplit->getParentBranch()->addChildBranch(newBranch); } else if (branchToSplit->getPositions().cols() - splitIndex - 1 < 5) // If the new branch is close to the end of the existing // branch: Connect it to the end of the existing branch { newBranch->setParentBranch(branchToSplit); branchToSplit->addChildBranch(newBranch); } } } } }
TEST(Skeleton, Referential) { std::vector<SkeletonPtr> skeletons = getSkeletons(); #ifndef NDEBUG // Debug mode size_t numIterations = 1; #else // Release mode size_t numIterations = 20; #endif for(size_t i=0; i<skeletons.size(); ++i) { SkeletonPtr skeleton = skeletons[i]; for(size_t j=0; j<skeleton->getNumTrees(); ++j) { BranchPtr tree = Branch::create(skeleton->getRootBodyNode(j)); const std::vector<BodyNode*>& skelBns = skeleton->getTreeBodyNodes(j); EXPECT_TRUE(tree->getNumBodyNodes() == skelBns.size()); for(BodyNode* bn : skelBns) { EXPECT_FALSE(tree->getIndexOf(bn) == INVALID_INDEX); EXPECT_TRUE(tree->getBodyNode(tree->getIndexOf(bn)) == bn); } const std::vector<DegreeOfFreedom*>& skelDofs = skeleton->getTreeDofs(j); EXPECT_TRUE(tree->getNumDofs() == skelDofs.size()); for(DegreeOfFreedom* dof : skelDofs) { EXPECT_FALSE(tree->getIndexOf(dof) == INVALID_INDEX); EXPECT_TRUE(tree->getDof(tree->getIndexOf(dof)) == dof); } Eigen::VectorXd q = tree->getPositions(); Eigen::VectorXd dq = tree->getVelocities(); Eigen::VectorXd ddq = tree->getAccelerations(); for(size_t k=0; k<numIterations; ++k) { for(int r=0; r<q.size(); ++r) { q[r] = math::random(-10, 10); dq[r] = math::random(-10, 10); ddq[r] = math::random(-10, 10); } tree->setPositions(q); tree->setVelocities(dq); tree->setAccelerations(ddq); EXPECT_TRUE( equals(q, tree->getPositions(), 0.0) ); EXPECT_TRUE( equals(dq, tree->getVelocities(), 0.0) ); EXPECT_TRUE( equals(ddq, tree->getAccelerations(), 0.0) ); const Eigen::MatrixXd& skelMassMatrix = skeleton->getMassMatrix(); const Eigen::MatrixXd& treeMassMatrix = tree->getMassMatrix(); const Eigen::MatrixXd& skelAugM = skeleton->getAugMassMatrix(); const Eigen::MatrixXd& treeAugM = tree->getAugMassMatrix(); const Eigen::MatrixXd& skelInvM = skeleton->getInvMassMatrix(); const Eigen::MatrixXd& treeInvM = tree->getInvMassMatrix(); const Eigen::MatrixXd& skelInvAugM = skeleton->getInvAugMassMatrix(); const Eigen::MatrixXd& treeInvAugM = tree->getInvAugMassMatrix(); const Eigen::VectorXd& skelCvec = skeleton->getCoriolisForces(); const Eigen::VectorXd& treeCvec = tree->getCoriolisForces(); const Eigen::VectorXd& skelFg = skeleton->getGravityForces(); const Eigen::VectorXd& treeFg = tree->getGravityForces(); const Eigen::VectorXd& skelCg = skeleton->getCoriolisAndGravityForces(); const Eigen::VectorXd& treeCg = tree->getCoriolisAndGravityForces(); const Eigen::VectorXd& skelFext = skeleton->getExternalForces(); const Eigen::VectorXd& treeFext = tree->getExternalForces(); const Eigen::VectorXd& skelFc = skeleton->getConstraintForces(); const Eigen::VectorXd& treeFc = tree->getConstraintForces(); const size_t nDofs = tree->getNumDofs(); for(size_t r1=0; r1<nDofs; ++r1) { const size_t sr1 = tree->getDof(r1)->getIndexInSkeleton(); for(size_t r2=0; r2<nDofs; ++r2) { const size_t sr2 = tree->getDof(r2)->getIndexInSkeleton(); EXPECT_TRUE( skelMassMatrix(sr1,sr2) == treeMassMatrix(r1,r2) ); EXPECT_TRUE( skelAugM(sr1,sr2) == treeAugM(r1,r2) ); EXPECT_TRUE( skelInvM(sr1,sr2) == treeInvM(r1,r2) ); EXPECT_TRUE( skelInvAugM(sr1,sr2) == treeInvAugM(r1,r2) ); } EXPECT_TRUE( skelCvec[sr1] == treeCvec[r1] ); EXPECT_TRUE( skelFg[sr1] == treeFg[r1] ); EXPECT_TRUE( skelCg[sr1] == treeCg[r1] ); EXPECT_TRUE( skelFext[sr1] == treeFext[r1] ); EXPECT_TRUE( skelFext[sr1] == treeFext[r1] ); EXPECT_TRUE( skelFc[sr1] == treeFc[r1] ); } const size_t numBodyNodes = tree->getNumBodyNodes(); for(size_t m=0; m<numBodyNodes; ++m) { const BodyNode* bn = tree->getBodyNode(m); const Eigen::MatrixXd Jtree = tree->getJacobian(bn); const Eigen::MatrixXd Jskel = skeleton->getJacobian(bn); for(size_t r2=0; r2<nDofs; ++r2) { const size_t sr2 = tree->getDof(r2)->getIndexInSkeleton(); for(size_t r1=0; r1<6; ++r1) { EXPECT_TRUE( Jtree(r1,r2) == Jskel(r1, sr2) ); } } } } } } }