C++ Vector4d::head方法代码示例

Vector3d transformPoint(Vector3d& point,const MatrixXd& transform)
{
  Vector4d tmp;
  tmp.head(3)=point;
  tmp(3)=1;
  tmp.head(3)=transform*tmp;
  return tmp.head(3);
}

Vector6d gc_asd_to_av(Vector4d asd) {
  Vector6d av;

  Vector3d aa = asd.head(3);

  // double d_inv = asd(3);

  // double sig_d_inv = (1.0 - exp(-asd(3))) / (2.0 * (1.0 + exp(-asd(3))));

  // double sig_d_inv = -log(1.0/asd(3) - 1.0);
  // double sig_d_inv = log( (2.0 * asd(3) + 1.0) / (1.0 - 2.0*asd(3)) );
  // double sig_d_inv = atan(asd(3)) / 2.0;
  // double sig_d_inv = atan2(asd(3), 1.0) / 2.0;
  // double sig_d_inv = atan2(asd(3), 1.0);

  // double sig_d_inv = atan2(asd(3), 1.0) * 1.0;
  
  // double sig_d_inv = tan(4.0 * asd(3));
  double sig_d_inv = log(asd(3));
  // cout << "sig_d_inv = " << sig_d_inv << endl;

  // double sig_d_inv = cos(asd(3)) / sin(asd(3));

  // double sig_d_inv = sin(asd(3)) / cos(asd(3));
  // double sig_d_inv = sin(asd(3)) / cos(asd(3));

  Matrix3d R = gc_Rodriguez(aa);

  // av.head(3) = R.col(2) / sig_d_inv;
  av.head(3) = R.col(2) * sig_d_inv;
  av.tail(3) = R.col(0);

  return av;
}

Vector6d gc_aid_to_av(Vector4d aid) {

  Vector6d av;
  Vector3d aa = aid.head(3);
  double d = 1.0 / aid(3);
  Matrix3d R = gc_Rodriguez(aa);
  av.head(3) = R.col(2) * d;
  av.tail(3) = R.col(0);

//  Vector6d av;
//  double a = aid[0];
//  double b = aid[1];
//  double g = aid[2];
//  double t = aid[3];
//
//  double s1 = sin(a);
//  double c1 = cos(a);
//  double s2 = sin(b);
//  double c2 = cos(b);
//  double s3 = sin(g);
//  double c3 = cos(g);
//
//  Matrix3d R;
//  R <<
//      c2 * c3,   s1 * s2 * c3 - c1 * s3,   c1 * s2 * c3 + s1 * s3,
//      c2 * s3,   s1 * s2 * s3 + c1 * c3,   c1 * s2 * s3 - s1 * c3,
//      -s2,                  s1 * c2,                  c1 * c2;
//
//  double d = 1.0 / t;
//  av.head(3) = -R.col(2) * d;
//  av.tail(3) = R.col(1);

  return av;
}

void 
addnode(SysSPA &spa, int n, 
	// node translation
	std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ntrans,
	// node rotation
	std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > nqrot,
	// constraint indices
	std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind,
	// constraint local translation 
	std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ctrans,
	// constraint local rotation as quaternion
	std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > cqrot,
	// constraint covariance
	std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > cvar)

{
  Node nd;

  // rotation
  Quaternion<double> frq;
  frq.coeffs() = nqrot[n];
  frq.normalize();
  if (frq.w() <= 0.0) frq.coeffs() = -frq.coeffs();
  nd.qrot = frq.coeffs();

  // translation
  Vector4d v;
  v.head(3) = ntrans[n];
  v(3) = 1.0;
  nd.trans = v;
  nd.setTransform();        // set up world2node transform
  nd.setDr(true);

  // add to system
  spa.nodes.push_back(nd);

  // add in constraints
  for (int i=0; i<(int)ctrans.size(); ++i)
    {
      ConP2 con;
      con.ndr = cind[i].x();
      con.nd1 = cind[i].y();

      if ((con.ndr == n && con.nd1 <= n-1) ||
          (con.nd1 == n && con.ndr <= n-1))
        {
	  con.tmean = ctrans[i];
	  Quaternion<double> qr;
	  qr.coeffs() = cqrot[i];
	  qr.normalize();
	  con.qpmean = qr.inverse(); // inverse of the rotation measurement
	  con.prec = cvar[i];       // ??? should this be inverted ???

	  // need a boost for noise-offset system
	  //con.prec.block<3,3>(3,3) *= 10.0;
	  spa.p2cons.push_back(con);
	}
    }
}

