C++ VectorXd::tail方法代码示例

//! Convert Cartesian to cylindrical state.
Eigen::VectorXd convertCartesianToCylindrical( const Eigen::VectorXd& cartesianState )
{
    // Create cylindrical state vector, initialized with zero entries.
    Eigen::VectorXd cylindricalState = Eigen::VectorXd::Zero( 6 );

    // Compute and set cylindrical coordinates.
    cylindricalState.head( 3 ) = convertCartesianToCylindrical(
                Eigen::Vector3d( cartesianState.head( 3 ) ) );

    // Compute and set cylindrical velocities.
    /* If radius = 0, then Vr = sqrt(xdot^2+ydot^2) and Vtheta = 0,
       else Vr = (x*xdot+y*ydot)/radius and Vtheta = (x*ydot-y*xdot)/radius.
    */
    if ( cylindricalState( 0 ) <= std::numeric_limits< double >::epsilon( ) )
    {
        cylindricalState.tail( 3 ) <<
            std::sqrt( pow( cartesianState( 3 ), 2 ) + pow( cartesianState( 4 ), 2 ) ), // Vr
            0.0,                                                                        // Vtheta
            cartesianState( 5 );                                                        // Vz
    }

    else
    {
        cylindricalState.tail( 3 ) <<
            ( cartesianState( 0 ) * cartesianState( 3 )
              + cartesianState( 1 ) * cartesianState( 4 ) ) / cylindricalState( 0 ),    // Vr
            ( cartesianState( 0 ) * cartesianState( 4 )
              - cartesianState( 1 ) * cartesianState( 3 ) ) / cylindricalState( 0 ),    // Vtheta
                cartesianState( 5 );                                                    // Vz
    }

    return cylindricalState;
}

bool RectangleHandler::getFeaturePoseInWorldFrame(long int id,
		Eigen::VectorXd& c) const {

	const string &sensor = getFeatureSensor(id);

	ParameterWrapper_Ptr f_par = _filter->getParameterByName(sensor + "_F"); // anchor frame

	if (!f_par) {
		return false;
	}

	const Eigen::VectorXd &fw = f_par->getEstimate();

	Eigen::VectorXd dim(2);
	getFeatureDimensions(id, dim);

	Eigen::Quaterniond R_WO(fw(3),fw(4),fw(5),fw(6));
	Eigen::Vector3d t_WO(fw(0),fw(1),fw(2));
	Eigen::Vector3d t_OC(dim(0)/2.0, dim(1)/2.0, 0.0);

	c.head(3) = t_WO+R_WO.toRotationMatrix()*t_OC;
	c.tail(4) =  fw.tail(4);

	return true;
}

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 rootBounds( double &lb, double &ub )
 {
     int deg = coef.rows()-1;
     Eigen::VectorXd mycoef = coef.tail(deg).array().abs()/fabs(coef(0));
     mycoef(0) += 1.;
     ub = mycoef.maxCoeff();
     lb = -ub;
 }

            void set_h_params(const Eigen::VectorXd& p)
            {
                _h_params = p.head(_tr.rows() * _tr.cols());
                for (int c = 0; c < _tr.cols(); c++)
                    for (int r = 0; r < _tr.rows(); r++)
                        _tr(r, c) = p[r * _tr.cols() + c];

                if (_mean_function.h_params_size() > 0)
                    _mean_function.set_h_params(p.tail(_mean_function.h_params_size()));
            }

 FunctionARD(size_t dim_out = 1)
     : _mean_function(dim_out), _tr(dim_out, dim_out + 1)
 {
     Eigen::VectorXd h = Eigen::VectorXd::Zero(dim_out * (dim_out + 1) + _mean_function.h_params_size());
     for (size_t i = 0; i < dim_out; i++)
         h[i * (dim_out + 2)] = 1;
     if (_mean_function.h_params_size() > 0)
         h.tail(_mean_function.h_params_size()) = _mean_function.h_params();
     this->set_h_params(h);
 }

 void realRoots(std::vector<double> &roots) const
 {
     if ( coef[0] == 0 )
     {
         int deg = coef.rows()-1;
         Internal::RootFinder<Eigen::Dynamic>::compute(coef.tail(deg),roots);
     } else {
         Internal::RootFinder<Eigen::Dynamic>::compute(coef,roots);
     }
 }

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 realRootsSturm(const double lb, const double ub, std::vector<double> &roots) const
 {
     if ( coef[0] == 0 )
     {
         int deg = coef.rows()-1;
         Internal::SturmRootFinder<Eigen::Dynamic> sturm( coef.tail(deg) );
         sturm.realRoots( lb, ub, roots );
     } else {
         Internal::SturmRootFinder<Eigen::Dynamic> sturm( coef );
         sturm.realRoots( lb, ub, roots );
     }
 }

