C++ eigen::Matrix类代码示例

void cv2eigen( const Matx<_Tp, 1, _cols>& src,
               Eigen::Matrix<_Tp, 1, Eigen::Dynamic>& dst )
{
    dst.resize(_cols);
    if( !(dst.Flags & Eigen::RowMajorBit) )
    {
        Mat _dst(_cols, 1, DataType<_Tp>::type,
                 dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
        transpose(src, _dst);
        CV_DbgAssert(_dst.data == (uchar*)dst.data());
    }
    else
    {
        Mat _dst(1, _cols, DataType<_Tp>::type,
                 dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
        Mat(src).copyTo(_dst);
        CV_DbgAssert(_dst.data == (uchar*)dst.data());
    }
}

template <typename PointT, typename Scalar> inline void
pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 
                        const std::vector<int> &indices,
                        Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
{
  typedef typename pcl::traits::fieldList<PointT>::type FieldList;

  // Get the size of the fields
  centroid.setZero (boost::mpl::size<FieldList>::value);

  if (indices.empty ())
    return;
  // Iterate over each point
  int nr_points = static_cast<int> (indices.size ());
  for (int i = 0; i < nr_points; ++i)
  {
    // Iterate over each dimension
    pcl::for_each_type <FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[indices[i]], centroid));
  }
  centroid /= static_cast<Scalar> (nr_points);
}

    Eigen::Matrix<T, Eigen::Dynamic, 1>
    positive_ordered_free(const Eigen::Matrix<T, Eigen::Dynamic, 1>& y) {
      using Eigen::Matrix;
      using Eigen::Dynamic;
      using stan::math::index_type;

      typedef typename index_type<Matrix<T, Dynamic, 1> >::type size_type;

      stan::math::check_positive_ordered("stan::math::positive_ordered_free",
                                                   "Positive ordered variable",
                                                   y);

      size_type k = y.size();
      Matrix<T, Dynamic, 1> x(k);
      if (k == 0)
        return x;
      x[0] = log(y[0]);
      for (size_type i = 1; i < k; ++i)
        x[i] = log(y[i] - y[i-1]);
      return x;
    }

TEST(ErrorHandlingMatrix, checkMatchingDimsMatrix) {
  Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> y;
  Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> x;
  
  y.resize(3,3);
  x.resize(3,3);
  EXPECT_TRUE(stan::math::check_matching_dims("checkMatchingDims", "x", x,
                                                        "y", y));
  x.resize(0,0);
  y.resize(0,0);
  EXPECT_TRUE(stan::math::check_matching_dims("checkMatchingDims", "x", x,
                                                        "y", y));

  y.resize(1,2);
  EXPECT_THROW(stan::math::check_matching_dims("checkMatchingDims", "x", x,
                                                         "y", y), 
               std::invalid_argument);

  x.resize(2,1);
  EXPECT_THROW(stan::math::check_matching_dims("checkMatchingDims", "x", x,
                                                         "y", y), 
               std::invalid_argument);
}

 inline 
 Eigen::Matrix<fvar<T>,R1,C2> 
 multiply(const Eigen::Matrix<fvar<T>,R1,C1>& m1,
          const Eigen::Matrix<double,R2,C2>& m2) {
   stan::math::validate_multiplicable(m1,m2,"multiply");
   Eigen::Matrix<fvar<T>,R1,C2>
     result(m1.rows(),m2.cols());
   for (size_type i = 0; i < m1.rows(); i++) {
     Eigen::Matrix<fvar<T>,1,C1> crow = m1.row(i);
     for (size_type j = 0; j < m2.cols(); j++) {
       Eigen::Matrix<double,R2,1> ccol = m2.col(j);
       result(i,j) = stan::agrad::dot_product(crow,ccol);
     }
   }
   return result;
 }

