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

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 TaskSolverDmpArm2D::performRollout(const Eigen::VectorXd& sample, const Eigen::VectorXd& task_parameters, Eigen::MatrixXd& cost_vars) const
{
  TaskSolverDmp::performRollout(sample, task_parameters, cost_vars);

  // cost_vars_angles structure is (without the link positions!)
  //
  // time  joint angles (e.g. n_dofs = 3)     forcing term  link positions (e.g. 3+1) 
  // ____  __________________________________  __________  _________________________    
  // t     | a a a | ad ad ad | add add add |  f f f       | x y | x y | x y | x y  |    
  //
  // We now compute the link positions and add them to the end
  
  int n_dofs = dmp_->dim_orig();
  int n_time_steps = cost_vars.rows();
  assert(cost_vars.cols() == 1+4*n_dofs);
  
  // Make room for link positions, i.e. 2 * (n_links+1)
  cost_vars.conservativeResize(n_time_steps, 1 + 4*n_dofs + 2*(n_dofs+1));
  
  MatrixXd angles = cost_vars.block(0,1,n_time_steps,n_dofs);
  MatrixXd link_positions;
  anglesToLinkPositions(angles,link_positions);
  
  // Add the link positions to the right side of the cost_vars matrix
  cost_vars.rightCols(2*(n_dofs+1)) = link_positions;
}

Eigen::MatrixXd ExperimentalTrajectory::getWaypointData()
{
    Eigen::MatrixXd dataMat = Eigen::MatrixXd::Zero(nWaypoints, nDoF+1);

    dataMat.col(0) = kernelCenters;
    dataMat.rightCols(nDoF) = waypoints.transpose();

    return dataMat;
}

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;
        }
    }


}

int main(int argc, char * argv[])
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;
  VectorXd D;
  if(!read_triangle_mesh("../shared/beetle.off",V,F))
  {
    cout<<"failed to load mesh"<<endl;
  }
  twod = V.col(2).minCoeff()==V.col(2).maxCoeff();
  bbd = (V.colwise().maxCoeff()-V.colwise().minCoeff()).norm();
  SparseMatrix<double> L,M;
  cotmatrix(V,F,L);
  L = (-L).eval();
  massmatrix(V,F,MASSMATRIX_TYPE_DEFAULT,M);
  const size_t k = 5;
  if(!eigs(L,M,k+1,EIGS_TYPE_SM,U,D))
  {
    cout<<"failed."<<endl;
  }
  U = ((U.array()-U.minCoeff())/(U.maxCoeff()-U.minCoeff())).eval();

  igl::viewer::Viewer viewer;
  viewer.callback_key_down = [&](igl::viewer::Viewer & viewer,unsigned char key,int)->bool
  {
    switch(key)
    {
      default: 
        return false;
      case ' ':
      {
        U = U.rightCols(k).eval();
        // Rescale eigen vectors for visualization
        VectorXd Z = 
          bbd*0.5*U.col(c);
        Eigen::MatrixXd C;
        igl::parula(U.col(c).eval(),false,C);
        c = (c+1)%U.cols();
        if(twod)
        {
          V.col(2) = Z;
        }
        viewer.data.set_mesh(V,F);
        viewer.data.compute_normals();
        viewer.data.set_colors(C);
        return true;
      }
    }
  };
  viewer.callback_key_down(viewer,' ',0);
  viewer.core.show_lines = false;
  viewer.launch();
}

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();
}

Eigen::MatrixXd eraseCol(int removeIndex, Eigen::MatrixXd positions)
{
	positions.block(0 , removeIndex , positions.rows() , positions.cols() - removeIndex - 1) = positions.rightCols(positions.cols() - removeIndex - 1);
    positions.conservativeResize(Eigen::NoChange, positions.cols() - 1);
	return positions;
}

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);
				}

			}

		}
	}
}