C++ Matrix::col方法代码示例

void GreenStrain_LIMSolver2D::computeHessian(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, const Eigen::Matrix<double*,Eigen::Dynamic,1>& hess)
{
  // green strain tensor energy
  Eigen::Matrix<double,2,3> S;
  for(int t=0;t<mesh->Triangles->rows();t++)
  {
    Eigen::Vector2d A(x[TriangleVertexIdx.coeff(0,t)],x[TriangleVertexIdx.coeff(1,t)]);
    Eigen::Vector2d B(x[TriangleVertexIdx.coeff(2,t)],x[TriangleVertexIdx.coeff(3,t)]);
    Eigen::Vector2d C(x[TriangleVertexIdx.coeff(4,t)],x[TriangleVertexIdx.coeff(5,t)]);

    Eigen::Matrix<double,2,3> V;
    V.col(0) = A;
    V.col(1) = B;
    V.col(2) = C;

    // hessian(E) = 4*r_x'*((SMM'V'V+VMM'*(V'S+SV))*MM' - SMM')*c_x
    Eigen::Matrix3d VTV = V.transpose()*V;
    Eigen::Matrix3d MMT = MMTs.block<3,3>(0,3*t);
    Eigen::Matrix<double,2,3> VMMT = V*MMT;
    Eigen::Matrix3d MMTVTV = MMT*VTV;

    int numElem = 0;
    for(int r=0;r<6;r++)
    {
      S = Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>::Zero(2,3);
      S.coeffRef(r) = 1;

      Eigen::Matrix<double,2,3> Temp = 4*((S*MMTVTV + VMMT*(V.transpose()*S+S.transpose()*V))*MMT - S*MMT);
      
      for(int c=r;c<6;c++)
        *denseHessianCoeffs(numElem++,t) += Temp.coeff(c)*Divider;
    }
  }
}

//==============================================================================
void FreeJoint::updateLocalJacobianTimeDeriv() const
{
  Eigen::Matrix<double, 6, 3> J;
  J.topRows<3>()    = Eigen::Matrix3d::Zero();
  J.bottomRows<3>() = Eigen::Matrix3d::Identity();

  const Eigen::Vector6d& positions = getPositionsStatic();
  const Eigen::Vector6d& velocities = getVelocitiesStatic();
  Eigen::Matrix<double, 6, 3> dJ;
  dJ.topRows<3>()    = math::expMapJacDot(positions.head<3>(),
                                          velocities.head<3>()).transpose();
  dJ.bottomRows<3>() = Eigen::Matrix3d::Zero();

  const Eigen::Isometry3d T = mT_ChildBodyToJoint
                              * math::expAngular(-positions.head<3>());

  mJacobianDeriv.leftCols<3>() = math::AdTJacFixed(mT_ChildBodyToJoint, dJ);
  const Eigen::Matrix<double, 6, 6>& Jacobian = getLocalJacobianStatic();
  mJacobianDeriv.col(3)
      = -math::ad(Jacobian.leftCols<3>() * velocities.head<3>(),
                  math::AdT(T, J.col(0)));
  mJacobianDeriv.col(4)
      = -math::ad(Jacobian.leftCols<3>() * velocities.head<3>(),
                  math::AdT(T, J.col(1)));
  mJacobianDeriv.col(5)
      = -math::ad(Jacobian.leftCols<3>() * velocities.head<3>(),
                  math::AdT(T, J.col(2)));
}

double GreenStrain_LIMSolver3D::computeFunction(const Eigen::Matrix<double,Eigen::Dynamic,1>& x)
{
  // green strain energy
  double shape = 0;
  Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
  for(int t=0;t<mesh->Tetrahedra->rows();t++)
  {
    Eigen::Vector3d A(x[TetrahedronVertexIdx.coeff(0,t)],x[TetrahedronVertexIdx.coeff(1,t)],x[TetrahedronVertexIdx.coeff(2,t)]);
    Eigen::Vector3d B(x[TetrahedronVertexIdx.coeff(3,t)],x[TetrahedronVertexIdx.coeff(4,t)],x[TetrahedronVertexIdx.coeff(5,t)]);
    Eigen::Vector3d C(x[TetrahedronVertexIdx.coeff(6,t)],x[TetrahedronVertexIdx.coeff(7,t)],x[TetrahedronVertexIdx.coeff(8,t)]);
    Eigen::Vector3d D(x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(11,t)]);

    Eigen::Matrix<double,3,4> V;
    V.col(0) = A;
    V.col(1) = B;
    V.col(2) = C;
    V.col(3) = D;

    Eigen::Matrix3d F = V*Ms.block<4,3>(0,3*t);
    Eigen::Matrix3d E = (F.transpose()*F - I);
    shape += E.squaredNorm()*Divider;
  }

  return shape;
}

