C++ Matrix4d::corner方法代码示例

void Thread_RRT::getNextGoal(RRT_Node& goal, RRT_Node& goalThisIter)
{
  //first, see if we are sampling randomly, or around goal
  double randNum = ((double)rand()) / ((double)RAND_MAX);
  if (randNum < RANDOM_SAMPLE_THRESH)
  {
    //goalThisIter.copyNode(goal);   
    //return;
    
    setThisGoalNearGoal(goal, goalThisIter);
    return;



  }

  //sample random goal
  Matrix4d startTrans;
  goal.thread->getStartTransform(startTrans);

  Vector3d startPos = startTrans.corner(Eigen::TopRight,3,1);
  Vector3d startTan = startTrans.corner(Eigen::TopLeft,3,1);
  goalThisIter.thread = new Thread(goal.thread->length(), startPos, startTan);

  goalThisIter.thread->minimize_energy_fixedPieces();
  goalThisIter.setPoints();

}

void Thread::printThreadInfo()
{
  //print out some information
	Eigen::Transform3d first_rotation_transform(Eigen::AngleAxisd(_angle_first_rot, _tangents[0]));
  int numPieces = 0;
  double energy = 0.0;
	double twist = 0.0;
  double length = 0.0;
  ThreadPiece* currPiece = threadList;
	Matrix4d trans = _translate_to_start*first_rotation_transform*_transform_to_start;
  trans.corner(Eigen::TopLeft,3,3) *= _length;
  while (currPiece != NULL)
  {
    numPieces++;
    energy += currPiece->_length*(currPiece->_torsion*currPiece->_torsion + currPiece->_curvature*currPiece->_curvature); 
		//twist += currPiece->_length*currPiece->_torsion;
    length += currPiece->_length;
    std::cout << "length:" << currPiece->_length << std::endl;
    std::cout << "curvature: " << currPiece->_curvature/_length << "  torsion: " << currPiece->_torsion/_length << std::endl;
    trans *= currPiece->_transform;
    currPiece = currPiece->_next_segment;
  }

  std::cout << "Optimal thread has " << numPieces << " pieces, and a calculated energy of " << energy*_length << std::endl;
  std::cout << "Final Position: \n" << trans.corner(Eigen::TopRight, 3,1) << std::endl;
  std::cout << "Final Position Wanted: \n" << _positions[1] << std::endl;
  std::cout << "Final Tangent: \n" << trans.corner(Eigen::TopLeft, 3, 1)/_length << std::endl;
}

void Thread::getActualEndTangent(Vector3d& endTan)
{
  Matrix4d currTrans;
  getStartTransform(currTrans);
  currTrans.corner(Eigen::TopLeft,3,3) *= _length;
  ThreadPiece* currPiece = threadList;
  while (currPiece != NULL)
  {
    currTrans *= currPiece->_transform;
    currPiece = currPiece->_next_segment;
  }

  endTan = currTrans.corner(Eigen::TopLeft,3,1);
}

void Thread::getPoints(MatrixXd& points)
{
	Eigen::Transform3d first_rotation_transform(Eigen::AngleAxisd(_angle_first_rot, _tangents[0]));
	Matrix4d trans = _translate_to_start*first_rotation_transform*_transform_to_start;
  trans.corner(Eigen::TopLeft,3,3) *= _length;

	threadList->getPoints(points, 0.0, 1.0/((double)(points.rows()-1)), 0, trans); 
}

 Matrix4d rotateZ(double _angle)
 {
 	Matrix4d m = Matrix4d::Identity();
 	Matrix3d r;
 	r = AngleAxisd(_angle, Vector3d::UnitZ());
 	m.corner(TopLeft, 3, 3) = r;
 	return m;
 }