//! Convert cylindrical to Cartesian state.
Eigen::VectorXd convertCylindricalToCartesian( const Eigen::VectorXd& cylindricalState )
{
    // Create Cartesian state vector, initialized with zero entries.
    Eigen::VectorXd cartesianState = Eigen::VectorXd::Zero( 6 );

    // Get azimuth angle, theta.
    double azimuthAngle = cylindricalState( 1 );

    // Compute and set Cartesian coordinates.
    cartesianState.head( 3 ) = convertCylindricalToCartesian(
                Eigen::Vector3d( cylindricalState.head( 3 ) ) );

    // If r = 0 AND Vtheta > 0, then give warning and assume Vtheta=0.
    if ( std::fabs(cylindricalState( 0 )) <= std::numeric_limits< double >::epsilon( )
         && std::fabs(cylindricalState( 4 )) > std::numeric_limits< double >::epsilon( ) )
    {
        std::cerr << "Warning: cylindrical velocity Vtheta (r*thetadot) does not equal zero while "
                  << "the radius (r) is zero! Vtheta is taken equal to zero!\n";

        // Compute and set Cartesian velocities.
        cartesianState.tail( 3 )
                << cylindricalState( 3 ) * std::cos( azimuthAngle ),   // xdot
                   cylindricalState( 3 ) * std::sin( azimuthAngle ),   // ydot
                   cylindricalState( 5 );                              // zdot
    }

    else
    {
        // Compute and set Cartesian velocities.
        cartesianState.tail( 3 )
                << cylindricalState( 3 ) * std::cos( azimuthAngle )
                   - cylindricalState( 4 ) * std::sin( azimuthAngle ),   // xdot
                   cylindricalState( 3 ) * std::sin( azimuthAngle )
                   + cylindricalState( 4 ) * std::cos( azimuthAngle ),   // ydot
                   cylindricalState( 5 );                                // zdot
    }

    return cartesianState;
}

// receive FT Sensor data
void
recvFT(const geometry_msgs::WrenchStampedConstPtr& msg){
    mutex_force.lock();
//    std::cout <<"Fx"<<"\t"<<"Fy"<<"\t"<<"Fz"<<"\t"<<"Tx"<<"\t"<<"Ty"<<"\t"<<"Tz"<<std::endl;
//     if (counter >50){
//    std::cout << msg->wrench.force.x<<","<<msg->wrench.force.y<<","<<msg->wrench.force.z<<","<<msg->wrench.torque.x<<","<<msg->wrench.torque.y<<","<<msg->wrench.torque.z<<std::endl;
//         counter = 0;
//     }
//    else{
//        counter ++;
// }
    Eigen::Vector3d grav_tmp;
    grav_tmp.setZero();
    grav_tmp(2) = Grav_Schunk_Acc+tool_grav;
    ft_gama->raw_ft_f(0) = msg->wrench.force.x;
    ft_gama->raw_ft_f(1) = msg->wrench.force.y;
    ft_gama->raw_ft_f(2) = msg->wrench.force.z-Grav_Schunk_Acc;
    ft_gama->raw_ft_t(0) = msg->wrench.torque.x;
    ft_gama->raw_ft_t(1) = msg->wrench.torque.y;
    ft_gama->raw_ft_t(2) = msg->wrench.torque.z;
    //get rid of tool/pokingstick gravity--calib
    ft_gama->calib_ft_f = right_rs->robot_orien["eef"] * ft_gama->raw_ft_f + grav_tmp;
    ft_gama->calib_ft_t = ft_gama->raw_ft_t;
    Eigen::Vector3d f_offset;
    f_offset.setZero();
    f_offset(0) = -0.5;
    f_offset(1) = -0.5;
    f_offset(2) = -0.6;
    //use smooth filter to get rid of noise.
    ft.head(3) = ft_gama->filtered_gama_f = gama_f_filter->push(ft_gama->calib_ft_f) - f_offset;
    //get rid of noise of no contacting
    if(ft_gama->filtered_gama_f.norm() <1.5)
	    ft.head(3).setZero();
    ft.tail(3) = ft_gama->filtered_gama_t = gama_t_filter->push(ft_gama->calib_ft_t);
    
    counter_t ++;
    if(counter_t%50==0){
        counter_t = 0;
//        std::cout<<"estimated contact force: "<<ft_gama->filtered_gama_f(0)<<","<<ft_gama->filtered_gama_f(1)<<","
//                <<ft_gama->filtered_gama_f(2)<<","<<ft_gama->filtered_gama_t(0)<<","<<ft_gama->filtered_gama_t(1)<<","<<ft_gama->filtered_gama_t(2)<<std::endl;
}
    
    mutex_force.unlock();
}