// Project a 3D point into this Pose.
Vector2d Pose::ProjectTo2D(const Vector3d& pt3d) {
  Vector4d pt3d_h = Vector4d::Constant(1.0);
  pt3d_h.head(3) = pt3d;

  const Vector4d proj_h = Rt_ * pt3d_h;
  Vector2d proj = proj_h.head(2);
  proj /= proj_h(2);

  return proj;
}

Vector3d rigidBodyDynamics::diffQuaternion(Vector4d& q, Vector4d& qprev, double dt) const {
	/*
    qm = qinv.*qcurr;
    M = [   qm(4)    qm(3)    -qm(2)   -qm(1);
            -qm(3)   qm(4)    qm(1)    -qm(2);
            qm(2)    -qm(1)   qm(4)    -qm(3);
            qm(1)    qm(2)    qm(3)    qm(4)];
    w = 2*M*dq'
    */

	Vector4d dq = (q - qprev) / dt;
	Matrix4d M;

	M << 	qprev(3) , qprev(2),  -qprev(1),   -qprev(0),
	        -qprev(2),   qprev(3),   qprev(0),  -qprev(1),
	        qprev(1),  -qprev(0),   qprev(3),   -qprev(2),
	        qprev(0),  qprev(1),  qprev(2),  qprev(3);

	Vector4d wp = 2*M*dq;
	Vector3d w = wp.head(3);

	return w;
}