Eigen::MatrixXd bb_overlap(
		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> const & bb,
		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> const & bb1, int x) {

	int N = bb.cols();
	int NN = bb1.cols();
	Eigen::VectorXd out(N);

	if (x == 1) {
		for (int j = 0; j < N; j++) {
			out(j) = bb_overlaphelper(bb.col(j), bb1.col(j));
		}
	} else {
		for (int j = 0; j < N; j++) {
			double maxOvrlp = 0;
			for (int i = 0; i < NN; i++) {
				double overlap = bb_overlaphelper(bb.col(j), bb1.col(i));
				if (overlap > maxOvrlp) {
					maxOvrlp = overlap;
					out(j) = i + 1.0;
				}
			}
		}
	}
	return out;
}

//==============================================================================
void PlanarJoint::updateLocalJacobianTimeDeriv() const
{
  Eigen::Matrix<double, 6, 3> J = Eigen::Matrix<double, 6, 3>::Zero();
  J.block<3, 1>(3, 0) = mPlanarP.mTransAxis1;
  J.block<3, 1>(3, 1) = mPlanarP.mTransAxis2;
  J.block<3, 1>(0, 2) = mPlanarP.mRotAxis;

  const Eigen::Matrix<double, 6, 3>& Jacobian = getLocalJacobianStatic();
  const Eigen::Vector3d& velocities = getVelocitiesStatic();
  mJacobianDeriv.col(0)
      = -math::ad(Jacobian.col(2) * velocities[2],
                  math::AdT(mJointP.mT_ChildBodyToJoint
                            * math::expAngular(mPlanarP.mRotAxis
                                               * -getPositionsStatic()[2]),
                            J.col(0)));

  mJacobianDeriv.col(1)
      = -math::ad(Jacobian.col(2) * velocities[2],
                  math::AdT(mJointP.mT_ChildBodyToJoint
                            * math::expAngular(mPlanarP.mRotAxis
                                               * -getPositionsStatic()[2]),
                            J.col(1)));

  assert(mJacobianDeriv.col(2) == Eigen::Vector6d::Zero());
  assert(!math::isNan(mJacobianDeriv.col(0)));
  assert(!math::isNan(mJacobianDeriv.col(1)));
}

Eigen::Matrix<double,3,3> Plane::getRotationMatrix(){
	Eigen::Matrix<double,3,3> rotMat;
	rotMat.col(0) = inertiaAxisX.normalized();
	rotMat.col(1) = inertiaAxisY.normalized();
	rotMat.col(2) = Eigen::Vector3d(normalVector[0], normalVector[1], normalVector[2]).normalized();
	return rotMat;
}