TEST(ErrorHandlingMatrix, checkMultiplicableMatrix) {
  Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> y;
  Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> x;
  
  y.resize(3,3);
  x.resize(3,3);
  EXPECT_TRUE(stan::math::check_multiplicable("checkMultiplicable", "x", x,
                                                        "y", y));
  x.resize(3,2);
  y.resize(2,4);
  EXPECT_TRUE(stan::math::check_multiplicable("checkMultiplicable", "x", x,
                                                        "y", y));

  y.resize(1,2);
  EXPECT_THROW(stan::math::check_multiplicable("checkMultiplicable", "x", x,
                                                         "y", y), 
               std::invalid_argument);

  x.resize(2,2);
  EXPECT_THROW(stan::math::check_multiplicable("checkMultiplicable", "x", x,
                                                         "y", y), 
               std::invalid_argument);
}

template <typename PointT, typename Scalar> void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                          const std::vector<int> &indices, 
                          pcl::PointCloud<PointT> &cloud_out,
                          const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
  size_t npts = indices.size ();
  // In order to transform the data, we need to remove NaNs
  cloud_out.is_dense = cloud_in.is_dense;
  cloud_out.header   = cloud_in.header;
  cloud_out.width    = static_cast<int> (npts);
  cloud_out.height   = 1;
  cloud_out.points.resize (npts);

  if (cloud_in.is_dense)
  {
    // If the dataset is dense, simply transform it!
    for (size_t i = 0; i < npts; ++i)
    {
      // Copy fields first, then transform xyz data
      //cloud_out.points[i] = cloud_in.points[indices[i]]; 
      //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
    }
  }
  else
  {
    // Dataset might contain NaNs and Infs, so check for them first,
    // otherwise we get errors during the multiplication (?)
    for (size_t i = 0; i < npts; ++i)
    {
      if (!pcl_isfinite (cloud_in.points[indices[i]].x) || 
          !pcl_isfinite (cloud_in.points[indices[i]].y) || 
          !pcl_isfinite (cloud_in.points[indices[i]].z))
        continue;
      //cloud_out.points[i] = cloud_in.points[indices[i]]; 
      //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
    }
  }
}

Eigen::Matrix<T, -1, 1> getPolynomialVariables(const Eigen::Matrix<T, -1, 1> &vars,
                                               const size_t & degree)
{


    typedef Eigen::Matrix<T, -1, 1> Vector;
    typedef Eigen::Matrix<T, -1, -1, Eigen::RowMajor> Matrix;

    auto expand_to_degree = [](const float &x, const int &degree)
    {
        Vector out(degree+1);

        for (int i = 0; i <= degree ; ++i)
            out(i)  = pow(x, i);

        return out;
    };


    Vector current (1);
    current << 1;

    for (int i = 0; i < vars.rows() ; ++i)
    {
        // we expand to the given degree the variable
        Vector expanded = expand_to_degree(vars(i), degree);

        Matrix mul = current * expanded.transpose();


        current.resize(mul.size());

        current = Eigen::Map<Vector> (mul.data(), mul.size());

    }

    return current;

}

		void
		MitsubishiH7::setMotorPulse(const ::Eigen::Matrix< ::std::int32_t, ::Eigen::Dynamic, 1>& p)
		{
			assert(p.size() >= this->getDof());
			
			this->out.dat2.pls.p1 = 0;
			this->out.dat2.pls.p2 = 0;
			this->out.dat2.pls.p3 = 0;
			this->out.dat2.pls.p4 = 0;
			this->out.dat2.pls.p5 = 0;
			this->out.dat2.pls.p6 = 0;
			this->out.dat2.pls.p7 = 0;
			this->out.dat2.pls.p8 = 0;
			
			switch (this->getDof())
			{
			case 8:
				this->out.dat2.pls.p8 = p(7);
			case 7:
				this->out.dat2.pls.p7 = p(6);
			case 6:
				this->out.dat2.pls.p6 = p(5);
			case 5:
				this->out.dat2.pls.p5 = p(4);
			case 4:
				this->out.dat2.pls.p4 = p(3);
			case 3:
				this->out.dat2.pls.p3 = p(2);
			case 2:
				this->out.dat2.pls.p2 = p(1);
			case 1:
				this->out.dat2.pls.p1 = p(0);
			default:
				break;
			}
			
			this->out.command = MXT_COMMAND_MOVE;
			this->out.sendType = MXT_TYPE_PULSE;
		}

