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;
}