void ExperimentalTrajectory::removeWaypoint(int index)
{
std::cout << "Removing waypoint index: " << index << "..." << std::endl;
if ( (index>=0) && (index<nWaypoints) )
{
Eigen::MatrixXd newWaypointsMat = Eigen::MatrixXd::Zero(nDoF, nWaypoints-1);
newWaypointsMat.leftCols(index) = waypoints.leftCols(index);
newWaypointsMat.rightCols(nWaypoints-1-index) = waypoints.rightCols(nWaypoints-1-index);
waypoints.resize(nDoF, nWaypoints-1);
waypoints = newWaypointsMat;
Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWaypoints-2);
int durVecIndex = index;
durationVectorTmp.head(durVecIndex) = pointToPointDurationVector.head(durVecIndex);
durationVectorTmp.tail(nWaypoints -1 -durVecIndex) = pointToPointDurationVector.tail(nWaypoints -1 -durVecIndex);
pointToPointDurationVector.resize(durationVectorTmp.size());
pointToPointDurationVector = durationVectorTmp;
reinitialize();
}
else{
std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint time is out of index bounds. Should have fall between 0 and " << nWaypoints-1 << "." << std::endl;
}
}
void ExperimentalTrajectory::addNewWaypoint(Eigen::VectorXd newWaypoint, double waypointTime)
{
std::cout << "Adding waypoint at: " << waypointTime << " seconds..." << std::endl;
if ( (newWaypoint.rows() == nDoF) && (waypointTime>=0.0) && (waypointTime<=kernelCenters.maxCoeff()) )
{
//Find out where the new T falls...
for (int i=0; i<kernelCenters.size(); i++)
{
std::cout << "i = " << i << std::endl;
if (kernelCenters(i)>waypointTime) {
youngestWaypointIndex=i;
std::cout << "youngestWaypointIndex" << youngestWaypointIndex << std::endl;
i = kernelCenters.size();
}
}
Eigen::MatrixXd newWaypointsMat = Eigen::MatrixXd::Zero(nDoF, nWaypoints+1);
newWaypointsMat.leftCols(youngestWaypointIndex) = waypoints.leftCols(youngestWaypointIndex);
newWaypointsMat.col(youngestWaypointIndex) = newWaypoint;
newWaypointsMat.rightCols(nWaypoints - youngestWaypointIndex) = waypoints.rightCols(nWaypoints - youngestWaypointIndex);
waypoints.resize(nDoF, nWaypoints+1);
waypoints = newWaypointsMat;
Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWaypoints);
std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl;
durationVectorTmp.head(youngestWaypointIndex-1) = pointToPointDurationVector.head(youngestWaypointIndex-1);
durationVectorTmp.row(youngestWaypointIndex-1) << waypointTime - kernelCenters(youngestWaypointIndex-1);
durationVectorTmp.tail(nWaypoints - youngestWaypointIndex) = pointToPointDurationVector.tail(nWaypoints - youngestWaypointIndex);
pointToPointDurationVector.resize(durationVectorTmp.size());
pointToPointDurationVector = durationVectorTmp;
std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl;
reinitialize();
}
else{
if (newWaypoint.rows() != nDoF){
std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint is not the right size. Should have dim = " <<nDoF << "x1." << std::endl;
}
if ((waypointTime<=0.0) || (waypointTime>=kernelCenters.maxCoeff())){
std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint time is out of time bounds. Should have fall between 0.0 and " << kernelCenters.maxCoeff() << " seconds." << std::endl;
}
}
}
void BranchList::calculateOrientations()
{
for (int i = 0; i < mBranches.size(); i++)
{
Eigen::MatrixXd positions = mBranches[i]->getPositions();
Eigen::MatrixXd diff = positions.rightCols(positions.cols() - 1) - positions.leftCols(positions.cols() - 1);
Eigen::MatrixXd orientations(positions.rows(),positions.cols());
orientations.leftCols(orientations.cols() - 1) = diff / diff.norm();
orientations.rightCols(1) = orientations.col(orientations.cols() - 1);
mBranches[i]->setOrientations(orientations);
}
}
void ExperimentalTrajectory::initializeTrajectory()
{
originalWaypoints = waypoints;
kernelLengthParameter = pointToPointDurationVector.mean();
// Add a waypoint to the end.
int extraWaypoints = 1;
int nStartWp = extraWaypoints;
int nEndWp = extraWaypoints;
int nWpNew = nWaypoints + nStartWp + nEndWp;
Eigen::MatrixXd waypointsTmp = Eigen::MatrixXd::Zero(nDoF, nWpNew);
waypointsTmp.leftCols(nStartWp) = waypoints.leftCols(1).replicate(1,nStartWp);
waypointsTmp.middleCols(nStartWp, nWaypoints) = waypoints;
waypointsTmp.rightCols(nEndWp) = waypoints.rightCols(1).replicate(1,nEndWp);
waypoints.resize(nDoF, nWaypoints);
waypoints = waypointsTmp;
// std::cout << pointToPointDurationVector << std::endl;
Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWpNew-1);
double extraWpDt = 0.01 * kernelLengthParameter;
// double extraWpDt = 0.01 * pointToPointDurationVector.sum();
// std::cout << "extraWpDt: " << extraWpDt << std::endl;
durationVectorTmp.head(nStartWp) = Eigen::VectorXd::Constant(nStartWp, extraWpDt);
durationVectorTmp.segment(nStartWp, nWaypoints-1) = pointToPointDurationVector;
durationVectorTmp.tail(nEndWp) = Eigen::VectorXd::Constant(nEndWp, extraWpDt);
pointToPointDurationVector.resize(durationVectorTmp.size());
pointToPointDurationVector = durationVectorTmp;
nWaypoints = nWpNew;
std::cout << pointToPointDurationVector << std::endl;
calculateVarianceParameters();
}
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);
}
}
}
}
}
bool Tensor::ComputeJointSVD(Tensor& Umat, vector<Tensor*>& extra_tensor_list, vector<int>& mult_modes, int nonzerovals)
{
int num_sing_vals = (int)pow((double)nonzerovals, (double)mult_modes.size());
vector<int> free_modes;
vector<int> mult_dims;
vector<int> free_dims;
vector<int> mult_offsets;
vector<int> free_offsets;
Tensor& temp_tensor = *(extra_tensor_list.at(0));
VectorPlus::SetDiff(free_modes, temp_tensor.Modes(), mult_modes);
VectorPlus::Subset(mult_dims, temp_tensor.Dims(), mult_modes);
VectorPlus::CSubset(free_dims, temp_tensor.Dims(), mult_modes);
ComputeOffsets(mult_offsets, mult_dims);
ComputeOffsets(free_offsets, free_dims);
vector<int> usmalldims(free_modes.size(), nonzerovals);
vector<int> udims;
vector<int> usmall_offsets;
ComputeOffsets(usmall_offsets, usmalldims);
int numMultElements = VectorPlus::Product(mult_dims);
int numFreeElements = VectorPlus::Product(free_dims);
assert(numMultElements == numFreeElements);
Eigen::MatrixXd matricized_tensor(numFreeElements,extra_tensor_list.size() * numMultElements);
//cout << "copy start 1\n";
for (int z = 0; z < extra_tensor_list.size(); ++z)
{
int z_offset = z * numMultElements;
FastIndexer i_indexer(free_dims);
for (int i = 0; i < numFreeElements; ++i)
{
vector<int>& free_indices = i_indexer.GetNext();
// ComputeIndexArray(free_indices, free_offsets, i);
FastIndexer j_indexer(mult_dims);
for (int j = 0; j < numMultElements; ++j)
{
vector<int>& mult_indices = j_indexer.GetNext();
// ComputeIndexArray(mult_indices, mult_offsets, j);
vector<int> total_indices;
VectorPlus::Concat(total_indices, free_indices, mult_indices);
matricized_tensor(i,z_offset + j) = extra_tensor_list.at(z)->At(total_indices);
}
}
}
// cout << "copy end 1\n";
// MatrixXd matricized_inverse = matricized_tensor.inverse();
// cout << matricized_inverse;
// cout << "\n";
//compute pseudoinverse
//cout << "svd start 1\n";
Eigen::JacobiSVD<Eigen::MatrixXd> svd(matricized_tensor, Eigen::ComputeFullU);
Eigen::MatrixXd U = svd.matrixU();
// cout << "svd end 1\n";
Eigen::MatrixXd thinU = U.leftCols(num_sing_vals);
VectorPlus::Concat(udims, free_dims, usmalldims);
Umat.Initialize(udims);
vector<int> semi_dims;
semi_dims.push_back(thinU.rows());
semi_dims.push_back(thinU.cols());
// cout << "copy start 2 \n";
FastIndexer i_indexer(free_dims);
for (int i = 0; i < thinU.rows(); ++i)
{
vector<int>& left_indices = i_indexer.GetNext();
// ComputeIndexArray(left_indices, free_offsets, i);
FastIndexer j_indexer(usmalldims);
for (int j = 0; j < thinU.cols(); ++j)
{
vector<int>& right_indices = j_indexer.GetNext();
// ComputeIndexArray(right_indices, usmall_offsets, j);
vector<int> indices;
VectorPlus::Concat(indices, left_indices, right_indices);
// Umat.Set(indices, rand_matrix.At(i,j));
Umat.Set(indices, thinU(i,j));
if (thinU.rows() == thinU.cols())
{
if (VectorPlus::Equals(left_indices, right_indices))
{
Umat.Set(indices, 1);
}
else
{
Umat.Set(indices, 0);
}
}
}
}
// cout << "copy end 2 \n";
return true;
}