void Thread::optimizeManyPoints(MatrixXd& constraints, int num_pts_between, vector<double>&curvatures_resampled, vector<double>& torsions_resampled, Matrix4d& start_transform)
{
  int num_constraints = constraints.rows()-1;
  int num_pieces_reopt = num_pts_between*num_constraints;
  double length_per_piece = 1.0/( num_pieces_reopt);
  
  //opt_params_many_points.transform_back = _transform_to_start;
  opt_params_many_points.num_segments = num_pieces_reopt;
  opt_params_many_points.length_per_segment = length_per_piece;


  opt_params_many_points.points.resize(num_constraints);
  opt_params_many_points.orig_params_each_piece = new NEWMAT::ColumnVector(2*num_pieces_reopt+3);
  opt_params_many_points.num_pts_between = num_pts_between;

  opt_params_many_points.gravity_multipler = (GRAVITY_CONSTANT)*_length;
  opt_params_many_points.total_length = _length;


  for (int i=0; i < num_constraints; i++)
  {
    for (int j=0; j < num_pts_between; j++)
    {
      int piece_num = i*num_pts_between+j;
      opt_params_many_points.orig_params_each_piece->element(2*piece_num) = curvatures_resampled[i];
      opt_params_many_points.orig_params_each_piece->element(2*piece_num+1) = torsions_resampled[i];
    }
  }

  

  double eulerZ, eulerY, eulerX;
  eulerAnglesFramTransform(start_transform.corner(Eigen::TopLeft,3,3), eulerZ, eulerY, eulerX);
  opt_params_many_points.orig_params_each_piece->element(2*num_pieces_reopt) = eulerZ;
  opt_params_many_points.orig_params_each_piece->element(2*num_pieces_reopt+1) = eulerY;
  opt_params_many_points.orig_params_each_piece->element(2*num_pieces_reopt+2) = eulerX;

  for (int i=0; i < num_constraints; i++)
  {
    opt_params_many_points.points[i] = ((constraints.block((i+1),0,1,3) - constraints.block(0,0,1,3))/_length).transpose();
  }


  NEWMAT::ColumnVector x_sol;

  optimize_FDNLF(2*opt_params_many_points.num_segments+3, energyEvalFunctionManyPoints, energyEvalFunctionManyPoints_init, x_sol);


  std::cout << "done re-optimizing" << std::endl;
  delete opt_params_many_points.orig_params_each_piece;

 
  //set thread params
  _positions[0] = constraints.block(0,0,1,3).transpose();
  transformFromEulerAngles(_transform_to_start, x_sol(2*num_pieces_reopt+1), x_sol(2*num_pieces_reopt+2));
	_translate_to_start = Matrix4d::Identity();
  _translate_to_start.corner(Eigen::TopRight,3,1) = _positions[0];

  _tangents[0] = _transform_to_start.corner(Eigen::TopLeft,3,1);
  //_tangents[0] = orig_thread->_tangents[0];

  _angle_first_rot = x_sol(2*num_pieces_reopt+3);

	inverseTransform(_transform_to_start, _transform_to_unit);



  Matrix4d transform_back;
  getStartTransform(transform_back);
  transform_back.corner(Eigen::TopLeft,3,3) *= _length;
  threadList = new ThreadPiece(x_sol(1), x_sol(2), length_per_piece);
  ThreadPiece* currPiece = threadList;
  for (int i=1; i < num_pieces_reopt; i++)
  {
    currPiece->addSegmentAfter(new ThreadPiece(x_sol(2*i+1), x_sol(2*i+2), length_per_piece));
    transform_back *= currPiece->_transform;
    currPiece = currPiece->_next_segment;
  }

  //_positions[1] = transform_back.corner(Eigen::TopRight,3,1);
 // _positions[1] = orig_thread->_positions[1];
 // _tangents[1] = orig_thread->_tangents[1];
 // _tangents[1].normalize();
 // _tangents[0].normalize();

  _positions[1] = transform_back.corner(Eigen::TopRight,3,1);
  _tangents[1] = transform_back.corner(Eigen::TopLeft,3,1);
  _tangents[1].normalize();

  //printThreadInfo();



}

 Matrix4d rotationFromMatrix3d(const Matrix3d &_mat)
 {
 	Matrix4d m = Matrix4d::Identity();
 	m.corner(TopLeft, 3, 3) = _mat;
 	return m;
 }