bool AnchoredRectangleHandler::getFeaturePoseInWorldFrame(long int id,
    Eigen::VectorXd& c) const {

  const string &sensor = getFeatureSensor(id);

  ParameterWrapper_Ptr f_par = _filter->getParameterByName(sensor + "_F"); // anchor frame
  ParameterWrapper_Ptr fohp_par = _filter->getParameterByName(sensor + "_FOhp"); // anchor frame
  ParameterWrapper_Ptr foq_par = _filter->getParameterByName(sensor + "_FOq"); // anchor frame

  if (!f_par || !fohp_par || !foq_par) {
    return false;
  }

  const Eigen::VectorXd &fw = f_par->getEstimate();

  const Eigen::VectorXd &FOhp = fohp_par->getEstimate();
  const Eigen::VectorXd &FOq = foq_par->getEstimate();

  Eigen::Quaterniond Fq_e(fw(3), fw(4), fw(5), fw(6)); // anchor frame orientation wrt world

  // compute the position of the lower left corner of the object starting from anchor frame and homogeneous point

  Eigen::Vector3d FOhp_e;
  FOhp_e << FOhp(0), FOhp(1), 1.0; // construct a proper homogeneous point from the first two components of the parameter

  Eigen::Vector3d Ow;
  Ow = fw.head(3) + 1 / FOhp(2) * (Fq_e.toRotationMatrix() * FOhp_e);

  // orientation of the object
  Eigen::Quaterniond FOq_e(FOq(0), FOq(1), FOq(2), FOq(3));
  Eigen::Quaterniond R_WO = Fq_e * FOq_e;

  Eigen::VectorXd dim(2);
  getFeatureDimensions(id, dim);

  Eigen::Vector3d t_OC(dim(0) / 2.0, dim(1) / 2.0, 0.0);

  c.head(3) = Ow + R_WO.toRotationMatrix() * t_OC;
  c.tail(4) << R_WO.w(), R_WO.x(), R_WO.y(), R_WO.z();

  return true;
}

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 KalmanFilter::Propagate(Eigen::VectorXd x_hat_plus, Eigen::MatrixXd P_plus, double v_m, double w_m, Eigen::MatrixXd Q, double dt)
{
	int n = x_hat_plus.size();
	
	double ori = x_hat_plus(2);

		
	Eigen::VectorXd x_hat_min(n);
	Eigen::MatrixXd P_min(n,n); 
	
	Eigen::MatrixXd Set(n,n+1);		//Set of state and covariance
	
	Eigen::MatrixXd Phi_R(3,3);
	Eigen::MatrixXd G(3,2);
	
	Eigen::MatrixXd tempTrans;
	
	//Propagate state (Robot and Landmark, no change in Landmark)
	x_hat_min.head(3) << v_m*cos(ori),
				v_m*sin(ori),
				w_m;
	
	x_hat_min.head(3) = x_hat_plus.head(3) + dt*x_hat_min.head(3);	
	x_hat_min.tail(n-3) = x_hat_plus.tail(n-3);


	// Set up Phi_R and G
	Phi_R << 1, 0, -dt*v_m*sin(ori),
		0, 1, dt*v_m*cos(ori),
		0, 0, 1;
	
	G << -dt*cos(ori), 0,
		-dt*sin(ori), 0,
		0, -dt;
	

	// Propagate Covariance
	// P_RR
	P_min.block(0,0,3,3) = Phi_R * P_plus.block(0,0,3,3) * Phi_R.transpose() + G * Q * G.transpose();

	// P_RL
	P_min.block(0,3,3,n-3) = Phi_R * P_plus.block(0,3,3,n-3);

	// P_LR
	tempTrans = P_min.block(0,3,3,n-3).transpose();
	P_min.block(3,0,n-3,3) = tempTrans;

	// P_LL are not affected by propagation.
	P_min.block(3,3,n-3,n-3) = P_plus.block(3,3,n-3, n-3);

	// Make sure the matrix is symmetric, may skip this option
	tempTrans = 0.5 * (P_min + P_min.transpose());
	P_min = tempTrans;	

	// Put State vector and Covariance into one matrix and return it.
	// We can parse these out in the main function.
	Set.block(0,0,n,1) = x_hat_min;
	Set.block(0,1,n,n) = P_min;

	return Set;
}

