36 #include <vtkPolyData.h>
57 mBranches.push_back(b);
62 for(
int i = 0; i < mBranches.size(); i++ )
64 if (b == mBranches[i])
66 mBranches.erase(mBranches.begin() + i);
84 std::vector<int> branchNumbersToBeDeleted;
85 for(
int i = 0; i < mBranches.size(); i++ )
87 int generationCounter = 1;
89 while (currentBranch->getParentBranch()){
91 currentBranch = currentBranch->getParentBranch();
92 if (generationCounter > maxGeneration)
94 branchNumbersToBeDeleted.push_back(i);
101 for (
int i = branchNumbersToBeDeleted.size() - 1; i >= 0; i-- )
108 for (
int i = 0; i < mBranches.size(); i++)
110 Eigen::MatrixXd positions = mBranches[i]->getPositions();
111 Eigen::MatrixXd diff = positions.rightCols(positions.cols() - 1) - positions.leftCols(positions.cols() - 1);
112 Eigen::MatrixXd orientations(positions.rows(),positions.cols());
113 orientations.leftCols(orientations.cols() - 1) = diff / diff.norm();
114 orientations.rightCols(1) = orientations.col(orientations.cols() - 1);
115 mBranches[i]->setOrientations(orientations);
121 for (
int i = 0; i < mBranches.size(); i++)
123 Eigen::MatrixXd orientations = mBranches[i]->getOrientations();
124 Eigen::MatrixXd newOrientations(orientations.rows(),orientations.cols());
125 int numberOfColumns = orientations.cols();
126 for (
int j = 0; j < numberOfColumns; j++)
128 newOrientations.col(j) = orientations.block(0,std::max(j-2,0),orientations.rows(),std::min(5,numberOfColumns-j)).rowwise().mean();
129 newOrientations.col(j) = newOrientations.col(j) / newOrientations.col(j).norm();
131 mBranches[i]->setOrientations(newOrientations);
139 Eigen::MatrixXd positionsNotUsed = positions;
144 Eigen::MatrixXd::Index startIndex;
146 while (positionsNotUsed.cols() > 0)
148 if (!mBranches.empty())
150 double minDistance = 1000;
151 for (
int i = 0; i < mBranches.size(); i++)
153 std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distances;
154 distances =
dsearchn(positionsNotUsed, mBranches[i]->getPositions());
155 double d = distances.second.minCoeff(&index);
159 branchToSplit = mBranches[i];
165 std::pair<Eigen::MatrixXd::Index, double> dsearchResult =
dsearch(positionsNotUsed.col(startIndex) , branchToSplit->getPositions());
166 splitIndex = dsearchResult.first;
169 startIndex = positionsNotUsed.cols() - 1;
171 std::pair<Eigen::MatrixXd,Eigen::MatrixXd > connectedPointsResult =
findConnectedPointsInCT(startIndex , positionsNotUsed);
172 Eigen::MatrixXd branchPositions = connectedPointsResult.first;
173 positionsNotUsed = connectedPointsResult.second;
175 if (branchPositions.cols() >= 5)
178 newBranch->setPositions(branchPositions);
179 mBranches.push_back(newBranch);
181 if (mBranches.size() > 1)
183 if ((splitIndex + 1 >= 5) && (branchToSplit->getPositions().cols() - splitIndex - 1 >= 5))
189 Eigen::MatrixXd branchToSplitPositions = branchToSplit->getPositions();
190 newBranchFromSplit->setPositions(branchToSplitPositions.rightCols(branchToSplitPositions.cols() - splitIndex - 1));
191 branchToSplit->setPositions(branchToSplitPositions.leftCols(splitIndex + 1));
192 mBranches.push_back(newBranchFromSplit);
193 newBranchFromSplit->setParentBranch(branchToSplit);
194 newBranch->setParentBranch(branchToSplit);
195 newBranchFromSplit->setChildBranches(branchToSplit->getChildBranches());
196 branchVector branchToSplitChildren = branchToSplit->getChildBranches();
197 for (
int i = 0; i < branchToSplitChildren.size(); i++)
198 branchToSplitChildren[i]->setParentBranch(newBranchFromSplit);
199 branchToSplit->deleteChildBranches();
200 branchToSplit->addChildBranch(newBranchFromSplit);
201 branchToSplit->addChildBranch(newBranch);
203 else if (splitIndex + 1 < 5)
208 newBranch->setParentBranch(branchToSplit->getParentBranch());
209 branchToSplit->getParentBranch()->addChildBranch(newBranch);
211 else if (branchToSplit->getPositions().cols() - splitIndex - 1 < 5)
215 newBranch->setParentBranch(branchToSplit);
216 branchToSplit->addChildBranch(newBranch);
230 for (
int i = 0; i < mBranches.size(); i++)
233 b->setPositions(mBranches[i]->getPositions());
234 b->setOrientations(mBranches[i]->getOrientations());
235 retval->addBranch(b);
238 std::vector<BranchPtr> branches = retval->getBranches();
239 Eigen::MatrixXd positions;
240 Eigen::MatrixXd orientations;
241 for (
int i = 0; i < branches.size(); i++)
243 positions = branches[i]->getPositions();
244 orientations = branches[i]->getOrientations();
245 std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distanceData;
246 distanceData =
dsearchn(positions, trackingPositions);
247 Eigen::VectorXd distance = distanceData.second;
248 for (
int j = positions.cols() - 1; j <= 0; j--)
250 if (distance(j) > maxDistance)
253 orientations =
eraseCol(j, orientations);
256 branches[i]->setPositions(positions);
257 branches[i]->setOrientations(orientations);
262 Eigen::MatrixXd
sortMatrix(
int rowNumber, Eigen::MatrixXd matrix)
264 for (
int i = 0; i < matrix.cols() - 1; i++) {
265 for (
int j = i + 1; j < matrix.cols(); j++) {
266 if (matrix(rowNumber,i) > matrix(rowNumber,j)){
267 matrix.col(i).swap(matrix.col(j));
276 Eigen::MatrixXd
eraseCol(
int removeIndex, Eigen::MatrixXd positions)
278 positions.block(0 , removeIndex , positions.rows() , positions.cols() - removeIndex - 1) = positions.rightCols(positions.cols() - removeIndex - 1);
279 positions.conservativeResize(Eigen::NoChange, positions.cols() - 1);
283 std::pair<Eigen::MatrixXd::Index, double>
dsearch(Eigen::Vector3d p, Eigen::MatrixXd positions)
285 Eigen::MatrixXd::Index index;
287 (positions.colwise() - p).colwise().squaredNorm().minCoeff(&index);
288 double d = (positions.col(index) - p).norm();
290 return std::make_pair(index , d);
293 std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd >
dsearchn(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
295 Eigen::MatrixXd::Index index;
296 std::vector<Eigen::MatrixXd::Index> indexVector;
297 Eigen::VectorXd D(p1.cols());
298 for (
int i = 0; i < p1.cols(); i++)
301 (p2.colwise() - p1.col(i)).colwise().squaredNorm().minCoeff(&index);
302 D(i) = (p2.col(index) - p1.col(i)).norm();
303 indexVector.push_back(index);
305 return std::make_pair(indexVector , D);
311 Eigen::MatrixXd thisPosition(3,1);
312 std::vector<Eigen::MatrixXd> branchPositionsVector;
313 thisPosition = positionsNotUsed.col(startIndex);
314 branchPositionsVector.push_back(thisPosition);
315 positionsNotUsed =
eraseCol(startIndex,positionsNotUsed);;
317 while (positionsNotUsed.cols() > 0)
319 std::pair<Eigen::MatrixXd::Index, double > minDistance =
dsearch(thisPosition, positionsNotUsed);
320 Eigen::MatrixXd::Index index = minDistance.first;
321 double d = minDistance.second;
325 thisPosition = positionsNotUsed.col(index);
326 positionsNotUsed =
eraseCol(index,positionsNotUsed);
328 branchPositionsVector.push_back(thisPosition);
332 Eigen::MatrixXd branchPositions(3,branchPositionsVector.size());
334 for (
int j = 0; j < branchPositionsVector.size(); j++)
336 branchPositions.block(0,j,3,1) = branchPositionsVector[j];
339 return std::make_pair(branchPositions, positionsNotUsed);
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > findConnectedPointsInCT(int startIndex, Eigen::MatrixXd positionsNotUsed)
boost::shared_ptr< class BranchList > BranchListPtr
BranchListPtr removePositionsForLocalRegistration(Eigen::MatrixXd trackingPositions, double maxDistance)
void findBranchesInCenterline(Eigen::MatrixXd positions)
void addBranch(BranchPtr b)
boost::shared_ptr< class Branch > BranchPtr
std::pair< Eigen::MatrixXd::Index, double > dsearch(Eigen::Vector3d p, Eigen::MatrixXd positions)
void calculateOrientations()
std::vector< BranchPtr > getBranches()
void deleteBranch(BranchPtr b)
class org_custusx_registration_method_bronchoscopy_EXPORT Branch
std::pair< std::vector< Eigen::MatrixXd::Index >, Eigen::VectorXd > dsearchn(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
Eigen::MatrixXd eraseCol(int removeIndex, Eigen::MatrixXd positions)
void selectGenerations(int maxGeneration)
Eigen::MatrixXd sortMatrix(int rowNumber, Eigen::MatrixXd matrix)
void smoothOrientations()
std::vector< BranchPtr > branchVector