C++ MatrixXd::leftCols方法代码示例

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);
					}
				}
			}
		}
//.........这里部分代码省略.........