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