cxRouteToTarget.cpp

Go to the documentation of this file.

#include "cxRouteToTarget.h"
#include <vtkPolyData.h>
#include "cxBranchList.h"
#include "cxBranch.h"
#include "cxPointMetric.h"
#include <vtkCellArray.h>
#include "vtkCardinalSpline.h"
#include <QDir>
#include "cxTime.h"
#include "cxVisServices.h"
#include <QTextStream>
#include <QJsonObject>
#include <QJsonArray>
#include <QList>

typedef vtkSmartPointer<class vtkCardinalSpline> vtkCardinalSplinePtr;

namespace cx
{

RouteToTarget::RouteToTarget():
        mBranchListPtr(new BranchList),
        mProjectedIndex(0)
{
}


RouteToTarget::~RouteToTarget()
{
}

/*
void RouteToTarget::setCenterline(vtkPolyDataPtr centerline)
{
        mCLpoints = this->getCenterlinePositions(centerline);
}
*/

Eigen::MatrixXd RouteToTarget::getCenterlinePositions(vtkPolyDataPtr centerline_r)
{

    int N = centerline_r->GetNumberOfPoints();
        Eigen::MatrixXd CLpoints(3,N);
        for(vtkIdType i = 0; i < N; i++)
                {
                double p[3];
        centerline_r->GetPoint(i,p);
                Eigen::Vector3d position;
                position(0) = p[0]; position(1) = p[1]; position(2) = p[2];
                CLpoints.block(0 , i , 3 , 1) = position;
                }
        return CLpoints;
}

void RouteToTarget::processCenterline(MeshPtr mesh)
{
        if (mBranchListPtr)
                mBranchListPtr->deleteAllBranches();

        vtkPolyDataPtr centerline_r = mesh->getTransformedPolyDataCopy(mesh->get_rMd());

                Eigen::MatrixXd CLpoints_r = getCenterlinePositions(centerline_r);

                mBranchListPtr->findBranchesInCenterline(CLpoints_r);

        mBranchListPtr->calculateOrientations();
        mBranchListPtr->smoothOrientations();
                //mBranchListPtr->smoothBranchPositions(40);

        std::cout << "Number of branches in CT centerline: " << mBranchListPtr->getBranches().size() << std::endl;
}


void RouteToTarget::findClosestPointInBranches(Vector3D targetCoordinate_r)
{
        double minDistance = 100000;
        int minDistancePositionIndex;
        BranchPtr minDistanceBranch;
        std::vector<BranchPtr> branches = mBranchListPtr->getBranches();
        for (int i = 0; i < branches.size(); i++)
        {
                Eigen::MatrixXd positions = branches[i]->getPositions();
                for (int j = 0; j < positions.cols(); j++)
                {
            double D = findDistance(positions.col(j), targetCoordinate_r);
                        if (D < minDistance)
                        {
                                minDistance = D;
                                minDistanceBranch = branches[i];
                                minDistancePositionIndex = j;
                        }
                }
        }

                mProjectedBranchPtr = minDistanceBranch;
                mProjectedIndex = minDistancePositionIndex;
}


void RouteToTarget::findRoutePositions()
{
        mRoutePositions.clear();

        searchBranchUp(mProjectedBranchPtr, mProjectedIndex);
}

/*
    RouteToTarget::searchBranchUp is finding all positions from a given index on a branch and up
    the airway tree to the top of trachea. All positions are added to mRoutePositions, which stores
    all positions along the route-to-target.
    Before the positions are added they are smoothed by RouteToTarget::smoothBranch.
*/
void RouteToTarget::searchBranchUp(BranchPtr searchBranchPtr, int startIndex)
{
        if (!searchBranchPtr)
                return;

        std::vector< Eigen::Vector3d > positions = smoothBranch(searchBranchPtr, startIndex, searchBranchPtr->getPositions().col(startIndex));
        //std::vector< Eigen::Vector3d > positions = getBranchPositions(searchBranchPtr, startIndex);

        for (int i = 0; i<=startIndex && i<positions.size(); i++)
                mRoutePositions.push_back(positions[i]);

        mBranchingIndex.push_back(mRoutePositions.size()-1);

        BranchPtr parentBranchPtr = searchBranchPtr->getParentBranch();
        if (parentBranchPtr)
                searchBranchUp(parentBranchPtr, parentBranchPtr->getPositions().cols()-1);
}


vtkPolyDataPtr RouteToTarget::findRouteToTarget(PointMetricPtr targetPoint)
{
        mTargetPosition = targetPoint->getCoordinate();

        findClosestPointInBranches(mTargetPosition);
        findRoutePositions();

        vtkPolyDataPtr retval = addVTKPoints(mRoutePositions);

        return retval;
}

vtkPolyDataPtr RouteToTarget::findExtendedRoute(PointMetricPtr targetPoint)
{
                mTargetPosition = targetPoint->getCoordinate();
    double extentionPointIncrement = 0.25; //mm
    mExtendedRoutePositions.clear();
    mExtendedRoutePositions = mRoutePositions;
        if(mRoutePositions.size() > 0)
        {
                double extentionDistance = findDistance(mRoutePositions.front(),mTargetPosition);
                                Eigen::Vector3d extentionVectorNormalized = ( mTargetPosition - mRoutePositions.front() ) / extentionDistance;
        int numberOfextentionPoints = int(extentionDistance / extentionPointIncrement);
        Eigen::Vector3d extentionPointIncrementVector = extentionVectorNormalized * extentionDistance / numberOfextentionPoints;

                for (int i = 1; i<= numberOfextentionPoints; i++)
                {
                        mExtendedRoutePositions.insert(mExtendedRoutePositions.begin(), mRoutePositions.front() + extentionPointIncrementVector*i);
                }
        }

    vtkPolyDataPtr retval = addVTKPoints(mExtendedRoutePositions);
    return retval;
}

vtkPolyDataPtr RouteToTarget::addVTKPoints(std::vector<Eigen::Vector3d> positions)
{
        vtkPolyDataPtr retval = vtkPolyDataPtr::New();
        vtkPointsPtr points = vtkPointsPtr::New();
        vtkCellArrayPtr lines = vtkCellArrayPtr::New();
    int numberOfPositions = positions.size();
        for (int j = numberOfPositions - 1; j >= 0; j--)
        {
        points->InsertNextPoint(positions[j](0),positions[j](1),positions[j](2));
        }
        for (int j = 0; j < numberOfPositions-1; j++)
        {
                vtkIdType connection[2] = {j, j+1};
                lines->InsertNextCell(2, connection);
        }
        retval->SetPoints(points);
        retval->SetLines(lines);
        return retval;
}


/*
    RouteToTarget::getBranchPositions is used to get positions of a branch without smoothing.
    Equivalent to RouteToTarget::smoothBranch without smoothing.
*/
std::vector< Eigen::Vector3d > RouteToTarget::getBranchPositions(BranchPtr branchPtr, int startIndex)
{
    Eigen::MatrixXd branchPositions = branchPtr->getPositions();
    std::vector< Eigen::Vector3d > positions;

    for (int i = startIndex; i >=0; i--)
        positions.push_back(branchPositions.col(i));

    return positions;
}

/*
    RouteToTarget::smoothBranch is smoothing the positions of a centerline branch by using vtkCardinalSpline.
    The degree of smoothing is dependent on the branch radius and the shape of the branch.
    First, the method tests if a straight line from start to end of the branch is sufficient by the condition of
    all positions along the line being within the lumen of the airway (max distance from original centerline
    is set to branch radius).
    If this fails, one more control point is added to the spline at the time, until the condition is fulfilled.
    The control point added for each iteration is the position with the larges deviation from the original/unfiltered
    centerline.
*/
std::vector< Eigen::Vector3d > RouteToTarget::smoothBranch(BranchPtr branchPtr, int startIndex, Eigen::MatrixXd startPosition)
{
    vtkCardinalSplinePtr splineX = vtkSmartPointer<vtkCardinalSpline>::New();
        vtkCardinalSplinePtr splineY = vtkSmartPointer<vtkCardinalSpline>::New();
        vtkCardinalSplinePtr splineZ = vtkSmartPointer<vtkCardinalSpline>::New();

    double branchRadius = branchPtr->findBranchRadius();

    //add control points to spline

    //add first position
    Eigen::MatrixXd positions = branchPtr->getPositions();
    splineX->AddPoint(0,startPosition(0));
    splineY->AddPoint(0,startPosition(1));
    splineZ->AddPoint(0,startPosition(2));


    // Add last position if no parent branch, else add parents position closest to current branch.
    // Branch positions are stored in order from head to feet (e.g. first position is top of trachea),
    // while route-to-target is generated from target to top of trachea.
    if(!branchPtr->getParentBranch())
    {
        splineX->AddPoint(startIndex,positions(0,0));
        splineY->AddPoint(startIndex,positions(1,0));
        splineZ->AddPoint(startIndex,positions(2,0));
    }
    else
    {
        Eigen::MatrixXd parentPositions = branchPtr->getParentBranch()->getPositions();
        splineX->AddPoint(startIndex,parentPositions(0,parentPositions.cols()-1));
        splineY->AddPoint(startIndex,parentPositions(1,parentPositions.cols()-1));
        splineZ->AddPoint(startIndex,parentPositions(2,parentPositions.cols()-1));

    }

    //Add points until all filtered/smoothed postions are minimum 1 mm inside the airway wall, (within r - 1 mm).
    //This is to make sure the smoothed centerline is within the lumen of the airways.
    double maxAcceptedDistanceToOriginalPosition = std::max(branchRadius - 1, 1.0);
    double maxDistanceToOriginalPosition = maxAcceptedDistanceToOriginalPosition + 1;
    int maxDistanceIndex = -1;
    std::vector< Eigen::Vector3d > smoothingResult;

    //add positions to spline
    while (maxDistanceToOriginalPosition >= maxAcceptedDistanceToOriginalPosition && splineX->GetNumberOfPoints() < startIndex)
    {
        if(maxDistanceIndex > 0)
        {
            //add to spline the position with largest distance to original position
            splineX->AddPoint(maxDistanceIndex,positions(0,startIndex - maxDistanceIndex));
            splineY->AddPoint(maxDistanceIndex,positions(1,startIndex - maxDistanceIndex));
            splineZ->AddPoint(maxDistanceIndex,positions(2,startIndex - maxDistanceIndex));
        }

                //evaluate spline - get smoothed positions      
        maxDistanceToOriginalPosition = 0.0;
        smoothingResult.clear();
        for(int j=0; j<=startIndex; j++)
                {
                        double splineParameter = j;
                        Eigen::Vector3d tempPoint;
                        tempPoint(0) = splineX->Evaluate(splineParameter);
                        tempPoint(1) = splineY->Evaluate(splineParameter);
                        tempPoint(2) = splineZ->Evaluate(splineParameter);
                        smoothingResult.push_back(tempPoint);

            //calculate distance to original (non-filtered) position
            double distance = findDistanceToLine(tempPoint, positions);
            //finding the index with largest distance
            if (distance > maxDistanceToOriginalPosition)
            {
                maxDistanceToOriginalPosition = distance;
                maxDistanceIndex = j;
            }
        }
    }

    return smoothingResult;
}

void RouteToTarget::addRouteInformationToFile(VisServicesPtr services)
{
    QString RTTpath = services->patient()->getActivePatientFolder() + "/RouteToTargetInformation/";
    QDir RTTDirectory(RTTpath);
    if (!RTTDirectory.exists()) // Creating RouteToTargetInformation folder if it does not exist
        RTTDirectory.mkpath(RTTpath);

    QString format = timestampSecondsFormat();
    QString filePath = RTTpath + QDateTime::currentDateTime().toString(format) + "_RouteToTargetInformation.txt";

    QFile outfile(filePath);
    if (outfile.open(QIODevice::ReadWrite))
    {
        QTextStream stream(&outfile);

        stream << "#Target position:" << endl;
        stream << mTargetPosition(0) << " " << mTargetPosition(1) << " " << mTargetPosition(2) << " " << endl;
        if (mProjectedBranchPtr)
        {
            stream << "#Route to target generations:" << endl;
            stream << mProjectedBranchPtr->findGenerationNumber() << endl;
        }

        stream << "#Trachea length (mm):" << endl;
        BranchPtr trachea = mBranchListPtr->getBranches()[0];
        int numberOfPositionsInTrachea = trachea->getPositions().cols();
        double tracheaLength = calculateRouteLength(smoothBranch(trachea, numberOfPositionsInTrachea-1, trachea->getPositions().col(numberOfPositionsInTrachea-1)));
        stream << tracheaLength << endl;

        stream << "#Route to target length - from Carina (mm):" << endl;
        stream << calculateRouteLength(mRoutePositions) - tracheaLength << endl;
        stream << "#Extended route to target length - from Carina (mm):" << endl;
        stream << calculateRouteLength(mExtendedRoutePositions) - tracheaLength << endl;
    }
}

double RouteToTarget::calculateRouteLength(std::vector< Eigen::Vector3d > route)
{
    double routeLenght = 0;
    for (int i=0; i<route.size()-1; i++)
    {
        double d0 = route[i+1](0) - route[i](0);
        double d1 = route[i+1](1) - route[i](1);
        double d2 = route[i+1](2) - route[i](2);

        routeLenght += sqrt( d0*d0 +d1*d1 + d2*d2 );
    }

    return routeLenght;
}


void RouteToTarget::makeMarianaCenterlineFile(QString filename)
{
        if (mExtendedRoutePositions.empty())
        {
                        std::cout << "mExtendedRoutePositions is empty." << std::endl;
                        return;
        }

        int numberOfExtendedPositions = mExtendedRoutePositions.size() - mRoutePositions.size();

        ofstream out(filename.toStdString().c_str());
        out << "# [xPos yPos zPos BranchingPoint (0=Normal, 1=Branching position, 2=Extended from airway to target)] ";
        out << "Number of positions: ";
        out << mExtendedRoutePositions.size(); // write number of positions
        out << "\n";

         for (int i = 1; i < mExtendedRoutePositions.size(); i++)
         {
                        out <<  mExtendedRoutePositions[i](0) << " "; // write x coordinate
                        out <<  mExtendedRoutePositions[i](1) << " "; // write y coordinate
                        out <<  mExtendedRoutePositions[i](2) << " "; // write z coordinate

                        if ( std::find(mBranchingIndex.begin(), mBranchingIndex.end(), i - numberOfExtendedPositions) != mBranchingIndex.end() )
                                        out <<  "1 ";
                        else if (i <= numberOfExtendedPositions)
                                out <<  "2 ";
                        else
                                        out <<  "0 ";

                        out << "\n";
         }

         out.close();
}

QJsonArray RouteToTarget::makeMarianaCenterlineJSON()
{
        QJsonArray array;
        if ( mRoutePositions.empty() || mExtendedRoutePositions.empty() )
        {
                        std::cout << "mRoutePositions is empty." << std::endl;
                        return array;
        }

        int numberOfExtendedPositions = mExtendedRoutePositions.size() - mRoutePositions.size();

        for (int i = 1; i < mExtendedRoutePositions.size(); i++)
        {
                QJsonObject position;
                position.insert( "x", mExtendedRoutePositions[i](0) );
                position.insert( "y", mExtendedRoutePositions[i](1) );
                position.insert( "z", mExtendedRoutePositions[i](2) );

                if ( std::find(mBranchingIndex.begin(), mBranchingIndex.end(), i - numberOfExtendedPositions) != mBranchingIndex.end() )
                        position.insert("Flag", 1);
                else if (i <= numberOfExtendedPositions)
                        position.insert("Flag", 2);
                else
                        position.insert("Flag", 0);

                array.append(position);
         }

         return array;
}


double findDistanceToLine(Eigen::MatrixXd point, Eigen::MatrixXd line)
{
    double minDistance = findDistance(point, line.col(0));
    for (int i=1; i<line.cols(); i++)
        if (minDistance > findDistance(point, line.col(i)))
            minDistance = findDistance(point, line.col(i));

    return minDistance;
}

double findDistance(Eigen::MatrixXd p1, Eigen::MatrixXd p2)
{
        double d0 = p1(0) - p2(0);
        double d1 = p1(1) - p2(1);
        double d2 = p1(2) - p2(2);

        double D = sqrt( d0*d0 + d1*d1 + d2*d2 );

        return D;
}


} /* namespace cx */