inline void computeRNEADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, DataTpl<Scalar,Options,JointCollectionTpl> & data, const Eigen::MatrixBase<ConfigVectorType> & q, const Eigen::MatrixBase<TangentVectorType1> & v, const Eigen::MatrixBase<TangentVectorType2> & a) { computeRNEADerivatives(model,data,q.derived(),v.derived(),a.derived(), data.dtau_dq, data.dtau_dv, data.M); }
CwiseNullaryOp<indexing_functor<ArgType,RowIndexType,ColIndexType>, typename indexing_functor<ArgType,RowIndexType,ColIndexType>::MatrixType> indexing(const Eigen::MatrixBase<ArgType>& arg, const RowIndexType& row_indices, const ColIndexType& col_indices) { typedef indexing_functor<ArgType,RowIndexType,ColIndexType> Func; typedef typename Func::MatrixType MatrixType; return MatrixType::NullaryExpr(row_indices.size(), col_indices.size(), Func(arg.derived(), row_indices, col_indices)); }
template<typename Rhs> inline const Eigen::internal::solve_retval<MyJacobiPreconditioner, Rhs> solve(const Eigen::MatrixBase<Rhs>& b) const { eigen_assert(m_isInitialized && "MyJacobiPreconditioner is not initialized."); eigen_assert(m_invdiag.size()==b.rows() && "MyJacobiPreconditioner::solve(): invalid number of rows of the right hand side matrix b"); return Eigen::internal::solve_retval<MyJacobiPreconditioner, Rhs>(*this, b.derived()); }
void RadialTangentialDistortion::undistort( const Eigen::MatrixBase<DERIVED> & yconst, const Eigen::MatrixBase<DERIVED_JY> & outJy) const { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC( Eigen::MatrixBase<DERIVED>, 2); EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE_OR_DYNAMIC( Eigen::MatrixBase<DERIVED_JY>, 2, 2); Eigen::MatrixBase<DERIVED> & y = const_cast<Eigen::MatrixBase<DERIVED> &>(yconst); y.derived().resize(2); // we use f^-1 ' = ( f'(f^-1) ) ' // with f^-1 the undistortion // and f the distortion undistort(y); // first get the undistorted image Eigen::Vector2d kp = y; Eigen::Matrix2d Jd; distort(kp, Jd); // now y = f^-1(y0) DERIVED_JY & J = const_cast<DERIVED_JY &>(outJy.derived()); J = Jd.inverse(); /* std::cout << "J: " << std::endl << J << std::endl; double mx2_u = y[0]*y[0]; double my2_u = y[1]*y[1]; //double mxy_u = y[0]*y[1]; double rho2_u = mx2_u+my2_u; double rad_dist_u = _k1*rho2_u+_k2*rho2_u*rho2_u; // take the inverse as Jacobian. J(0,0) = 1/(1 + rad_dist_u + _k1*2*mx2_u + _k2*rho2_u*4*mx2_u + 2*_p1*y[1] + 6*_p2*y[0]); J(1,0) = (_k1*2*y[0]*y[1] + _k2*4*rho2_u*y[0]*y[1] + _p1*2*y[0] + 2*_p2*y[1]); J(0,1) = J(1,0); J(1,1) = 1/(1 + rad_dist_u + _k1*2*my2_u + _k2*rho2_u*4*my2_u + 6*_p1*y[1] + 2*_p2*y[0]); // this should only happen if the distortion coefficients are 0 // the coefficients being zero removes the cross dependence then it is safe to set J(1,0) = 0 if (J(1,0) != 0) J(1,0) = 1/J(1,0); J(0,1) = J(1,0);*/ }
Eigen::Product<MatrixReplacement,Rhs,Eigen::AliasFreeProduct> operator*(const Eigen::MatrixBase<Rhs>& x) const { return Eigen::Product<MatrixReplacement,Rhs,Eigen::AliasFreeProduct>(*this, x.derived()); }
static void resize( Eigen::MatrixBase< Derived > &m1 , const Eigen::MatrixBase< Derived > &m2 ) { m1.derived().resizeLike(m2); }
MatrixReplacement_ProductReturnType<Rhs> operator*(const Eigen::MatrixBase<Rhs>& x) const { return MatrixReplacement_ProductReturnType<Rhs>(*this, x.derived()); }
Eigen::Product<MutatorEquation, Rhs, Eigen::AliasFreeProduct> operator*(const Eigen::MatrixBase<Rhs> &x) const { return Eigen::Product<MutatorEquation, Rhs, Eigen::AliasFreeProduct>(*this, x.derived()); }
CwiseNullaryOp<circulant_functor<ArgType>, typename circulant_helper<ArgType>::MatrixType> makeCirculant(const Eigen::MatrixBase<ArgType>& arg) { typedef typename circulant_helper<ArgType>::MatrixType MatrixType; return MatrixType::NullaryExpr(arg.size(), arg.size(), circulant_functor<ArgType>(arg.derived())); }
void operator()(Eigen::MatrixBase<T1> &x, Eigen::MatrixBase<T2> &result) const { memcpy(goc.fc->est, x.derived().data(), sizeof(double) * goc.fc->numParam); goc.fc->copyParamToModel(); goc.solEqBFun(); result = goc.equality; }
double operator()(Eigen::MatrixBase<T1>& x) const { int mode = 0; return goc.solFun(x.derived().data(), &mode); }