void check_velocities(const double vmin_[N_MOTORS], const double vmax_[N_MOTORS], const Eigen::Matrix <double,
                      N_SEGMENTS, N_MOTORS> & m3w_, const Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> & m2w_, const Eigen::Matrix <
                      double, N_SEGMENTS, N_MOTORS> & m1w_)
{
#if 0
    cout << "vmin:\n";
    for (int mtr = 0; mtr < N_MOTORS; ++mtr) {
        cout << vmin_[mtr] << " ";
    }
    cout << endl;
    cout << "vmax:\n";
    for (int mtr = 0; mtr < N_MOTORS; ++mtr) {
        cout << vmax_[mtr] << " ";
    }
    cout << endl;
#endif

    // Compute extreme velocities for all segments and motors (at once! - mi low eigen;)).
    Eigen::Matrix <double, N_SEGMENTS, N_MOTORS> v_extremum = (m2w_.cwise() * m2w_).cwise() / (3.0 * m3w_) + m1w_;
    // Correct all NANs.
    for (int sgt = 0; sgt < N_SEGMENTS; ++sgt)
        for (int mtr = 0; mtr < N_MOTORS; ++mtr) {
            if (m3w_(sgt, mtr) == 0.0)
                v_extremum(sgt, mtr) = m1w_(sgt, mtr);
        }

#if 0
    cout << "v_extremum:\n" << v_extremum << endl;
#endif

    // Check conditions for all segments and motors.
    for (int sgt = 0; sgt < N_SEGMENTS; ++sgt)
        for (int mtr = 0; mtr < N_MOTORS; ++mtr) {
            if (v_extremum(sgt, mtr) > vmax_[mtr])
                BOOST_THROW_EXCEPTION(nfe_motor_velocity_constraint_exceeded() << constraint_type(MAXIMUM_CONSTRAINT) << motor_number(mtr) << segment_number(sgt) << constraint_value(vmax_[mtr]) << desired_value(v_extremum(sgt, mtr)));
            else if (v_extremum(sgt, mtr) < vmin_[mtr])
                BOOST_THROW_EXCEPTION(nfe_motor_velocity_constraint_exceeded() << constraint_type(MINIMUM_CONSTRAINT) << motor_number(mtr) << segment_number(sgt) << constraint_value(vmin_[mtr]) << desired_value(v_extremum(sgt, mtr)));
        }
}

void PositionCommand::getGoal(const geometry_msgs::PoseStamped::ConstPtr & goal)
{

    Eigen::Matrix<double,3,1> euler = Eigen::Quaterniond(goal->pose.orientation.w,
                                                         goal->pose.orientation.x,
                                                         goal->pose.orientation.y,
                                                         goal->pose.orientation.z).matrix().eulerAngles(2, 1, 0);
    double yaw   = euler(0,0);
    double pitch = euler(1,0);
    double roll  = euler(2,0);

    goal_pose_ <<   goal->pose.position.x,
            goal->pose.position.y,
            goal->pose.position.z,
            roll,
            pitch,
            yaw;

    ROS_INFO("GOT NEW GOAL");
    std::cout << "goal_pose: " << goal_pose_.transpose()<< std::endl;

}

    inline 
    Eigen::Matrix<fvar<T>,R1,C2>
    mdivide_right(const Eigen::Matrix<fvar<T>,R1,C1> &A,
                  const Eigen::Matrix<double,R2,C2> &b) {
      
      using stan::math::multiply;      
      using stan::math::mdivide_right;
      stan::math::validate_square(b,"mdivide_right");
      stan::math::validate_multiplicable(A,b,"mdivide_right");

      Eigen::Matrix<T,R2,C2> deriv_b_mult_inv_b(b.rows(),b.cols());
      Eigen::Matrix<T,R1,C1> val_A(A.rows(),A.cols()); 
      Eigen::Matrix<T,R1,C1> deriv_A(A.rows(),A.cols());

      for (int j = 0; j < A.cols(); j++) {
        for(int i = 0; i < A.rows(); i++) {
          val_A(i,j) = A(i,j).val_;
          deriv_A(i,j) = A(i,j).d_;
        }
      }

      return stan::agrad::to_fvar(mdivide_right(val_A, b), 
                                  mdivide_right(deriv_A, b));
    }

 /* ctor for cholesky function
  *
  * Stores varis for A
  * Instantiates and stores varis for L
  * Instantiates and stores dummy vari for
  * upper triangular part of var result returned
  * in cholesky_decompose function call
  *
  * variRefL aren't on the chainable
  * autodiff stack, only used for storage
  * and computation. Note that varis for
  * L are constructed externally in
  * cholesky_decompose.
  *
  * @param matrix A
  * @param matrix L, cholesky factor of A
  * */
 cholesky_decompose_v_vari(const Eigen::Matrix<var, -1, -1>& A,
                           const Eigen::Matrix<double, -1, -1>& L_A)
   : vari(0.0),
     M_(A.rows()),
     variRefA_(ChainableStack::memalloc_.alloc_array<vari*>
               (A.rows() * (A.rows() + 1) / 2)),
     variRefL_(ChainableStack::memalloc_.alloc_array<vari*>
               (A.rows() * (A.rows() + 1) / 2)) {
   size_t accum = 0;
   size_t accum_i = accum;
   for (size_type j = 0; j < M_; ++j) {
     for (size_type i = j; i < M_; ++i) {
       accum_i += i;
       size_t pos = j + accum_i;
       variRefA_[pos] = A.coeffRef(i, j).vi_;
       variRefL_[pos] = new vari(L_A.coeffRef(i, j), false);
     }
     accum += j;
     accum_i = accum;
   }
 }