inline void igl::PlanarizerShapeUp<DerivedV, DerivedF>::assembleP()
{
  P.setZero(3*ni*numF);
  for (int fi = 0; fi< numF; fi++)
  {
    // todo: this can be made faster by omitting the selector matrix
    Eigen::SparseMatrix<typename DerivedV::Scalar > Sfi;
    assembleSelector(fi, Sfi);
    Eigen::SparseMatrix<typename DerivedV::Scalar > NSi = Ni*Sfi;
    
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> Vi = NSi*Vv;
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> CC(3,ni);
    for (int i = 0; i <ni; ++i)
      CC.col(i) = Vi.segment(3*i, 3);
    Eigen::Matrix<typename DerivedV::Scalar, 3, 3> C = CC*CC.transpose();
    
    // Alec: Doesn't compile
    Eigen::EigenSolver<Eigen::Matrix<typename DerivedV::Scalar, 3, 3>> es(C);
    // the real() is for compilation purposes
    Eigen::Matrix<typename DerivedV::Scalar, 3, 1> lambda = es.eigenvalues().real();
    Eigen::Matrix<typename DerivedV::Scalar, 3, 3> U = es.eigenvectors().real();
    int min_i;
    lambda.cwiseAbs().minCoeff(&min_i);
    U.col(min_i).setZero();
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> PP = U*U.transpose()*CC;
    for (int i = 0; i <ni; ++i)
     P.segment(3*ni*fi+3*i, 3) =  weightsSqrt[fi]*PP.col(i);
    
  }
}

 // transforms
 void transformW2F(Eigen::Matrix<double,3,4> &m, 
                   const Eigen::Matrix<double,4,1> &trans, 
                   const Eigen::Quaternion<double> &qrot)
 {
   m.block<3,3>(0,0) = qrot.toRotationMatrix();//.transpose();
   m.col(3).setZero();         // make sure there's no translation
   m.col(3) = -m*trans;
 };

 inline Eigen::Matrix<double, 1, C1>
 columns_dot_product(const Eigen::Matrix<double, R1, C1>& v1, 
                     const Eigen::Matrix<double, R2, C2>& v2) {
   validate_matching_sizes(v1,v2,"columns_dot_product");
   Eigen::Matrix<double, 1, C1> ret(1,v1.cols());
   for (size_type j = 0; j < v1.cols(); ++j) {
     ret(j) = v1.col(j).dot(v2.col(j));
   }
   return ret;
 }    

boost::shared_ptr<const Mantid::Geometry::IObject> createShape() {
  Eigen::Matrix<double, 3, 3> pointsDef;
  // Vector to base positioned at center of cylinder
  pointsDef.col(0) = Eigen::Vector3d(-0.00101, 0.0, 0.0);
  // vector to base which is radial
  pointsDef.col(1) = Eigen::Vector3d(-0.00101, 0.00405, 0.0);
  // vector to top positioned at center of cylinder
  pointsDef.col(2) = Eigen::Vector3d(0.00101, 0.0, 0.0);
  return NexusShapeFactory::createCylinder(pointsDef);
}

Pixels generateNonCoLinearPixels() {
  // Add two 4 cylinders which are not CoLinear
  Eigen::Matrix<double, 3, 4> pix;
  pix.col(0) = Eigen::Vector3d(0, 0.1, 0);
  pix.col(1) = Eigen::Vector3d(0.3, 0.6, 0.3);
  pix.col(2) = Eigen::Vector3d(-0.7, -0.7, 0);
  pix.col(3) = Eigen::Vector3d(1, 1.9, 0);

  return pix;
}

/* Follows section 3.1, however we're using the scaled unscented transform. */
void UnscentedKalmanFilter::create_sigma_points() {
    Eigen::Matrix<real_t, UKF_STATE_DIM, UKF_STATE_DIM> S;

    AssertNormalized(Quaternionr(state.attitude()));

    /* Add the process noise before calculating the square root. */
    state_covariance.diagonal() += process_noise_covariance;
    state_covariance *= UKF_DIM_PLUS_LAMBDA;

    AssertPositiveDefinite(state_covariance);

    /* Compute the 'square root' of the covariance matrix. */
    S = state_covariance.llt().matrixL();

    /* Create centre sigma point. */
    sigma_points.col(0) = state;

    /* For each column in S, create the two complementary sigma points. */
    for(uint8_t i = 0; i < UKF_STATE_DIM; i++) {
        /*
        Construct error quaternions using the MRP method, equation 34 from the
        Markley paper.
        */
        Vector3r d_p = S.col(i).segment<3>(9);
        real_t x_2 = d_p.squaredNorm();
        real_t err_w = (-UKF_MRP_A * x_2 +
            UKF_MRP_F * std::sqrt(UKF_MRP_F_2 + ((real_t)1.0 - UKF_MRP_A_2) * x_2)) /
            (UKF_MRP_F_2 + x_2);
        Vector3r err_xyz = (((real_t)1.0 / UKF_MRP_F) * (UKF_MRP_A + err_w)) * d_p;
        Quaternionr noise;
        noise.vec() = err_xyz;
        noise.w() = err_w;

        Quaternionr temp;

        /* Create positive sigma point. */
        sigma_points.col(i+1).segment<9>(0) =
            state.segment<9>(0) + S.col(i).segment<9>(0);
        temp = noise * Quaternionr(state.attitude());
        AssertNormalized(temp);
        sigma_points.col(i+1).segment<4>(9) << temp.vec(), temp.w();
        sigma_points.col(i+1).segment<12>(13) =
            state.segment<12>(13) + S.col(i).segment<12>(12);

        /* Create negative sigma point. */
        sigma_points.col(i+1+UKF_STATE_DIM).segment<9>(0) =
            state.segment<9>(0) - S.col(i).segment<9>(0);
        temp = noise.conjugate() * Quaternionr(state.attitude());
        AssertNormalized(temp);
        sigma_points.col(i+1+UKF_STATE_DIM).segment<4>(9) <<
            temp.vec(), temp.w();
        sigma_points.col(i+1+UKF_STATE_DIM).segment<12>(13) =
            state.segment<12>(13) - S.col(i).segment<12>(12);
    }
}

 inline Eigen::Matrix<double, 1, C1>
 columns_dot_product(const Eigen::Matrix<double, R1, C1>& v1, 
                     const Eigen::Matrix<double, R2, C2>& v2) {
   stan::math::check_matching_sizes("columns_dot_product",
                                              "v1", v1,
                                              "v2", v2);
   Eigen::Matrix<double, 1, C1> ret(1,v1.cols());
   for (size_type j = 0; j < v1.cols(); ++j) {
     ret(j) = v1.col(j).dot(v2.col(j));
   }
   return ret;
 }    