//.........这里部分代码省略.........
                if (vertex_flag[index_j[p]]) {
                    for (int k=0; k<3; ++k) 
                        for (int dim=0; dim < 3; ++dim)
                            Y[dim](row + k) +=  weight_smooth*Q_j_hat(p, k)*dst.vertex_coord[vertex_real_col[index_j[p]]][dim];
                } else {
                    for (int k=0; k<3; ++k) 
                        A.coeffRef(row+k, src.poly_num + vertex_real_col[index_j[p]]) += -weight_smooth*Q_j_hat(p, k);
                }
            }
            row += 3;
        }
    }
    energy_size[0] = row;

    double weight_identity = weights[1];
    for (int i=0; i<src.poly_num; ++i) {
        Eigen::Matrix3d& Q_i_hat = Q_hat_inverse[i];
        unsigned int* index_i = src.polyIndex[i].vert_index;
        Y[0](row) = weight_identity;    Y[0](row+1) = 0.0;              Y[0](row+2) = 0.0; 
        Y[1](row) = 0.0;                Y[1](row+1) = weight_identity;  Y[1](row+2) = 0.0; 
        Y[2](row) = 0.0;                Y[2](row+1) = 0.0;              Y[2](row+2) = weight_identity; 

        for (int k=0; k<3; ++k)    
            A.coeffRef(row+k, i) = weight_identity*Q_i_hat(0, k);   //n

        if (vertex_flag[index_i[0]]) {
            for (int k=0; k<3; ++k) 
                for (int dim = 0; dim < 3; ++dim)
                    Y[dim](row+k) += weight_identity*(Q_i_hat(1, k)+Q_i_hat(2,k))*dst.vertex_coord[vertex_real_col[index_i[0]]][dim];
        } else {
            for (int k=0; k<3; ++k) 
                A.coeffRef(row+k, src.poly_num+vertex_real_col[index_i[0]]) = -weight_identity*(Q_i_hat(1, k)+Q_i_hat(2,k));
        }

        for (int p=1; p<3; ++p) {
            if (vertex_flag[index_i[p]]) {
                for (int k=0; k<3; ++k)
                    for (int dim=0; dim<3; ++dim)
                        Y[dim](row + k) += -weight_identity*Q_i_hat(p, k)*dst.vertex_coord[vertex_real_col[index_i[p]]][dim];
            } else {
                for (int k=0; k<3; ++k) 
                    A.coeffRef(row+k, src.poly_num + vertex_real_col[index_i[p]]) = weight_identity*Q_i_hat(p, k);
            }
        }

        row += 3;
    }
    energy_size[1] = row;

    double weight_soft_constraint = weights[2];
    for (int i=0; i<soft_corres.size(); ++i) {
        if (vertex_flag[soft_corres[i].first]) continue;
        A.coeffRef(row, src.poly_num + vertex_real_col[soft_corres[i].first]) = weight_soft_constraint;
        for (int dim=0; dim<3; ++dim)
            Y[dim](row) += weight_soft_constraint*dst.vertex_coord[soft_corres[i].second][dim];
        ++row;
    }
    energy_size[2] = row;

    assert (row == rows);
    
       //start solving the least-square problem
    fprintf(stdout, "finished filling matrix\n");
    Eigen::SparseMatrix<double> At = A.transpose();
    Eigen::SparseMatrix<double> AtA = At*A;

    Eigen::SimplicialCholesky<SparseMatrix<double>> solver;
    solver.compute(AtA);
    if (solver.info() != Eigen::Success) {
        fprintf(stdout, "unable to defactorize AtA\n");
        exit(-1);
    }

	VectorXd X[3];
	for (int i=0; i<3; ++i) {
		VectorXd AtY = At*Y[i];
		X[i] = solver.solve(AtY);
		Eigen::VectorXd Energy = A*X[i] - Y[i];
		Eigen::VectorXd smoothEnergy = Energy.head(energy_size[0]);
		Eigen::VectorXd identityEnergy = Energy.segment(energy_size[0], energy_size[1]-energy_size[0]);
		Eigen::VectorXd softRegularEnergy = Energy.tail(energy_size[2]-energy_size[1]);
		fprintf(stdout, "\t%lf = %lf + %lf + %lf\n", 
			Energy.dot(Energy), smoothEnergy.dot(smoothEnergy), 
			identityEnergy.dot(identityEnergy), softRegularEnergy.dot(softRegularEnergy));
	}
    
    //fill data back to src
    for (int i=0; i<src.poly_num; ++i)
        for (int d=0; d<3; ++d)
            src.face_norm[i][d] = X[d](i);
    for (size_t i=0; i<corres.size(); ++i) src.vertex_coord[corres[i].first] = dst.vertex_coord[corres[i].second];
    int p = 0;
    for (int i=0; i<src.vert_num; ++i) {
        if (vertex_flag[i]) continue;
        for (int d=0; d<3; ++d)
            src.vertex_coord[i][d] = X[d](src.poly_num+p);
        ++p;
    }
    return;
}