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

// Converts a representative vector per face in the full set of vectors that describe
// an N-RoSy field
void representative_to_nrosy(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& F,
    const Eigen::MatrixXd& R,
    const int N,
    Eigen::MatrixXd& Y)
{
    using namespace Eigen;
    using namespace std;
    MatrixXd B1, B2, B3;

    igl::local_basis(V,F,B1,B2,B3);

    Y.resize(F.rows()*N,3);
    for (unsigned i=0; i<F.rows(); ++i)
    {
        double x = R.row(i) * B1.row(i).transpose();
        double y = R.row(i) * B2.row(i).transpose();
        double angle = atan2(y,x);

        for (unsigned j=0; j<N; ++j)
        {
            double anglej = angle + 2*M_PI*double(j)/double(N);
            double xj = cos(anglej);
            double yj = sin(anglej);
            Y.row(i*N+j) = xj * B1.row(i) + yj * B2.row(i);
        }
    }
}

void color_intersections(
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & F,
  const Eigen::MatrixXd & U,
  const Eigen::MatrixXi & G,
  Eigen::MatrixXd & C,
  Eigen::MatrixXd & D)
{
  using namespace igl;
  using namespace igl::cgal;
  using namespace Eigen;
  MatrixXi IF;
  const bool first_only = false;
  intersect_other(V,F,U,G,first_only,IF);
  C.resize(F.rows(),3);
  C.col(0).setConstant(0.4);
  C.col(1).setConstant(0.8);
  C.col(2).setConstant(0.3);
  D.resize(G.rows(),3);
  D.col(0).setConstant(0.4);
  D.col(1).setConstant(0.3);
  D.col(2).setConstant(0.8);
  for(int f = 0;f<IF.rows();f++)
  {
    C.row(IF(f,0)) = RowVector3d(1,0.4,0.4);
    D.row(IF(f,1)) = RowVector3d(0.8,0.7,0.3);
  }
}

std::tuple<Eigen::VectorXi, Eigen::MatrixXd, Eigen::MatrixXd> 
    lu_row_pivoting(const Eigen::MatrixXd & A)
{
    Eigen::MatrixXd Acopy = A;
    int n = Acopy.rows();
    assert(n == Acopy.cols());

    Eigen::VectorXi p = Eigen::VectorXi::LinSpaced(n,1,n);
    Eigen::MatrixXd L = Eigen::MatrixXd::Zero(n,n);
    Eigen::MatrixXd U = Eigen::MatrixXd::Zero(n,n);

    for (int k=0; k<n-1; k++) {
        int maxRow, maxCol;
        Acopy.block(k,k,n-k,1).array().abs().maxCoeff(&maxRow, &maxCol);
        Acopy.row(k).swap(Acopy.row(maxRow+k));
        p.row(k).swap(p.row(maxRow+k));
        L.row(k).swap(L.row(maxRow+k));
        lu_helper(k, n, &Acopy, &L, &U);
    }
    
    L(n-1,n-1) = 1;
    U(n-1,n-1) = Acopy(n-1,n-1);

    return std::make_tuple(p,L,U);
}

    void save_bvecs_bvals (const Header& header, const std::string& bvecs_path, const std::string& bvals_path)
    {
      const auto grad = parse_DW_scheme (header);

      // rotate vectors from scanner space to image space
      Eigen::MatrixXd G = grad.leftCols<3>() * header.transform().rotation();

      // deal with FSL requiring gradient directions to coincide with data strides
      // also transpose matrices in preparation for file output
      vector<size_t> order;
      auto adjusted_transform = File::NIfTI::adjust_transform (header, order);
      Eigen::MatrixXd bvecs (3, grad.rows());
      Eigen::MatrixXd bvals (1, grad.rows());
      for (ssize_t n = 0; n < G.rows(); ++n) {
        bvecs(0,n) = header.stride(order[0]) > 0 ? G(n,order[0]) : -G(n,order[0]);
        bvecs(1,n) = header.stride(order[1]) > 0 ? G(n,order[1]) : -G(n,order[1]);
        bvecs(2,n) = header.stride(order[2]) > 0 ? G(n,order[2]) : -G(n,order[2]);
        bvals(0,n) = grad(n,3);
      }

      // bvecs format actually assumes a LHS coordinate system even if image is
      // stored using RHS - x axis is flipped to make linear 3x3 part of
      // transform have negative determinant:
      if (adjusted_transform.linear().determinant() > 0.0)
        bvecs.row(0) = -bvecs.row(0);

      save_matrix (bvecs, bvecs_path);
      save_matrix (bvals, bvals_path);
    }