void IDIM::computeY(const MultiBody& mb, const MultiBodyConfig& mbc)
{
	const std::vector<Body>& bodies = mb.bodies();
	const std::vector<Joint>& joints = mb.joints();
	const std::vector<int>& pred = mb.predecessors();

	Eigen::Matrix<double, 6, 10> bodyFPhi;
	for(int i = static_cast<int>(bodies.size()) - 1; i >= 0; --i)
	{
		const sva::MotionVecd& vb_i = mbc.bodyVelB[i];
		Eigen::Matrix<double, 6, 10> vb_i_Phi(IMPhi(vb_i));

		bodyFPhi.noalias() = IMPhi(mbc.bodyAccB[i]);
		// bodyFPhi += vb_i x* IMPhi(vb_i)
		// is faster to convert each col in a ForceVecd
		// than using sva::vector6ToCrossDualMatrix
		for(int c = 0; c < 10; ++c)
		{
			bodyFPhi.col(c).noalias() +=
				(vb_i.crossDual(sva::ForceVecd(vb_i_Phi.col(c)))).vector();
		}

		int iDofPos = mb.jointPosInDof(i);

		Y_.block(iDofPos, i*10, joints[i].dof(), 10).noalias() =
			mbc.motionSubspace[i].transpose()*bodyFPhi;

		int j = i;
		while(pred[j] != -1)
		{
			const sva::PTransformd& X_p_j = mbc.parentToSon[j];
			// bodyFPhi = X_p_j^T bodyFPhi
			// is faster to convert each col in a ForceVecd
			// than using X_p_j.inv().dualMatrix()
			for(int c = 0; c < 10; ++c)
			{
				bodyFPhi.col(c) =
					X_p_j.transMul(sva::ForceVecd(bodyFPhi.col(c))).vector();
			}
			j = pred[j];

			int jDofPos = mb.jointPosInDof(j);
			if(joints[j].dof() != 0)
			{
				Y_.block(jDofPos, i*10, joints[j].dof(), 10).noalias() =
					mbc.motionSubspace[j].transpose()*bodyFPhi;
			}
		}
	}
}

//==============================================================================
BallJoint::BallJoint(const std::string& _name)
    : MultiDofJoint(_name),
      mR(Eigen::Isometry3d::Identity())
{
    // Jacobian
    Eigen::Matrix<double, 6, 3> J = Eigen::Matrix<double, 6, 3>::Zero();
    J.topRows<3>() = Eigen::Matrix3d::Identity();
    mJacobian.col(0) = math::AdT(mT_ChildBodyToJoint, J.col(0));
    mJacobian.col(1) = math::AdT(mT_ChildBodyToJoint, J.col(1));
    mJacobian.col(2) = math::AdT(mT_ChildBodyToJoint, J.col(2));
    assert(!math::isNan(mJacobian));

    // Time derivative of Jacobian is always zero
}