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

void ChompOptimizer::getJacobian(int trajectory_point,
                                 Eigen::Vector3d& collision_point_pos,
                                 std::string& joint_name,
                                 Eigen::MatrixBase<Derived>& jacobian) const
{
  for(int j = 0; j < num_joints_; j++)
  {
    if(isParent(joint_name, joint_names_[j]))
    {
      Eigen::Vector3d column = joint_axes_[trajectory_point][j].cross(Eigen::Vector3d(collision_point_pos(0),
                                                                                      collision_point_pos(1),
                                                                                      collision_point_pos(2))
                                                                      - joint_positions_[trajectory_point][j]);

      jacobian.col(j)[0] = column.x();
      jacobian.col(j)[1] = column.y();
      jacobian.col(j)[2] = column.z();
    }
    else
    {
      jacobian.col(j)[0] = 0.0;
      jacobian.col(j)[1] = 0.0;
      jacobian.col(j)[2] = 0.0;
    }
  }
}

void LSTM::compute_intermediate(const Eigen::MatrixBase<Derived> &w_mat)
{

	for(int i = 0; i < intermediate.cols(); i++)
	{
		if( input(i) != -1 )
			intermediate.col(i) = w_mat.col( input(i) );
		else
			intermediate.col(i) = w_mat.col(0);
	}
}

         bool CameraModel::_project_intrinsic( const Eigen::MatrixBase<Derived1>& mP3D,
                                               Eigen::MatrixBase<Derived2>& mP2D,
                                               Eigen::MatrixBase<Derived3>& mdP2DI
                                               ) const { 
     if( !project( mP3D, mP2D ) ) {
         return false;
     }
     const unsigned int nNumPoints = mP3D.cols();
     const unsigned int nNumCamParams = get_num_parameters();
     // Compute finite-diff Jacobians
     Derived3 mdP2Dfd( 2, nNumPoints );
     Derived3 mP2Dfd( 2, nNumPoints );
     double dEps = 1e-4;
     Derived2 mP2DP = mP2D;
     Derived2 mP2DM = mP2D;
     { 
         // Compute intrinsic Jacobian
         // Horrible const_cast ... difficult to avoid
         for( size_t kk=0; kk<nNumCamParams; kk++ ) {
             std::string sVarName = parameter_index_to_name( kk );
             double dVal = get<double>( sVarName );
             const_cast<CameraModel*>( this )->set<double>( sVarName, dVal + dEps );
             project( mP3D, mP2DP );
             const_cast<CameraModel*>( this )->set<double>( sVarName, dVal - dEps );
             project( mP3D, mP2DM );
             mdP2Dfd = ( mP2DP - mP2DM ) / (2*dEps);
             const_cast<CameraModel*>( this )->set<double>( sVarName, dVal );
             for( size_t ll=0; ll<nNumPoints; ll++ ) {
                 mdP2DI.col( nNumCamParams*ll + kk ) = mdP2Dfd.col( ll );
             }
         }
     }
     return true;
 }

         bool CameraModel::_project_extrinsic( const Eigen::MatrixBase<Derived1>& mP3D,
                                               Eigen::MatrixBase<Derived2>& mP2D,
                                               Eigen::MatrixBase<Derived3>& mdP2DE
                                               ) const {
     if( !project( mP3D, mP2D ) ) {
         return false;
     }
     const unsigned int nNum3DDims = 3;
     const unsigned int nNumPoints = mP3D.cols();
     // Compute finite-diff Jacobians
     Derived3 mdP2Dfd( 2, nNumPoints );
     Derived3 mP2Dfd( 2, nNumPoints );
     typename Eigen::internal::traits<Derived1>::Scalar dEps = 1e-4;
     Derived2 mP2DP = mP2D;
     Derived2 mP2DM = mP2D;
     { 
         // Compute extrinsic Jacobian
         for( size_t kk=0; kk<nNum3DDims; kk++ ) {
             Derived1 mP3DP = mP3D;
             Derived1 mP3DM = mP3D;
             mP3DP.row( kk ).array() += dEps;
             mP3DM.row( kk ).array() -= dEps;
             project( mP3DP, mP2DP );
             project( mP3DM, mP2DM );
             Derived2 mdP2Dfd = ( mP2DP - mP2DM ) / (2*dEps);
             for( size_t ll=0; ll<nNumPoints; ll++ ) {
                 mdP2DE.col( nNum3DDims*ll + kk ) = mdP2Dfd.col( ll );
             }
         }
     }
     return true;
 }

 void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
 {
   data.U = I.col(Inertia::ANGULAR + axis);
   data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
   data.UDinv.noalias() = data.U * data.Dinv[0];
   
   if (update_I)
     PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
 }

     bool CameraModel::project_trans
     ( const Eigen::MatrixBase<Derived1>& mR,
       const Eigen::MatrixBase<Derived2>& mt,
       const Eigen::MatrixBase<Derived3>& mP3D,
       Eigen::MatrixBase<Derived4>& mP2D,  
       Eigen::MatrixBase<Derived5>& mdP2DE,
       Eigen::MatrixBase<Derived6>& mdP2DI 
       ) const {
     typedef typename Eigen::internal::traits<Derived4>::Scalar Derived4Type;
     typedef typename Eigen::internal::traits<Derived5>::Scalar Derived5Type;
     typedef Eigen::Matrix<Derived4Type, 1, 3> Derived4Vec;
     assert( mP3D.rows() == 3 );
     assert( mP2D.rows() == 2 );
     assert( mP3D.cols() == mP2D.cols() );
     assert( mR.rows() == 3 );
     assert( mR.cols() == 3 );
     assert( mt.rows() == 3 );
     assert( mt.cols() == 1 );
     Derived3 mP3DT = mR * mP3D;
     mP3DT.colwise() += mt;
     Eigen::Matrix<Derived5Type,2,Eigen::Dynamic,Derived5::Base::Options> mdP2DP3D( 2, 3*mP3D.cols() );
     bool bSuccess = project( mP3DT, mP2D, mdP2DP3D, mdP2DI );
     if( !bSuccess ) { return bSuccess; }
     Derived4 mdR0 = mR*CEIGEN::skew_rot<Derived4>(1,0,0);
     Derived4 mdR1 = mR*CEIGEN::skew_rot<Derived4>(0,1,0);
     Derived4 mdR2 = mR*CEIGEN::skew_rot<Derived4>(0,0,1);
         
     for( int ii=0; ii<mP3D.cols(); ii++ ) {
         // Chain rule: right part of the extrinsic matrix
         // computation related to the rotation and translation
         Derived4 mdRtX( 3, 6 );
         // Rotation part
         mdRtX.block(0,0,3,1) = mdR0*mP3D.col( ii );
         mdRtX.block(0,1,3,1) = mdR1*mP3D.col( ii );
         mdRtX.block(0,2,3,1) = mdR2*mP3D.col( ii );
         // Translation part
         mdRtX.block(0,3,3,3) = Derived4::Identity(3,3);
         // Combine extrinsic Jacobian with position Jacobian (right hand side)
         mdP2DE.block(0,6*ii,2,6) = mdP2DP3D.block(0,3*ii,2,3) * mdRtX;
     }
     return true;
 }