int main(int argc, char **argv)
{
  char *fin;

  if (argc < 2)
    {
      cout << "Arguments are:  <input filename> [<number of nodes to use>]" << endl;
      return -1;
    }

  // number of nodes to use
  int nnodes = -1;

  if (argc > 2)
    nnodes = atoi(argv[2]);

  int doiters = 10;
  if (argc > 3)
    doiters = atoi(argv[3]);

  fin = argv[1];

  // node translation
  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ntrans;
  // node rotation
  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > nqrot;
  // constraint indices
  std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind;
  // constraint local translation 
  std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ctrans;
  // constraint local rotation as quaternion
  std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > cqrot;
  // constraint covariance
  std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > cvar;
  // tracks
  std::vector<struct tinfo> tracks;


  ReadSPAFile(fin,ntrans,nqrot,cind,ctrans,cqrot,cvar,tracks);

  cout << "# [ReadSPAFile] Found " << (int)ntrans.size() << " nodes and " 
       << (int)cind.size() << " constraints" << endl;

  if (nnodes < 0)
    nnodes = ntrans.size();
  if (nnodes > (int)ntrans.size()) nnodes = ntrans.size();

  // system
  SysSPA spa;
  spa.verbose=false;
  //  spa.useCholmod(true);
  spa.csp.useCholmod = true;

  // add first node
  Node nd;

  // rotation
  Quaternion<double> frq;
  frq.coeffs() = nqrot[0];
  frq.normalize();
  if (frq.w() <= 0.0) frq.coeffs() = -frq.coeffs();
  nd.qrot = frq.coeffs();

  // translation
  Vector4d v;
  v.head(3) = ntrans[0];
  v(3) = 1.0;
  nd.trans = v;
  nd.setTransform();        // set up world2node transform
  nd.setDr(true);

  // add to system
  spa.nodes.push_back(nd);

  double cumtime = 0.0;
  //cout << nd.trans.transpose() << endl << endl;

  // add in nodes
  for (int i=0; i<nnodes-1; i+=doiters)
    {
      for (int j=0; j<doiters; ++j)
        if (i+j+1 < nnodes)
          addnode(spa, i+j+1, ntrans, nqrot, cind, ctrans, cqrot, cvar);

      long long t0, t1;

      spa.nFixed = 1;           // one fixed frame

      t0 = utime();
      //      spa.doSPA(1,1.0e-4,SBA_SPARSE_CHOLESKY);
      spa.doSPA(1,1.0e-4,SBA_BLOCK_JACOBIAN_PCG,1.0e-8,15);
      t1 = utime();
      cumtime += t1 - t0;

      cerr 
        << "nodes= " << spa.nodes.size()
        << "\tedges= " << spa.p2cons.size()
        << "\t chi2= " << spa.calcCost()
        << "\t time= " << (t1 - t0) * 1e-6
        << "\t cumTime= " << cumtime*1e-6
//.........这里部分代码省略.........

int
PatchFit::fit()
{
  // first check whether cloud subsampling is required
  RandomSample<PointXYZ> *rs;
  rs = new RandomSample<PointXYZ>();
  if (this->ssmax_)
  {
    rs->setInputCloud(cloud_);
    rs->setSample(ssmax_);
    rs->setSeed(rand()); 
    rs->filter(*cloud_); 
  }
  delete (rs);

  //remove NAN points from the cloud
  std::vector<int> indices;
  removeNaNFromPointCloud(*cloud_, *cloud_, indices); //is_dense should be false
 
  
  // Step 1: fit plane by lls
  Vector4d centroid;
  compute3DCentroid(*cloud_, centroid);
  p_->setC(centroid.head(3));

  cloud_vec.resize(cloud_->points.size(),3);
  fit_cloud_vec.resize(cloud_->points.size(),3);
  int vec_counter = 0;
  for (size_t i = 0; i<cloud_->points.size (); i++)
  {
    cloud_vec.row(vec_counter) = cloud_->points[i].getVector3fMap ().cast<double>();

    // for fitting sub the centroid
    fit_cloud_vec.row(vec_counter) = cloud_vec.row(vec_counter) - centroid.head(3).transpose();
    vec_counter++;
  }
  
  cloud_vec.conservativeResize(vec_counter-1, 3); //normal resize does not keep old values
  
  VectorXd b(fit_cloud_vec.rows());
  b.fill(0.0);

  Vector3d zl = lls(fit_cloud_vec,b);
  p_->setR(zh.cross(zl));
  double ss = p_->getR().norm();
  double cc = zh.adjoint()*zl;
  double t = atan2(ss,cc);
  double th = sqrt(sqrt(EPS))*10.0;
  double aa;
  if (t>th)
    aa = t/ss;
  else
    aa = 6.0/(6.0-t*t);
  p_->setR(aa*p_->getR());

  // Save the zl and the centroid p.c, for patch center constraint's use
  plane_n = zl;
  plane_c = p_->getC();


  // Step 2: continue surface fitting if not plane
  VectorXd a;

  if (st_a) // general paraboloid
  {
    if (ccon_)
    {
      a.resize(6);
      a << 0.0, 0.0, p_->getR(), 0.0;
    }
    else
    {
      a.resize(8);
      a << 0.0, 0.0, p_->getR(), p_->getC();
    }
    a = wlm(cloud_vec, a);

    // update the patch
    p_->setR(a.segment(2,3));
    if (ccon_)
      p_->setC(plane_c + a(5)*plane_n);
    else
      p_->setC(a.segment(5,3));
    Vector2d k;
    k << a(0), a(1);

    // refine into a specific paraboloid type
    if (k.cwiseAbs().maxCoeff() < this->ktol_)
    {
      // fitting planar paraboloid
      p_->setName("planar paraboloid");
      p_->setS(Patch::s_p);
      Matrix<double,1,1> k_p;
      k_p << 0.0;
      p_->setK(k_p);
      p_->setR(p_->rcanon2(p_->getR(),2,3)); //TBD not updating correctly
    }
    else if (k.cwiseAbs().minCoeff() < this->ktol_)
    {
      // fitting cylindric paraboloid
//.........这里部分代码省略.........

  // Set up spanning tree initialization
  void SysSPA::spanningTree(int node)
  {
    int nnodes = nodes.size();

    // set up an index from nodes to their constraints
    vector<vector<int> > cind;
    cind.resize(nnodes);

    for(size_t pi=0; pi<p2cons.size(); pi++)
      {
        ConP2 &con = p2cons[pi];
        int i0 = con.ndr;
        int i1 = con.nd1;
        cind[i0].push_back(i1);
        cind[i1].push_back(i0);        
      }

    // set up breadth-first algorithm
    VectorXd dist(nnodes);
    dist.setConstant(1e100);
    if (node >= nnodes)
      node = 0;
    dist[node] = 0.0;
    multimap<double,int> open;  // open list, priority queue - can have duplicates
    open.insert(std::make_pair<double,int>(0.0,int(node)));

    // do breadth-first computation
    while (!open.empty())
      {
        // get top node, remove it
        int ni = open.begin()->second;
        double di = open.begin()->first;
        open.erase(open.begin());
        if (di > dist[ni]) continue; // already dealt with

        // update neighbors
        Node &nd = nodes[ni];
        Matrix<double,3,4> n2w;
        transformF2W(n2w,nd.trans,nd.qrot); // from node to world coords

        vector<int> &nns = cind[ni];
        for (int i=0; i<(int)nns.size(); i++)
          {
            ConP2 &con = p2cons[nns[i]];
            double dd = con.tmean.norm(); // incremental distance
            // neighbor node index
            int nn = con.nd1;
            if (nn == ni)
              nn = con.ndr;
            Node &nd2 = nodes[nn];
            Vector3d tmean = con.tmean;
            Quaterniond qpmean = con.qpmean;
            if (nn == con.ndr)       // wrong way, reverse
              {
                qpmean = qpmean.inverse();
                tmean = nd.qrot.toRotationMatrix().transpose()*nd2.qrot.toRotationMatrix()*tmean;
              }
                
            if (dist[nn] > di + dd) // is neighbor now closer?
              {
                // set priority queue
                dist[nn] = di+dd;
                open.insert(std::make_pair<double,int>(di+dd,int(nn)));
                // update initial pose
                Vector4d trans;
                trans.head(3) = tmean;
                trans(3) = 1.0;
                nd2.trans.head(3) = n2w*trans;
                nd2.qrot = qpmean*nd.qrot;
                nd2.normRot();
                nd2.setTransform();
                nd2.setDr(true);
              }
          }
      }
    
  }

// Current code only works for the left leg with only one constraint
VectorXd MyWorld::updateGradients() {
  // compute c(q)
  mC = mSkel->getMarker(mConstrainedMarker)->getWorldPosition() - mTarget;

  // compute J(q)
  Vector4d offset;
  offset << mSkel->getMarker(mConstrainedMarker)->getLocalPosition(), 1; // Create a vector in homogeneous coordinates
  // w.r.t ankle dofs
  BodyNode *node = mSkel->getMarker(mConstrainedMarker)->getBodyNode();
  Joint *joint = node->getParentJoint();
  Matrix4d worldToParent = node->getParentBodyNode()->getTransform().matrix();
  Matrix4d parentToJoint = joint->getTransformFromParentBodyNode().matrix();
  Matrix4d dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
  Matrix4d R = joint->getTransform(1).matrix();
  Matrix4d jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
  Vector4d jCol = worldToParent * parentToJoint * dR * R * jointToChild * offset;
  int colIndex = joint->getIndexInSkeleton(0);
  mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
  dR = joint->getTransformDerivative(1);
  R = joint->getTransform(0).matrix();
  jCol = worldToParent * parentToJoint * R * dR * jointToChild * offset;
  colIndex = joint->getIndexInSkeleton(1);
  mJ.col(colIndex) = jCol.head(3);
  offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * jointToChild * offset; // Update offset so it stores the chain below the parent joint

  // w.r.t knee dof
  node = node->getParentBodyNode(); // return NULL if node is the root node
  joint = node->getParentJoint();
  worldToParent = node->getParentBodyNode()->getTransform().matrix();
  parentToJoint = joint->getTransformFromParentBodyNode().matrix();
  dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
  jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
  jCol = worldToParent * parentToJoint * dR * jointToChild * offset;
  colIndex = joint->getIndexInSkeleton(0);
  mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
  offset = parentToJoint * joint->getTransform(0).matrix() * jointToChild * offset;

  // w.r.t hip dofs
  node = node->getParentBodyNode();
  joint = node->getParentJoint();
  worldToParent = node->getParentBodyNode()->getTransform().matrix();
  parentToJoint = joint->getTransformFromParentBodyNode().matrix();
  dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
  Matrix4d R1 = joint->getTransform(1).matrix();
  Matrix4d R2 = joint->getTransform(2).matrix();
  jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
  jCol = worldToParent * parentToJoint * dR * R1 * R2 * jointToChild * offset;
  colIndex = joint->getIndexInSkeleton(0);
  mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of J

  R1 = joint->getTransform(0).matrix();
  dR = joint->getTransformDerivative(1);
  R2 = joint->getTransform(2).matrix();
  jCol = worldToParent * parentToJoint * R1 * dR * R2 * jointToChild * offset;
  colIndex = joint->getIndexInSkeleton(1);
  mJ.col(colIndex) = jCol.head(3);

  R1 = joint->getTransform(0).matrix();
  R2 = joint->getTransform(1).matrix();
  dR = joint->getTransformDerivative(2);
  jCol = worldToParent * parentToJoint * R1 * R2 * dR * jointToChild * offset;
  colIndex = joint->getIndexInSkeleton(2);
  mJ.col(colIndex) = jCol.head(3);

  // compute gradients
  VectorXd gradients = 2 * mJ.transpose() * mC;
  return gradients;
}

// Current code only works for the left leg with only one constraint
VectorXd MyWorld::updateGradients() {
    mJ.setZero();
    mC.setZero();

  // compute c(q)
  //std::cout << "HAMYDEBUG: mConstrainedMarker = " << getMarker(mConstrainedMarker) << std::endl;
  mC = getMarker(mConstrainedMarker)->getWorldPosition() - mTarget;
  std::cout << "C(q) = " << mC << std::endl;

  // compute J(q)
  Vector4d offset;
  offset << getMarker(mConstrainedMarker)->getLocalPosition(), 1; // Create a vector in homogeneous coordinates

  //Setup vars

  BodyNode *node = getMarker(mConstrainedMarker)->getBodyNode();
  Joint *joint = node->getParentJoint();
  Matrix4d worldToParent;
  Matrix4d parentToJoint;
  
  //Declare Vars
  Matrix4d dR; // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
  Matrix4d R;
  Matrix4d R1;
  Matrix4d R2;
  Matrix4d jointToChild;
  Vector4d jCol;
  int colIndex;

  //TODO: Might want to change this to check if root using given root fcn

  //Iterate until we get to the root node
  while(true) {

    //std::cout << "HAMY DEBUG: Beginning new looop" << std::endl;

    if(node->getParentBodyNode() == NULL) {
      worldToParent = worldToParent.setIdentity();
    } else {
      worldToParent = node->getParentBodyNode()->getTransform().matrix();
    }
    parentToJoint = joint->getTransformFromParentBodyNode().matrix();
     // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
    jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();

    //TODO: R1, R2, ... Rn code depending on DOFs
    int nDofs = joint->getNumDofs();
    //std::cout << "HAMY: nDofs=" << nDofs << std::endl;
    //Can only have up to 3 DOFs on any one piece
    switch(nDofs){
      case 1: //std::cout << "HAMY: 1 nDOF" << std::endl;

        dR = joint->getTransformDerivative(0);
        jCol = worldToParent * parentToJoint * dR * jointToChild * offset;
        colIndex = joint->getIndexInSkeleton(0);
        mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
        offset = parentToJoint * joint->getTransform(0).matrix() * jointToChild * offset;

        break;
      case 2: //std::cout << "HAMY: 2 nDOF" << std::endl;

        dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
        R = joint->getTransform(1).matrix();
        jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
        jCol = worldToParent * parentToJoint * dR * R * jointToChild * offset;
        colIndex = joint->getIndexInSkeleton(0);
        mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol

        dR = joint->getTransformDerivative(1);
        R = joint->getTransform(0).matrix();
        jCol = worldToParent * parentToJoint * R * dR * jointToChild * offset;
        colIndex = joint->getIndexInSkeleton(1);
        mJ.col(colIndex) = jCol.head(3);
        offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * jointToChild * offset; // Upd

        break;
      case 3: //std::cout << "HAMY: 3 nDOF" << std::endl;

        dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
        R1 = joint->getTransform(1).matrix();
        R2 = joint->getTransform(2).matrix();
        jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
        jCol = worldToParent * parentToJoint * dR * R1 * R2 * jointToChild * offset;
        colIndex = joint->getIndexInSkeleton(0);
        mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of J

        R1 = joint->getTransform(0).matrix();
        dR = joint->getTransformDerivative(1);
        R2 = joint->getTransform(2).matrix();
        jCol = worldToParent * parentToJoint * R1 * dR * R2 * jointToChild * offset;
        colIndex = joint->getIndexInSkeleton(1);
        mJ.col(colIndex) = jCol.head(3);

        R1 = joint->getTransform(0).matrix();
        R2 = joint->getTransform(1).matrix();
        dR = joint->getTransformDerivative(2);
        jCol = worldToParent * parentToJoint * R1 * R2 * dR * jointToChild * offset;
        colIndex = joint->getIndexInSkeleton(2);
        mJ.col(colIndex) = jCol.head(3);
//.........这里部分代码省略.........

int main(int argc, char **argv)
{
  // args
  double lscale = 0.0;
  double con_weight = 1.0;

  printf("Args: <lscale 0.0>  <scale weight 1.0>\n");

  if (argc > 1)
    lscale = atof(argv[1]);

  if (argc > 2)
    con_weight = atof(argv[2]);


  // set up markers for visualization
  ros::init(argc, argv, "VisBundler");
  ros::NodeHandle n ("~");
  ros::Publisher link_pub = n.advertise<visualization_msgs::Marker>("links", 0);
  ros::Publisher cam_pub = n.advertise<visualization_msgs::Marker>("cameras", 0);

  // set up system
  SysSPA spa;
  Node::initDr();               // set up fixed jacobians
  initPrecs();

  vector<Matrix<double,6,1>,Eigen::aligned_allocator<Matrix<double,6,1> > > cps;
  double kfang = 5.0;
  double kfrad = kfang*M_PI/180.0;

  // create a spiral test trajectory
  // connections are made between a frame and its three successors

  spa.nFixed = 1;               // three fixed frames
  spa_spiral_setup(spa, true, cps, // use cross links
#if 1
                   n2prec, n2vprec, n2aprec, n2bprec,  // rank-deficient
#else
                   diagprec, diagprec, diagprec, diagprec,
#endif
                   kfang, M_PI/2.0-3*kfrad, 220*kfang/360.0, // angle per node, init angle, total nodes
                   0.01, 1.0, 0.1, 0.1, 5.0); // node noise (m,deg), scale noise (increment),

  cout << "[SPA Spiral] Initial cost is " << spa.calcCost() << endl;
  cout << "[SPA Spiral] Number of constraints is " << spa.p2cons.size() << endl;  

#if 0
  // write out pose results and originals
  cout << "[SPAsys] Writing pose file" << endl;  
  ofstream ofs3("P400.init.txt");
  for (int i=0; i<(int)cps.size(); i++)
    {
      Vector3d tpn = spa.nodes[i].trans.head(3);
      ofs3 << tpn.transpose() << endl;
    }  
  ofs3.close();
#endif


  // add in distance constraint
  // works with either n0 -> ni or ni -> ni+1 constraints
#if 1
  ConScale con;
  con.w = con_weight;                // weight
  for (int i=0; i<(int)cps.size()-3; i++)
    {
      int k = i;
      if (i > 200)
        {
          k = 0;
          con.nd0 = i;              // first node
          con.nd1 = i+1;            // second node
          con.sv  = k;              // scale index
          con.ks  = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance
          spa.scons.push_back(con);

          con.nd0 = i+1;            // first node
          con.nd1 = i+2;            // second node
          con.sv  = k;              // scale index
          con.ks  = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance
          spa.scons.push_back(con);

          con.nd0 = i+2;            // first node
          con.nd1 = i+3;            // second node
          con.sv  = k;              // scale index
          con.ks  = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance
          spa.scons.push_back(con);

          con.nd0 = i;              // first node
          con.nd1 = i+3;            // second node
          con.sv  = k;              // scale index
          con.ks  = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance
          spa.scons.push_back(con);
        }

      else
        {
          spa.scales.push_back(1.0);
          Node nd0;
          Node nd1;
//.........这里部分代码省略.........