IGL_INLINE bool igl::decimate(
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & F,
  const size_t max_m,
  Eigen::MatrixXd & U,
  Eigen::MatrixXi & G)
{
  int m = F.rows();
  const auto & shortest_edge_and_midpoint = [](
    const int e,
    const Eigen::MatrixXd & V,
    const Eigen::MatrixXi & /*F*/,
    const Eigen::MatrixXi & E,
    const Eigen::VectorXi & /*EMAP*/,
    const Eigen::MatrixXi & /*EF*/,
    const Eigen::MatrixXi & /*EI*/,
    double & cost,
    Eigen::RowVectorXd & p)
  {
    cost = (V.row(E(e,0))-V.row(E(e,1))).norm();
    p = 0.5*(V.row(E(e,0))+V.row(E(e,1)));
  };
  return decimate(
    V,
    F,
    shortest_edge_and_midpoint,
    max_faces_stopping_condition(m,max_m),
    U,
    G);
}

UnifiedModel::UnifiedModel(const Eigen::MatrixXd& centers, const Eigen::MatrixXd& widths, const Eigen::VectorXd& weights, bool normalized_basis_functions, bool lines_pivot_at_max_activation) 
{
  int n_basis_functions = centers.rows();
  int n_dims = centers.cols();
  
  assert(n_basis_functions==widths.rows());
  assert(n_dims           ==widths.cols());
  assert(n_basis_functions==weights.size());
  
  centers_.resize(n_basis_functions);
  covars_.resize(n_basis_functions);
  slopes_.resize(n_basis_functions);
  offsets_.resize(n_basis_functions);
  priors_.resize(n_basis_functions);
  
  for (int i=0; i<n_basis_functions; i++)
  {
    centers_[i] = centers.row(i);
    covars_[i] = widths.row(i).asDiagonal();
    covars_[i] = covars_[i].array().square();
    offsets_[i] = weights[i];
    
    slopes_[i] = VectorXd::Zero(n_dims);
    priors_[i] = 1.0;
  }
  
  normalized_basis_functions_ = normalized_basis_functions;
  lines_pivot_at_max_activation_ = lines_pivot_at_max_activation;
  slopes_as_angles_ = false;
 
  cosine_basis_functions_ = false;
  
  initializeAllValuesVectorSize();
};

void shortest_edge_and_midpoint1(const int e, const Eigen::MatrixXd &V,
                                 const Eigen::MatrixXi &F,
                                 const Eigen::MatrixXi &E,
                                 const Eigen::VectorXi &EMAP,
                                 const Eigen::MatrixXi &EF,
                                 const Eigen::MatrixXi &EI, double &cost,
                                 RowVectorXd &p) {
    // vectorsum
    const int eflip = E(e, 0) > E(e, 1);
    const std::vector<int> nV2Fd =
        igl::circulation(e, !eflip, F, E, EMAP, EF, EI);
    p = 0.5 * (V.row(E(e, 0)) + V.row(E(e, 1)));
    Eigen::RowVectorXd pointy(3);
    pointy.setZero();
    std::set<int> newEdges;
    for (int i = 0; i < nV2Fd.size(); i++) {
        for (int j = 0; j < 3; j++) {
            int curVert = F.row(nV2Fd[i])[j];
            if (curVert != E(e, 0) || curVert != E(e, 1)) {
                if (newEdges.insert(curVert).second) {
                    pointy = (V.row(curVert) - p) + pointy;
                }
            }
        }
    }
    cost = (pointy).norm();
}

void Model::compute_subjective_value(std::vector<std::vector<double>> &U,
                                     const Eigen::MatrixXd &M,
                                     const Parameter &parameter) {

  assert(n_dimensions == 2);
  U.resize(n_alternatives);

  double a, b, angle;

  for (unsigned int i = 0; i < n_alternatives; i++) {

    U.at(i).resize(n_dimensions);

    a = M.row(i).sum();
    b = a;

    angle = atan(M.row(i)(1) / M.row(i)(0));

    U.at(i).at(0) =
        b / pow(pow(tan(angle), parameter.m) + pow(b / a, parameter.m),
                1 / parameter.m);

    U.at(i).at(1) =
        b * pow(1 - pow(U.at(i).at(0) / a, parameter.m), 1 / parameter.m);
  }
}

