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; } case MASSMATRIX_TYPE_FULL: assert(false && "Implementation incomplete"); break; default: assert(false && "Unknown Mass matrix eff_type"); } }else if(simplex_size == 4) { assert(V.cols() == 3); assert(eff_type == MASSMATRIX_TYPE_BARYCENTRIC); MI.resize(m*4,1); MJ.resize(m*4,1); MV.resize(m*4,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); MI.block(3*m,0,m,1) = F.col(3); MJ = MI; // loop over tets for(int i = 0;i<m;i++) { // http://en.wikipedia.org/wiki/Tetrahedron#Volume Matrix<Scalar,3,1> v0m3 = V.row(F(i,0)) - V.row(F(i,3)); Matrix<Scalar,3,1> v1m3 = V.row(F(i,1)) - V.row(F(i,3)); Matrix<Scalar,3,1> v2m3 = V.row(F(i,2)) - V.row(F(i,3)); Scalar v = fabs(v0m3.dot(v1m3.cross(v2m3)))/6.0; MV(i+0*m) = v/4.0; MV(i+1*m) = v/4.0; MV(i+2*m) = v/4.0; MV(i+3*m) = v/4.0; } }else { // Unsupported simplex size assert(false && "Unsupported simplex size"); } sparse(MI,MJ,MV,n,n,M); }
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 ) ; } } } } return dLastMaxErr < dMaxErr; }