void doSomeRankPreservingOperations(Eigen::MatrixBase<Derived>& m)
{
  typedef typename Derived::RealScalar RealScalar;
  for(int a = 0; a < 3*(m.rows()+m.cols()); a++)
  {
    RealScalar d = Eigen::ei_random<RealScalar>(-1,1);
    int i = Eigen::ei_random<int>(0,m.rows()-1); // i is a random row number
    int j;
    do {
      j = Eigen::ei_random<int>(0,m.rows()-1);
    } while (i==j); // j is another one (must be different)
    m.row(i) += d * m.row(j);

    i = Eigen::ei_random<int>(0,m.cols()-1); // i is a random column number
    do {
      j = Eigen::ei_random<int>(0,m.cols()-1);
    } while (i==j); // j is another one (must be different)
    m.col(i) += d * m.col(j);
  }
}

IGL_INLINE void igl::face_areas(
  const Eigen::MatrixBase<DerivedL>& L,
  const typename DerivedL::Scalar doublearea_nan_replacement,
  Eigen::PlainObjectBase<DerivedA>& A)
{
  using namespace Eigen;
  assert(L.cols() == 6);
  const int m = L.rows();
  // (unsigned) face Areas (opposite vertices: 1 2 3 4)
  Matrix<typename DerivedA::Scalar,Dynamic,1> 
    A0(m,1), A1(m,1), A2(m,1), A3(m,1);
  Matrix<typename DerivedA::Scalar,Dynamic,3> 
    L0(m,3), L1(m,3), L2(m,3), L3(m,3);
  L0<<L.col(1),L.col(2),L.col(3);
  L1<<L.col(0),L.col(2),L.col(4);
  L2<<L.col(0),L.col(1),L.col(5);
  L3<<L.col(3),L.col(4),L.col(5);
  doublearea(L0,doublearea_nan_replacement,A0);
  doublearea(L1,doublearea_nan_replacement,A1);
  doublearea(L2,doublearea_nan_replacement,A2);
  doublearea(L3,doublearea_nan_replacement,A3);
  A.resize(m,4);
  A.col(0) = 0.5*A0;
  A.col(1) = 0.5*A1;
  A.col(2) = 0.5*A2;
  A.col(3) = 0.5*A3;
}