gaussian_process::gaussian_process( const Eigen::MatrixXd& domains
                                , const Eigen::VectorXd& targets
                                , const gaussian_process::covariance& covariance
                                , double self_covariance )
    : domains_( domains )
    , targets_( targets )
    , covariance_( covariance )
    , self_covariance_( self_covariance )
    , offset_( targets.sum() / targets.rows() )
    , K_( domains.rows(), domains.rows() )
{
    if( domains.rows() != targets.rows() ) { COMMA_THROW( comma::exception, "expected " << domains.rows() << " row(s) in targets, got " << targets.rows() << " row(s)" ); }
    targets_.array() -= offset_; // normalise
    //use m_K as Kxx + variance*I, then invert it
    //fill Kxx with values from covariance function
    //for elements r,c in upper triangle
    for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
    {
        K_( r, r ) = self_covariance_;
        const Eigen::VectorXd& row = domains.row( r );
        for( std::size_t c = r + 1; c < std::size_t( domains.rows() ); ++c )
        {
            K_( c, r ) = K_( r, c ) = covariance_( row, domains.row( c ) );
        }
    }
    L_.compute( K_ ); // invert Kxx + variance * I to become (by definition) B
    alpha_ = L_.solve( targets_ );
}

IGL_INLINE void igl::ViewerData::add_edges(const Eigen::MatrixXd& P1, const Eigen::MatrixXd& P2, const Eigen::MatrixXd& C)
{
  Eigen::MatrixXd P1_temp,P2_temp;

  // If P1 only has two columns, pad with a column of zeros
  if (P1.cols() == 2)
  {
    P1_temp = Eigen::MatrixXd::Zero(P1.rows(),3);
    P1_temp.block(0,0,P1.rows(),2) = P1;
    P2_temp = Eigen::MatrixXd::Zero(P2.rows(),3);
    P2_temp.block(0,0,P2.rows(),2) = P2;
  }
  else
  {
    P1_temp = P1;
    P2_temp = P2;
  }

  int lastid = lines.rows();
  lines.conservativeResize(lines.rows() + P1_temp.rows(),9);
  for (unsigned i=0; i<P1_temp.rows(); ++i)
    lines.row(lastid+i) << P1_temp.row(i), P2_temp.row(i), i<C.rows() ? C.row(i) : C.row(C.rows()-1);

  dirty |= DIRTY_OVERLAY_LINES;
}

Eigen::MatrixXd linearCamera( Eigen::MatrixXd &xpt, Eigen::MatrixXd &Xpt)
{
    int n = xpt.cols();
    if (n < 6)
    {
        DEBUG_E( ("Camera Matrix can't be estimated in linearCamera(). Minimun required points are 6") ); 
        exit(-1);
    }
    
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(2*n,12);
    
    for (register int k = 0; k < n; ++k)
    {
        Eigen::RowVectorXd St(4);
        St << Xpt(0,k), Xpt(1,k), Xpt(2,k), Xpt(3,k);
        A.row(2*k) << Eigen::RowVectorXd::Zero(4), -xpt(2,k)*St, xpt(1,k)*St;
        A.row(2*k+1) << xpt(2,k)*St, Eigen::RowVectorXd::Zero(4), -xpt(0,k)*St;
    }
//     std::cout << "A:\n " << A <<'\n';
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullV | Eigen::ComputeThinU);
    Eigen::MatrixXd VV = svd.matrixV();
    Eigen::MatrixXd P(3,4);
    P << VV(0,11), VV(1,11), VV(2,11), VV(3,11), 
        VV(4,11), VV(5,11), VV(6,11), VV(7,11), 
        VV(8,11), VV(9,11), VV(10,11), VV(11,11);
    P = P/P(2,3);
    return P;
}

