36 #include <vtkPolyData.h> 37 #include <vtkCardinalSpline.h> 59 mBranches.push_back(b);
64 for(
int i = 0; i < mBranches.size(); i++ )
66 if (b == mBranches[i])
68 mBranches.erase(mBranches.begin() + i);
86 std::vector<int> branchNumbersToBeDeleted;
87 for(
int i = 0; i < mBranches.size(); i++ )
89 int generationCounter = 1;
91 while (currentBranch->getParentBranch()){
93 currentBranch = currentBranch->getParentBranch();
94 if (generationCounter > maxGeneration)
96 branchNumbersToBeDeleted.push_back(i);
103 for (
int i = branchNumbersToBeDeleted.size() - 1; i >= 0; i-- )
110 for (
int i = 0; i < mBranches.size(); i++)
112 Eigen::MatrixXd positions = mBranches[i]->getPositions();
113 Eigen::MatrixXd diff = positions.rightCols(positions.cols() - 1) - positions.leftCols(positions.cols() - 1);
114 Eigen::MatrixXd orientations(positions.rows(),positions.cols());
115 orientations.leftCols(orientations.cols() - 1) = diff / diff.norm();
116 orientations.rightCols(1) = orientations.col(orientations.cols() - 1);
117 mBranches[i]->setOrientations(orientations);
123 for (
int i = 0; i < mBranches.size(); i++)
125 Eigen::MatrixXd orientations = mBranches[i]->getOrientations();
126 Eigen::MatrixXd newOrientations(orientations.rows(),orientations.cols());
127 int numberOfColumns = orientations.cols();
128 for (
int j = 0; j < numberOfColumns; j++)
130 newOrientations.col(j) = orientations.block(0,std::max(j-2,0),orientations.rows(),std::min(5,numberOfColumns-j)).rowwise().mean();
131 newOrientations.col(j) = newOrientations.col(j) / newOrientations.col(j).norm();
133 mBranches[i]->setOrientations(newOrientations);
139 for (
int i = 0; i < mBranches.size(); i++)
141 Eigen::MatrixXd positions = mBranches[i]->getPositions();
142 std::vector<Eigen::Vector3d> interpolatedPositions;
143 for (
int j = 0; j < positions.cols()-1; j++)
145 for (
int k = 0; k < interpolationFactor; k++){
146 Eigen::Vector3d interpolationPoint;
147 interpolationPoint[0] = (positions(0,j)*(interpolationFactor-k) + positions(0,j+1)*(k) ) / interpolationFactor;
148 interpolationPoint[1] = (positions(1,j)*(interpolationFactor-k) + positions(1,j+1)*(k) ) / interpolationFactor;
149 interpolationPoint[2] = (positions(2,j)*(interpolationFactor-k) + positions(2,j+1)*(k) ) / interpolationFactor;
150 interpolatedPositions.push_back(interpolationPoint);
153 Eigen::MatrixXd interpolationResult(3 , interpolatedPositions.size());
154 for (
int j = 0; j < interpolatedPositions.size(); j++)
156 interpolationResult(0,j) = interpolatedPositions[j](0);
157 interpolationResult(1,j) = interpolatedPositions[j](1);
158 interpolationResult(2,j) = interpolatedPositions[j](2);
160 mBranches[i]->setPositions(interpolationResult);
167 for (
int i = 0; i < mBranches.size(); i++)
169 Eigen::MatrixXd positions = mBranches[i]->getPositions();
170 int numberOfInputPoints = positions.cols();
173 double branchLength = (positions.rightCols(1) - positions.leftCols(1)).norm();
174 int numberOfControlPoints =
std::ceil(branchLength/controlPointDistance);
175 numberOfControlPoints = std::max(numberOfControlPoints, 2);
182 for(
int j=0; j<numberOfControlPoints; j++)
184 int indexP = (j*numberOfInputPoints)/numberOfControlPoints;
186 splineX->AddPoint(indexP,positions(0,indexP));
187 splineY->AddPoint(indexP,positions(1,indexP));
188 splineZ->AddPoint(indexP,positions(2,indexP));
191 splineX->AddPoint(numberOfInputPoints-1,positions(0,numberOfInputPoints-1));
192 splineY->AddPoint(numberOfInputPoints-1,positions(1,numberOfInputPoints-1));
193 splineZ->AddPoint(numberOfInputPoints-1,positions(2,numberOfInputPoints-1));
196 Eigen::MatrixXd smoothingResult(3 , numberOfInputPoints);
197 for(
int j=0; j<numberOfInputPoints; j++)
199 double splineParameter = j;
200 smoothingResult(0,j) = splineX->Evaluate(splineParameter);
201 smoothingResult(1,j) = splineY->Evaluate(splineParameter);
202 smoothingResult(2,j) = splineZ->Evaluate(splineParameter);
204 mBranches[i]->setPositions(smoothingResult);
211 Eigen::MatrixXd positionsNotUsed_r = positions_r;
216 Eigen::MatrixXd::Index startIndex;
218 while (positionsNotUsed_r.cols() > 0)
220 if (!mBranches.empty())
222 double minDistance = 1000;
223 for (
int i = 0; i < mBranches.size(); i++)
225 std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distances;
226 distances =
dsearchn(positionsNotUsed_r, mBranches[i]->getPositions());
227 double d = distances.second.minCoeff(&index);
231 branchToSplit = mBranches[i];
237 std::pair<Eigen::MatrixXd::Index, double> dsearchResult =
dsearch(positionsNotUsed_r.col(startIndex) , branchToSplit->getPositions());
238 splitIndex = dsearchResult.first;
241 startIndex = positionsNotUsed_r.cols() - 1;
243 std::pair<Eigen::MatrixXd,Eigen::MatrixXd > connectedPointsResult =
findConnectedPointsInCT(startIndex , positionsNotUsed_r);
244 Eigen::MatrixXd branchPositions = connectedPointsResult.first;
245 positionsNotUsed_r = connectedPointsResult.second;
247 if (branchPositions.cols() >= 5)
250 newBranch->setPositions(branchPositions);
251 mBranches.push_back(newBranch);
253 if (mBranches.size() > 1)
255 if ((splitIndex + 1 >= 5) && (branchToSplit->getPositions().cols() - splitIndex - 1 >= 5))
261 Eigen::MatrixXd branchToSplitPositions = branchToSplit->getPositions();
262 newBranchFromSplit->setPositions(branchToSplitPositions.rightCols(branchToSplitPositions.cols() - splitIndex - 1));
263 branchToSplit->setPositions(branchToSplitPositions.leftCols(splitIndex + 1));
264 mBranches.push_back(newBranchFromSplit);
265 newBranchFromSplit->setParentBranch(branchToSplit);
266 newBranch->setParentBranch(branchToSplit);
267 newBranchFromSplit->setChildBranches(branchToSplit->getChildBranches());
268 branchVector branchToSplitChildren = branchToSplit->getChildBranches();
269 for (
int i = 0; i < branchToSplitChildren.size(); i++)
270 branchToSplitChildren[i]->setParentBranch(newBranchFromSplit);
271 branchToSplit->deleteChildBranches();
272 branchToSplit->addChildBranch(newBranchFromSplit);
273 branchToSplit->addChildBranch(newBranch);
275 else if (splitIndex + 1 < 5)
280 newBranch->setParentBranch(branchToSplit->getParentBranch());
281 if(branchToSplit->getParentBranch())
282 branchToSplit->getParentBranch()->addChildBranch(newBranch);
284 else if (branchToSplit->getPositions().cols() - splitIndex - 1 < 5)
288 newBranch->setParentBranch(branchToSplit);
289 branchToSplit->addChildBranch(newBranch);
303 for (
int i = 0; i < mBranches.size(); i++)
306 b->setPositions(mBranches[i]->getPositions());
307 b->setOrientations(mBranches[i]->getOrientations());
308 retval->addBranch(b);
311 std::vector<BranchPtr> branches = retval->getBranches();
312 Eigen::MatrixXd positions;
313 Eigen::MatrixXd orientations;
314 for (
int i = 0; i < branches.size(); i++)
316 positions = branches[i]->getPositions();
317 orientations = branches[i]->getOrientations();
318 std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distanceData;
319 distanceData =
dsearchn(positions, trackingPositions);
320 Eigen::VectorXd distance = distanceData.second;
321 for (
int j = positions.cols() - 1; j >= 0; j--)
323 if (distance(j) > maxDistance)
326 orientations =
eraseCol(j, orientations);
329 branches[i]->setPositions(positions);
330 branches[i]->setOrientations(orientations);
359 int positionCounter = 0;
360 for (
size_t i = 0; i < mBranches.size(); ++i)
362 Eigen::MatrixXd positions = mBranches[i]->getPositions();
366 points->InsertNextPoint(positions(0,0),positions(1,0),positions(2,0));
367 points->InsertNextPoint(positions(0,positions.cols()-1),positions(1,positions.cols()-1),positions(2,positions.cols()-1));
368 vtkIdType connection[2] = {positionCounter-1, positionCounter};
369 lines->InsertNextCell(2, connection);
374 for (
int j = 0; j < positions.cols(); ++j)
377 points->InsertNextPoint(positions(0,j),positions(1,j),positions(2,j));
378 if (j < positions.cols()-1)
380 vtkIdType connection[2] = {positionCounter-1, positionCounter};
381 lines->InsertNextCell(2, connection);
388 int this_branchs_first_point_in_full_polydata_point_list = 0;
389 for(
size_t i = 0; i < mBranches.size(); ++i)
393 if(!straightBranches)
394 this_branchs_first_point_in_full_polydata_point_list += mBranches[i-1]->getPositions().cols();
396 this_branchs_first_point_in_full_polydata_point_list += 2;
398 int parent_index_in_branch_list = mBranches[i]->findParentIndex(mBranches);
400 if(parent_index_in_branch_list > -1)
402 int parent_branch_last_point_in_full_polydata = 0;
403 for(
int j = 0; j <= parent_index_in_branch_list; ++j)
405 if(!straightBranches)
406 parent_branch_last_point_in_full_polydata += mBranches[j]->getPositions().cols() - 1;
408 parent_branch_last_point_in_full_polydata += (1 + j*2);
410 vtkIdType connection[2] = {parent_branch_last_point_in_full_polydata, this_branchs_first_point_in_full_polydata_point_list};
411 lines->InsertNextCell(2, connection);
417 retval->SetPoints(points);
418 retval->SetLines(lines);
423 Eigen::MatrixXd
sortMatrix(
int rowNumber, Eigen::MatrixXd matrix)
425 for (
int i = 0; i < matrix.cols() - 1; i++) {
426 for (
int j = i + 1; j < matrix.cols(); j++) {
427 if (matrix(rowNumber,i) > matrix(rowNumber,j)){
428 matrix.col(i).swap(matrix.col(j));
437 Eigen::MatrixXd
eraseCol(
int removeIndex, Eigen::MatrixXd positions)
439 positions.block(0 , removeIndex , positions.rows() , positions.cols() - removeIndex - 1) = positions.rightCols(positions.cols() - removeIndex - 1);
440 positions.conservativeResize(Eigen::NoChange, positions.cols() - 1);
444 std::pair<Eigen::MatrixXd::Index, double>
dsearch(Eigen::Vector3d p, Eigen::MatrixXd positions)
446 Eigen::MatrixXd::Index index;
448 (positions.colwise() - p).colwise().squaredNorm().minCoeff(&index);
449 double d = (positions.col(index) - p).norm();
451 return std::make_pair(index , d);
454 std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd >
dsearchn(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
456 Eigen::MatrixXd::Index index;
457 std::vector<Eigen::MatrixXd::Index> indexVector;
458 Eigen::VectorXd D(p1.cols());
459 for (
int i = 0; i < p1.cols(); i++)
462 (p2.colwise() - p1.col(i)).colwise().squaredNorm().minCoeff(&index);
463 D(i) = (p2.col(index) - p1.col(i)).norm();
464 indexVector.push_back(index);
466 return std::make_pair(indexVector , D);
472 Eigen::MatrixXd thisPosition(3,1);
473 std::vector<Eigen::MatrixXd> branchPositionsVector;
474 thisPosition = positionsNotUsed.col(startIndex);
475 branchPositionsVector.push_back(thisPosition);
476 positionsNotUsed =
eraseCol(startIndex,positionsNotUsed);;
478 while (positionsNotUsed.cols() > 0)
480 std::pair<Eigen::MatrixXd::Index, double > minDistance =
dsearch(thisPosition, positionsNotUsed);
481 Eigen::MatrixXd::Index index = minDistance.first;
482 double d = minDistance.second;
486 thisPosition = positionsNotUsed.col(index);
487 positionsNotUsed =
eraseCol(index,positionsNotUsed);
489 branchPositionsVector.push_back(thisPosition);
493 Eigen::MatrixXd branchPositions(3,branchPositionsVector.size());
495 for (
int j = 0; j < branchPositionsVector.size(); j++)
497 branchPositions.block(0,j,3,1) = branchPositionsVector[j];
500 return std::make_pair(branchPositions, positionsNotUsed);
Vector3D ceil(const Vector3D &a)
std::pair< Eigen::MatrixXd, Eigen::MatrixXd > findConnectedPointsInCT(int startIndex, Eigen::MatrixXd positionsNotUsed)
void interpolateBranchPositions(int interpolationFactor)
vtkSmartPointer< class vtkCellArray > vtkCellArrayPtr
boost::shared_ptr< class BranchList > BranchListPtr
BranchListPtr removePositionsForLocalRegistration(Eigen::MatrixXd trackingPositions, double maxDistance)
vtkSmartPointer< vtkPoints > vtkPointsPtr
void addBranch(BranchPtr b)
boost::shared_ptr< class Branch > BranchPtr
vtkSmartPointer< class vtkCardinalSpline > vtkCardinalSplinePtr
vtkSmartPointer< class vtkCardinalSpline > vtkCardinalSplinePtr
vtkPolyDataPtr createVtkPolyDataFromBranches(bool fullyConnected=false, bool straightBranches=false) const
BranchList::createVtkPolyDataFromBranches Return a VtkPolyData object created from the branches in th...
std::pair< Eigen::MatrixXd::Index, double > dsearch(Eigen::Vector3d p, Eigen::MatrixXd positions)
void calculateOrientations()
void smoothBranchPositions(int controlPointDistance)
std::vector< BranchPtr > getBranches()
void deleteBranch(BranchPtr b)
vtkSmartPointer< vtkPolyData > vtkPolyDataPtr
void findBranchesInCenterline(Eigen::MatrixXd positions_r)
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
Namespace for all CustusX production code.