void Element::CalculateStiffnessMatrix(const Eigen::Matrix3f& D, std::vector<Eigen::Triplet<float> >& triplets)
{
	Eigen::Vector3f x, y;
	x << nodesX[nodesIds[0]], nodesX[nodesIds[1]], nodesX[nodesIds[2]];
	y << nodesY[nodesIds[0]], nodesY[nodesIds[1]], nodesY[nodesIds[2]];
	
	Eigen::Matrix3f C;
	C << Eigen::Vector3f(1.0f, 1.0f, 1.0f), x, y;
	
	Eigen::Matrix3f IC = C.inverse();

	for (int i = 0; i < 3; i++)
	{
		B(0, 2 * i + 0) = IC(1, i);
		B(0, 2 * i + 1) = 0.0f;
		B(1, 2 * i + 0) = 0.0f;
		B(1, 2 * i + 1) = IC(2, i);
		B(2, 2 * i + 0) = IC(2, i);
		B(2, 2 * i + 1) = IC(1, i);
	}
	Eigen::Matrix<float, 6, 6> K = B.transpose() * D * B * C.determinant() / 2.0f;

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			Eigen::Triplet<float> trplt11(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 0, K(2 * i + 0, 2 * j + 0));
			Eigen::Triplet<float> trplt12(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 1, K(2 * i + 0, 2 * j + 1));
			Eigen::Triplet<float> trplt21(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 0, K(2 * i + 1, 2 * j + 0));
			Eigen::Triplet<float> trplt22(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 1, K(2 * i + 1, 2 * j + 1));

			triplets.push_back(trplt11);
			triplets.push_back(trplt12);
			triplets.push_back(trplt21);
			triplets.push_back(trplt22);
		}
	}
}

    inline Eigen::VectorXd
    multi_student_t_rng(const double nu,
                        const Eigen::Matrix<double, Dynamic, 1>& mu,
                        const Eigen::Matrix<double, Dynamic, Dynamic>& s,
                        RNG& rng) {
      static const char* function("stan::prob::multi_student_t_rng");

      using stan::math::check_finite;
      using stan::math::check_not_nan;
      using stan::math::check_symmetric;
      using stan::math::check_positive;

      check_finite(function, "Location parameter", mu);
      check_symmetric(function, "Scale parameter", s);
      check_not_nan(function, "Degrees of freedom parameter", nu);
      check_positive(function, "Degrees of freedom parameter", nu);

      Eigen::VectorXd z(s.cols());
      z.setZero();

      double w = stan::prob::inv_gamma_rng(nu / 2, nu / 2, rng);
      return mu + std::sqrt(w) * stan::prob::multi_normal_rng(z, s, rng);
    }