void Thread_RRT::setThisGoalNearGoal(RRT_Node& goal, RRT_Node& goalThisIter)
{
  //sample something in sphere between current point and goal
  //first, set the things we know
  Matrix4d startTrans;
  goal.thread->getStartTransform(startTrans);
  Vector3d posNewThread[2];
  Vector3d tanNewThread[2];
  double lengthNewThread[2];
  double length_thread = goal.thread->length();
  lengthNewThread[0] = lengthNewThread[1] = length_thread/2.0;
  posNewThread[0] = startTrans.corner(Eigen::TopRight,3,1);
  tanNewThread[0] = startTrans.corner(Eigen::TopLeft,3,1);
  Vector3d endPos;
  goal.thread->getWantedEndPosition(endPos);
  Vector3d endTan;
  goal.thread->getWantedEndTangent(endTan);


  //random params for initializing
	double curvatureNewThread[2] = {randomNumUnit()*RANDOM_THREAD_MAX_CURVATURE_INIT, randomNumUnit()*RANDOM_THREAD_MAX_CURVATURE_INIT};
	double torsionNewThread[2] = {randomNumUnit()*RANDOM_THREAD_MAX_TORSION_INIT, randomNumUnit()*RANDOM_THREAD_MAX_TORSION_INIT};

  //find closest thread to goal
  int indClosest;
  findClosestNode(goal, indClosest);

  Vector3d endPosClosestToGoal;
  currNodes[indClosest]->this_node.thread->getWantedEndPosition(endPosClosestToGoal);
  Vector3d endTanClosestToGoal;
  currNodes[indClosest]->this_node.thread->getWantedEndTangent(endTanClosestToGoal);

  //calculate the distance to this end
  //sample from this radius, add to end point
  double randX, randY, randZ, sampledRadius;
  double radius_pos = (endPos - endPosClosestToGoal).norm() + ADD_TO_GOAL_DIST_RADIUS;
  double dist_new_thread;
  do 
  {
    //find point in sphere with this radius, add to goal endpoint
    //std::cout << "sampling end point" << std::endl;
    double sampledRadius;
    do
    {
      //std::cout << "sampling circle" << std::endl;
      randX = randomMaxAbsValue(radius_pos);
      randY = randomMaxAbsValue(radius_pos);
      randZ = randomMaxAbsValue(radius_pos);
      sampledRadius = randX*randX + randY*randY + randZ*randZ;
    } while (sampledRadius >= (radius_pos*radius_pos));
    posNewThread[1](0) = randX+endPos(0);
    posNewThread[1](1) = randY+endPos(1);
    posNewThread[1](2) = randZ+endPos(2);

    dist_new_thread = (posNewThread[1]-posNewThread[0]).norm();
  } while (dist_new_thread >= length_thread);

  //sample from a tan around the goal tan
  //sample from outside of sphere with radius equal to length between goal tan and closest tan
  //normalize the length of that new tan
  double radius_tan = (endTan - endTanClosestToGoal).norm() + ADD_TO_GOAL_TAN_RADIUS;
  double randTheta = M_PI*randomNumUnit();
  double randPhi = 2.0*M_PI*randomNumUnit();
  tanNewThread[1](0) = endTan(0)+radius_tan*cos(randTheta)*sin(randPhi);
  tanNewThread[1](1) = endTan(1)+radius_tan*sin(randTheta)*sin(randPhi);
  tanNewThread[1](2) = endTan(2)+radius_tan*cos(randPhi);

  tanNewThread[1].normalize();

//  std::cout << "goal end pos: " << endPos << std::endl;
//  std::cout << "goal end tan: " << endTan << std::endl;

//  std::cout << "new end pos: " << posNewThread[1] << std::endl;
//  std::cout << "new end tan: " << tanNewThread[1] << std::endl;

  
  goalThisIter.thread = new Thread(goal.thread);
  goalThisIter.thread->setEndConstraint(posNewThread[1], tanNewThread[1]);
 // goalThisIter.thread->upsampleAndOptimize_minLength(0.065);
  goalThisIter.thread->minimize_energy_fixedPieces();


  //goalThisIter.thread = new Thread(curvatureNewThread, torsionNewThread, lengthNewThread, 2, posNewThread, tanNewThread);
  //goalThisIter.thread = new Thread(length_thread, posNewThread[0], tanNewThread[0]);
  //goalThisIter.thread->minimize_energy_fixedPieces();
  goalThisIter.setPoints();

  double distToGoal = goal.distanceToNode(goalThisIter);
  //std::cout << "distance to actual goal: " << distToGoal << std::endl;

}