void LSTM::setupForwardProp(const Eigen::MatrixBase<Derived> &prev_h, 
						const Eigen::MatrixBase<Derived> &prev_c, 
						const Eigen::MatrixBase<Derived2> &inputMiniBatch, 
						int index)
{


	this->prev_h.noalias() = prev_h;
	this->prev_c.noalias() = prev_c;
	this->input = inputMiniBatch.col(index);
	intermediate.setZero(intermediate.rows(), intermediate.cols());

}

template<typename Derived> inline static int changeBase(Eigen::MatrixBase<Derived>& J, const Frame& T) {

    if (J.rows() != 6)
        return -1;
    for (int j = 0; j < J.cols(); ++j) {
		typename Derived::ColXpr Jj = J.col(j);
		Twist arg;
        for(unsigned int i=0;i<6;++i)
            arg(i)=Jj[i];
		Twist tmp(T*arg);
        for(unsigned int i=0;i<6;++i)
            Jj[i]=e_scalar(tmp(i));
    }
    return 0;
}

IGL_INLINE void igl::massmatrix(
  const Eigen::MatrixBase<DerivedV> & V, 
  const Eigen::MatrixBase<DerivedF> & F, 
  const MassMatrixType type,
  Eigen::SparseMatrix<Scalar>& M)
{
  using namespace Eigen;
  using namespace std;

  const int n = V.rows();
  const int m = F.rows();
  const int simplex_size = F.cols();

  MassMatrixType eff_type = type;
  // Use voronoi of for triangles by default, otherwise barycentric
  if(type == MASSMATRIX_TYPE_DEFAULT)
  {
    eff_type = (simplex_size == 3?MASSMATRIX_TYPE_VORONOI:MASSMATRIX_TYPE_BARYCENTRIC);
  }

  // Not yet supported
  assert(type!=MASSMATRIX_TYPE_FULL);

  Matrix<int,Dynamic,1> MI;
  Matrix<int,Dynamic,1> MJ;
  Matrix<Scalar,Dynamic,1> MV;
  if(simplex_size == 3)
  {
    // Triangles
    // edge lengths numbered same as opposite vertices
    Matrix<Scalar,Dynamic,3> l(m,3);
    // loop over faces
    for(int i = 0;i<m;i++)
    {
      l(i,0) = (V.row(F(i,1))-V.row(F(i,2))).norm();
      l(i,1) = (V.row(F(i,2))-V.row(F(i,0))).norm();
      l(i,2) = (V.row(F(i,0))-V.row(F(i,1))).norm();
    }
    Matrix<Scalar,Dynamic,1> dblA;
    doublearea(l,dblA);

    switch(eff_type)
    {
      case MASSMATRIX_TYPE_BARYCENTRIC:
        // diagonal entries for each face corner
        MI.resize(m*3,1); MJ.resize(m*3,1); MV.resize(m*3,1);
        MI.block(0*m,0,m,1) = F.col(0);
        MI.block(1*m,0,m,1) = F.col(1);
        MI.block(2*m,0,m,1) = F.col(2);
        MJ = MI;
        repmat(dblA,3,1,MV);
        MV.array() /= 6.0;
        break;
      case MASSMATRIX_TYPE_VORONOI:
        {
          // diagonal entries for each face corner
          // http://www.alecjacobson.com/weblog/?p=874
          MI.resize(m*3,1); MJ.resize(m*3,1); MV.resize(m*3,1);
          MI.block(0*m,0,m,1) = F.col(0);
          MI.block(1*m,0,m,1) = F.col(1);
          MI.block(2*m,0,m,1) = F.col(2);
          MJ = MI;

          // Holy shit this needs to be cleaned up and optimized
          Matrix<Scalar,Dynamic,3> cosines(m,3);
          cosines.col(0) = 
            (l.col(2).array().pow(2)+l.col(1).array().pow(2)-l.col(0).array().pow(2))/(l.col(1).array()*l.col(2).array()*2.0);
          cosines.col(1) = 
            (l.col(0).array().pow(2)+l.col(2).array().pow(2)-l.col(1).array().pow(2))/(l.col(2).array()*l.col(0).array()*2.0);
          cosines.col(2) = 
            (l.col(1).array().pow(2)+l.col(0).array().pow(2)-l.col(2).array().pow(2))/(l.col(0).array()*l.col(1).array()*2.0);
          Matrix<Scalar,Dynamic,3> barycentric = cosines.array() * l.array();
          normalize_row_sums(barycentric,barycentric);
          Matrix<Scalar,Dynamic,3> partial = barycentric;
          partial.col(0).array() *= dblA.array() * 0.5;
          partial.col(1).array() *= dblA.array() * 0.5;
          partial.col(2).array() *= dblA.array() * 0.5;
          Matrix<Scalar,Dynamic,3> quads(partial.rows(),partial.cols());
          quads.col(0) = (partial.col(1)+partial.col(2))*0.5;
          quads.col(1) = (partial.col(2)+partial.col(0))*0.5;
          quads.col(2) = (partial.col(0)+partial.col(1))*0.5;

          quads.col(0) = (cosines.col(0).array()<0).select( 0.25*dblA,quads.col(0));
          quads.col(1) = (cosines.col(0).array()<0).select(0.125*dblA,quads.col(1));
          quads.col(2) = (cosines.col(0).array()<0).select(0.125*dblA,quads.col(2));

          quads.col(0) = (cosines.col(1).array()<0).select(0.125*dblA,quads.col(0));
          quads.col(1) = (cosines.col(1).array()<0).select(0.25*dblA,quads.col(1));
          quads.col(2) = (cosines.col(1).array()<0).select(0.125*dblA,quads.col(2));

          quads.col(0) = (cosines.col(2).array()<0).select(0.125*dblA,quads.col(0));
          quads.col(1) = (cosines.col(2).array()<0).select(0.125*dblA,quads.col(1));
          quads.col(2) = (cosines.col(2).array()<0).select( 0.25*dblA,quads.col(2));

          MV.block(0*m,0,m,1) = quads.col(0);
          MV.block(1*m,0,m,1) = quads.col(1);
          MV.block(2*m,0,m,1) = quads.col(2);
          
          break;
        }
//.........这里部分代码省略.........

 inline const typename M6Like::ConstColXpr
 operator*(const Eigen::MatrixBase<M6Like> & Y, const ConstraintRevoluteTpl<S2,O2,axis> &)
 {
   EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
   return Y.col(Inertia::ANGULAR + axis);
 }

void LSTM::setupBackwardProp(const Eigen::MatrixBase<Derived> &output, int index)
{
	this->output = output.col(index);
}

        bool CameraModel::_lift( const Eigen::MatrixBase<Derived1>& mP2D,
                                 Eigen::MatrixBase<Derived2>& mP3D ) const {
        typedef typename Eigen::internal::traits<Derived2>::Scalar ScalarType;
        const unsigned int nNumPoints = mP2D.cols();
#if 0
        typedef Eigen::Matrix<ScalarType,1,Eigen::Dynamic> RowVect;
        const int nWidth = 1000;
        const int nHeight = 1000;
        const int nWidthSize = nWidth/10;
        const int nHeightSize = nHeight/10;
        Derived1 mP2DUndist( 3, nWidthSize*nHeightSize );
        Derived1 m1 = RowVect::LinSpaced( nWidthSize, 0, nWidth-1 ).replicate( nHeightSize, 1 );
        mP2DUndist.row( 0 ) = Eigen::Map<Derived1>( m1.data(), 1, nWidthSize*nHeightSize );
        mP2DUndist.row( 1 ) = RowVect::LinSpaced( nHeightSize, 0, nHeight-1 ).replicate( 1, nWidthSize );
        mP2DUndist.row( 2 ) = RowVect::Ones( 1, nWidthSize*nHeightSize );
        Derived1 mP2DDist( 2, nWidthSize*nHeightSize );
        project( mP2DUndist, mP2DDist );

        // Initialise with closest values
        for( int ii=0; ii<mP2D.cols(); ii++ ) {
            //Derived1::Index nMinIndex;
            int nMinIndex;
            Derived1 mDiff = mP2DDist;
            mDiff.colwise() -= mP2D.col( ii );
            mDiff.colwise().norm().minCoeff( &nMinIndex );
            mP3D.col( ii ) = mP2DUndist.col( nMinIndex );
        }
#else
        // Start from distortionless points if possible
        if( get( "fx" ) != "" && get( "fy" ) != "" && 
            get( "cx" ) != "" && get( "cy" ) != "" ) {
            double fx = get<double>( "fx" ); double fy = get<double>( "fy" );
            double cx = get<double>( "cx" ); double cy = get<double>( "cy" );
            mP3D.row( 0 ) = ( mP2D.row( 0 ) - cx*Derived2::Ones( 1, nNumPoints ) ) / fx;
            mP3D.row( 1 ) = ( mP2D.row( 1 ) - cy*Derived2::Ones( 1, nNumPoints ) ) / fy;
            mP3D.row( 2 ) = Derived2::Ones( 1, nNumPoints );            
        }
        else {
            mP3D.row( 0 ) = mP2D.row( 0 );
            mP3D.row( 1 ) = mP2D.row( 1 );
            mP3D.row( 2 ) = Derived2::Ones( 1, nNumPoints );
        }
#endif
        for( size_t nIndex=0; nIndex<nNumPoints; nIndex++ ) {
            mP3D.col( nIndex ) /= mP3D.col( nIndex ).norm();
        }

        Derived1 mP2DEst( 2, nNumPoints );
        Derived2 mdP2DE( 2, 3*nNumPoints );
        Derived2 mdP2DI( 2, get_num_parameters()*nNumPoints );

        double dMaxErr = 0.001;
        double dLastMaxErr = 10;
        const unsigned int nMaxIter = 30;
        const unsigned int nMaxOuterIter = 5;
        for( size_t nOuterIter=0; nOuterIter<nMaxOuterIter; nOuterIter++ ) {
            for( size_t nIter=0; nIter<nMaxIter; nIter++ ) {
                project( mP3D, mP2DEst, mdP2DE, mdP2DI );
                for( size_t nIndex=0; nIndex<nNumPoints; nIndex++ ) {
                    Eigen::Matrix<ScalarType,2,3> mJ = mdP2DE.block( 0, 3*nIndex, 2, 3 );
                    Eigen::Matrix<ScalarType,3,3> mJtJ = mJ.transpose()*mJ + 0.1*Eigen::Matrix<ScalarType,3,3>::Identity();
                    Eigen::Matrix<ScalarType,3,1> mJte = mJ.transpose() * ( mP2DEst.col( nIndex ) - mP2D.col( nIndex ) );
                    mP3D.col( nIndex ) -= mJtJ.ldlt().solve( mJte );
                
                    double dErr = ( mP2DEst.col( nIndex ) - mP2D.col( nIndex ) ).norm();
                    if( nIndex > 0 ) {
                        if( std::isnan( dErr ) ) {
                            mP3D.col( nIndex ) =  mP3D.col( nIndex-1 );
                        }
                    }
                    mP3D.col( nIndex ) /= mP3D.col( nIndex ).norm();
                }
                dLastMaxErr = ( mP2DEst - mP2D ).colwise().norm().maxCoeff();
                if( dLastMaxErr < dMaxErr ) {
                    break;
                }
            }

            if( dLastMaxErr >= dMaxErr ) {
                Derived1 mErrors = ( mP2DEst - mP2D ).colwise().norm();
                for( int ii=0; ii<mErrors.cols(); ii++ ) {
                    if( mErrors(0,ii) > dMaxErr ) {
                        // Find closest value
                        double dMinDiff = 1e10;
                        int nBestIndex = -1;
                        for( int jj=0; jj<mErrors.cols(); jj++ ) {
                            if( jj != ii && mErrors(0,jj) < dMaxErr ) {
                                double dDiff = ( mP2D.col( ii ) - mP2D.col( jj ) ).norm();
                                if( dDiff <  dMinDiff ) { 
                                    dMinDiff = dDiff;
                                    nBestIndex = jj;
                                }
                            }
                        }
                        if( nBestIndex == -1 ) {
                            // All is lost, could not find an index 
                            // that passes the error test
                            return false;
                        }
                        mP3D.col( ii ) = mP3D.col( nBestIndex ) ;
//.........这里部分代码省略.........