Eigen::MatrixXd kalmanFilter::kalmanFunc(Eigen::MatrixXd phi, Eigen::MatrixXd upsilon, Eigen::MatrixXd basis, Eigen::MatrixXd initial, Eigen::MatrixXd initial_cov, int measurements, Eigen::MatrixXd noise){
		Eigen::MatrixXd x(measurements,2), xe(measurements,2), ym(measurements,1), covariance(measurements,2);
		Eigen::MatrixXd gain;
		x.setZero(measurements,2);
		x.row(0) = initial;
		
		
		xe.setZero(measurements,2);
		
		ym.setZero(measurements,1);
		ym.row(0) = (basis*x.row(0).transpose()).transpose()+(noise.cwiseSqrt().row(0))*Eigen::MatrixXd::Random(1,1);

		covariance.setZero(measurements,2);
		covariance.row(0) = initial_cov.diagonal().transpose();
		
		
		
		// Main loop
		
		for(int i=0; i<(measurements-1); i++){
			
			// Truth and Measurements
			x.row(i+1) = (phi*x.row(i).transpose()+upsilon*(noise.cwiseSqrt().row(1))*Eigen::MatrixXd::Random(1,1)).transpose();
			ym.row(i+1) = (basis*x.row(i+1).transpose()).transpose()+(noise.cwiseSqrt().row(0))*Eigen::MatrixXd::Random(1,1);
			

			
			// Update Equations
			
			gain = initial_cov*basis.transpose()*((basis*initial_cov*basis.transpose())+noise.row(0)).cwiseInverse();
			initial_cov = (Eigen::MatrixXd::Identity(2,2)-gain*basis)*initial_cov;
			xe.row(i) = xe.row(i)+(gain*(ym.row(i)-basis*xe.row(i).transpose())).transpose();


			
			// Propagation Equations
			
			xe.row(i+1) = (phi*xe.row(i).transpose()).transpose();
			initial_cov = phi*initial_cov*phi.transpose()+upsilon*noise.row(1)*upsilon.transpose();
			covariance.row(i+1) = initial_cov.diagonal().transpose();
			
		}

		
		Eigen::MatrixXd result(measurements,6);
		result << xe, x, (covariance.cwiseSqrt())*3;
		
		return result;







}

// computes the optimal rigid body transformation given a set of points
void PoseTracker::computeOptimalRigidTransformation(Eigen::MatrixXd startP, Eigen::MatrixXd finalP)
{	
	Eigen::Matrix4d transf;
	
	if (startP.rows()!=finalP.rows())
	{	
		ROS_ERROR("We need that the rows be the same at the beggining");
		exit(1);
	}

	Eigen::RowVector3d centroid_startP = Eigen::RowVector3d::Zero(); 
	Eigen::RowVector3d centroid_finalP = Eigen::RowVector3d::Zero(); //= mean(B);
	Eigen::Matrix3d H = Eigen::Matrix3d::Zero();

	//calculate the mean
	for (int i=0;i<startP.rows();i++)
	{	
		centroid_startP = centroid_startP+startP.row(i);
		centroid_finalP = centroid_finalP+finalP.row(i);
	}
	
	centroid_startP = centroid_startP/startP.rows();
	centroid_finalP = centroid_finalP/startP.rows();

	for (int i=0;i<startP.rows();i++)
		H = H + (startP.row(i)-centroid_startP).transpose()*(finalP.row(i)-centroid_finalP);

   	Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
   
    Eigen::MatrixXd U = svd.matrixU();
   	Eigen::MatrixXd V = svd.matrixV();
  
    if (V.determinant()<0)
   		V.col(2)=-V.col(2)*(-1);

	Eigen::MatrixXd R = V*U.transpose();

	Eigen::Matrix4d C_A = Eigen::Matrix4d::Identity();
	Eigen::Matrix4d C_B = Eigen::Matrix4d::Identity();
	Eigen::Matrix4d R_new = Eigen::Matrix4d::Identity();
			
	C_A.block<3,1>(0,3) = -centroid_startP.transpose();
	R_new.block<3,3>(0,0) = R;
	
	C_B.block<3,1>(0,3) = centroid_finalP.transpose();


	transf = C_B * R_new * C_A;

	Eigen::Quaterniond mat_rot(transf.block<3,3>(0,0));

	Eigen::Vector3d trasl = transf.block<3,1>(0,3).transpose();

	transfParameters_ << trasl(0), trasl(1), trasl(2), mat_rot.x(), mat_rot.y(), mat_rot.z(), mat_rot.w();

}

void StompOptimizationTask::computeCosts(const Eigen::MatrixXd& features, Eigen::VectorXd& costs, Eigen::MatrixXd& weighted_feature_values) const
{
  weighted_feature_values = Eigen::MatrixXd::Zero(num_time_steps_, num_split_features_);
  for (int t=0; t<num_time_steps_; ++t)
  {
    weighted_feature_values.row(t) = (((features.row(t+stomp::TRAJECTORY_PADDING) - feature_means_.transpose()).array() /
        feature_variances_.array().transpose()) * feature_weights_.array().transpose()).matrix();
  }
  costs = weighted_feature_values.rowwise().sum();
}

void reflect_points3d(const Eigen::MatrixXd& ptsin, const Eigen::Vector3d& center, const Eigen::Vector3d& normal, Eigen::MatrixXd& ptsout)
{
    ptsout.resize(ptsin.rows(), ptsin.cols() );
    for ( int i = 0; i < (int) ptsin.rows(); ++i)
    {
        Eigen::Vector3d ptout;
        reflect_point3d(ptsin.row(i), center, normal, ptout);
        ptsout.row(i) = ptout;
    }
}