std::vector<double> unit_vector_grad(Eigen::Matrix<double,Eigen::Dynamic,1>& y_dbl, int k) { using Eigen::Matrix; using Eigen::Dynamic; using stan::math::var; Matrix<var,Dynamic,1> y(y_dbl.size()); for (int i = 0; i < y.size(); ++i) y(i) = y_dbl(i); std::vector<var> x(y.size()); for (size_t i = 0; i < x.size(); ++i) x[i] = y(i); var fx_k = stan::math::unit_vector_constrain(y)[k]; std::vector<double> grad(y.size()); fx_k.grad(x,grad); return grad; }
// compute grad using templated definition in math // to check custom derivatives std::vector<double> softmax_grad(Eigen::Matrix<double,Eigen::Dynamic,1>& alpha_dbl, int k) { using Eigen::Matrix; using Eigen::Dynamic; using stan::agrad::var; Matrix<var,Dynamic,1> alpha(alpha_dbl.size()); for (int i = 0; i < alpha.size(); ++i) alpha(i) = alpha_dbl(i); std::vector<var> x(alpha.size()); for (size_t i = 0; i < x.size(); ++i) x[i] = alpha(i); var fx_k = stan::math::softmax(alpha)[k]; std::vector<double> grad(alpha.size()); fx_k.grad(x,grad); return grad; }
inline var log_determinant_spd(const Eigen::Matrix<var,R,C>& m) { using stan::math::domain_error; using Eigen::Matrix; math::check_square("log_determinant_spd", "m", m); Matrix<double,R,C> m_d(m.rows(),m.cols()); for (int i = 0; i < m.size(); ++i) m_d(i) = m(i).val(); Eigen::LDLT<Matrix<double,R,C> > ldlt(m_d); if (ldlt.info() != Eigen::Success) { double y = 0; domain_error("log_determinant_spd", "matrix argument", y, "failed LDLT factorization"); } // compute the inverse of A (needed for the derivative) m_d.setIdentity(m.rows(), m.cols()); ldlt.solveInPlace(m_d); if (ldlt.isNegative() || (ldlt.vectorD().array() <= 1e-16).any()) { double y = 0; domain_error("log_determinant_spd", "matrix argument", y, "matrix is negative definite"); } double val = ldlt.vectorD().array().log().sum(); if (!boost::math::isfinite(val)) { double y = 0; domain_error("log_determinant_spd", "matrix argument", y, "log determininant is infinite"); } vari** operands = ChainableStack::memalloc_ .alloc_array<vari*>(m.size()); for (int i = 0; i < m.size(); ++i) operands[i] = m(i).vi_; double* gradients = ChainableStack::memalloc_ .alloc_array<double>(m.size()); for (int i = 0; i < m.size(); ++i) gradients[i] = m_d(i); return var(new precomputed_gradients_vari(val,m.size(),operands,gradients)); }
Eigen::Matrix<T, Eigen::Dynamic, 1> simplex_free(const Eigen::Matrix<T, Eigen::Dynamic, 1>& x) { using Eigen::Dynamic; using Eigen::Matrix; using std::log; typedef typename index_type<Matrix<T, Dynamic, 1> >::type size_type; check_simplex("stan::math::simplex_free", "Simplex variable", x); int Km1 = x.size() - 1; Eigen::Matrix<T, Eigen::Dynamic, 1> y(Km1); T stick_len(x(Km1)); for (size_type k = Km1; --k >= 0; ) { stick_len += x(k); T z_k(x(k) / stick_len); y(k) = logit(z_k) + log(Km1 - k); // note: log(Km1 - k) = logit(1.0 / (Km1 + 1 - k)); } return y; }
Eigen::Matrix<T, Eigen::Dynamic, 1> positive_ordered_free(const Eigen::Matrix<T, Eigen::Dynamic, 1>& y) { using Eigen::Matrix; using Eigen::Dynamic; using stan::math::index_type; typedef typename index_type<Matrix<T, Dynamic, 1> >::type size_type; stan::math::check_positive_ordered("stan::math::positive_ordered_free", "Positive ordered variable", y); size_type k = y.size(); Matrix<T, Dynamic, 1> x(k); if (k == 0) return x; x[0] = log(y[0]); for (size_type i = 1; i < k; ++i) x[i] = log(y[i] - y[i-1]); return x; }
IGL_INLINE void igl::print_ijv( const Eigen::SparseMatrix<T>& X, const int offset) { Eigen::Matrix<int,Eigen::Dynamic,1> I; Eigen::Matrix<int,Eigen::Dynamic,1> J; Eigen::Matrix<T,Eigen::Dynamic,1> V; igl::find(X,I,J,V); // Concatenate I,J,V Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> IJV(I.size(),3); IJV.col(0) = I.cast<T>(); IJV.col(1) = J.cast<T>(); IJV.col(2) = V; // Offset if(offset != 0) { IJV.col(0).array() += offset; IJV.col(1).array() += offset; } std::cout<<IJV; }
void MitsubishiH7::setMotorPulse(const ::Eigen::Matrix< ::std::int32_t, ::Eigen::Dynamic, 1>& p) { assert(p.size() >= this->getDof()); this->out.dat2.pls.p1 = 0; this->out.dat2.pls.p2 = 0; this->out.dat2.pls.p3 = 0; this->out.dat2.pls.p4 = 0; this->out.dat2.pls.p5 = 0; this->out.dat2.pls.p6 = 0; this->out.dat2.pls.p7 = 0; this->out.dat2.pls.p8 = 0; switch (this->getDof()) { case 8: this->out.dat2.pls.p8 = p(7); case 7: this->out.dat2.pls.p7 = p(6); case 6: this->out.dat2.pls.p6 = p(5); case 5: this->out.dat2.pls.p5 = p(4); case 4: this->out.dat2.pls.p4 = p(3); case 3: this->out.dat2.pls.p3 = p(2); case 2: this->out.dat2.pls.p2 = p(1); case 1: this->out.dat2.pls.p1 = p(0); default: break; } this->out.command = MXT_COMMAND_MOVE; this->out.sendType = MXT_TYPE_PULSE; }
void grad_hessian(const F& f, const Eigen::Matrix<double, Eigen::Dynamic, 1>& x, double& fx, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& H, std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> >& grad_H) { using Eigen::Matrix; using Eigen::Dynamic; fx = f(x); int d = x.size(); H.resize(d, d); grad_H.resize(d, Matrix<double, Dynamic, Dynamic>(d, d)); try { for (int i = 0; i < d; ++i) { for (int j = i; j < d; ++j) { start_nested(); Matrix<fvar<fvar<var> >, Dynamic, 1> x_ffvar(d); for (int k = 0; k < d; ++k) x_ffvar(k) = fvar<fvar<var> >(fvar<var>(x(k), i == k), fvar<var>(j == k, 0)); fvar<fvar<var> > fx_ffvar = f(x_ffvar); H(i, j) = fx_ffvar.d_.d_.val(); H(j, i) = H(i, j); grad(fx_ffvar.d_.d_.vi_); for (int k = 0; k < d; ++k) { grad_H[i](j, k) = x_ffvar(k).val_.val_.adj(); grad_H[j](i, k) = grad_H[i](j, k); } recover_memory_nested(); } } } catch (const std::exception& e) { recover_memory_nested(); throw; } }
inline Eigen::Matrix < typename boost::math::tools::promote_args<T1, T2>::type, Eigen::Dynamic, Eigen::Dynamic> quad_form_diag(const Eigen::Matrix<T1, Eigen::Dynamic, Eigen::Dynamic>& mat, const Eigen::Matrix<T2, R, C>& vec) { using boost::math::tools::promote_args; check_vector("quad_form_diag", "vec", vec); check_square("quad_form_diag", "mat", mat); int size = vec.size(); check_equal("quad_form_diag", "matrix size", mat.rows(), size); Eigen::Matrix<typename promote_args<T1, T2>::type, Eigen::Dynamic, Eigen::Dynamic> result(size, size); for (int i = 0; i < size; i++) { result(i, i) = vec(i)*vec(i)*mat(i, i); for (int j = i+1; j < size; ++j) { typename promote_args<T1, T2>::type temp = vec(i)*vec(j); result(j, i) = temp*mat(j, i); result(i, j) = temp*mat(i, j); } } return result; }
void test_sort_indices_desc3(Eigen::Matrix<T,R,C> val) { typedef Eigen::Matrix<fvar<fvar<double> >,R,C> AVEC; const size_t size = val.size(); AVEC x(size); for(size_t i=0U; i<size; i++) x.data()[i] = fvar<fvar<double> >(val[i]); std::vector<int> val_sorted = sort_indices_desc(val); std::vector<int> x_sorted = sort_indices_desc(x); for(size_t i=0U; i<size; i++) EXPECT_EQ(val_sorted.data()[i],x_sorted.data()[i]); for(size_t i=0U; i<size; i++) for(size_t j=0U; j<size; j++) if(val_sorted.data()[i] == val.data()[j]) EXPECT_EQ(x_sorted.data()[i],x.data()[j]); else EXPECT_FALSE(x_sorted.data()[i]==x.data()[j]); }
bool check_simplex(const char* function, const char* name, const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) { using Eigen::Dynamic; using Eigen::Matrix; using stan::math::index_type; typedef typename index_type<Matrix<T_prob, Dynamic, 1> >::type size_t; check_nonzero_size(function, name, theta); if (!(fabs(1.0 - theta.sum()) <= CONSTRAINT_TOLERANCE)) { std::stringstream msg; T_prob sum = theta.sum(); msg << "is not a valid simplex."; msg.precision(10); msg << " sum(" << name << ") = " << sum << ", but should be "; std::string msg_str(msg.str()); domain_error(function, name, 1.0, msg_str.c_str()); return false; } for (size_t n = 0; n < theta.size(); n++) { if (!(theta[n] >= 0)) { std::ostringstream msg; msg << "is not a valid simplex. " << name << "[" << n + stan::error_index::value << "]" << " = "; std::string msg_str(msg.str()); domain_error(function, name, theta[n], msg_str.c_str(), ", but should be greater than or equal to 0"); return false; } } return true; }
void test_sort_indices_desc(Eigen::Matrix<T,R,C> val) { using stan::math::sort_indices_desc; typedef Eigen::Matrix<AVAR,R,C> AVEC; const size_t size = val.size(); AVEC x(size); for(size_t i=0U; i<size; i++) x.data()[i] = AVAR(val[i]); std::vector<int> val_sorted = sort_indices_desc(val); std::vector<int> x_sorted = sort_indices_desc(x); for(size_t i=0U; i<size; i++) EXPECT_EQ(val_sorted.data()[i],x_sorted.data()[i]); for(size_t i=0U; i<size; i++) for(size_t j=0U; j<size; j++) if(val_sorted.data()[i] == val.data()[j]) EXPECT_EQ(x_sorted.data()[i],x.data()[j]); else EXPECT_FALSE(x_sorted.data()[i]==x.data()[j]); }
typename boost::math::tools::promote_args<T_prob>::type categorical_logit_log(const std::vector<int>& ns, const Eigen::Matrix<T_prob,Eigen::Dynamic,1>& beta) { static const char* function = "stan::prob::categorical_logit_log(%1%)"; using stan::math::check_bounded; using stan::math::check_finite; using stan::math::log_softmax; using stan::math::sum; double lp = 0.0; for (size_t k = 0; k < ns.size(); ++k) if (!check_bounded(function, ns[k], 1, beta.size(), "categorical outcome out of support", &lp)) return lp; if (!check_finite(function, beta, "log odds parameter", &lp)) return lp; if (!include_summand<propto,T_prob>::value) return 0.0; if (ns.size() == 0) return 0.0; Eigen::Matrix<T_prob,Eigen::Dynamic,1> log_softmax_beta = log_softmax(beta); // FIXME: replace with more efficient sum() Eigen::Matrix<typename boost::math::tools::promote_args<T_prob>::type, Eigen::Dynamic,1> results(ns.size()); for (size_t i = 0; i < ns.size(); ++i) results[i] = log_softmax_beta(ns[i] - 1); return sum(results); }
typename boost::math::tools::promote_args<T_prob>::type categorical_logit_log(int n, const Eigen::Matrix<T_prob,Eigen::Dynamic,1>& beta) { static const char* function = "stan::prob::categorical_logit_log(%1%)"; using stan::math::check_bounded; using stan::math::check_finite; using stan::math::log_sum_exp; double lp = 0.0; if (!check_bounded(function, n, 1, beta.size(), "categorical outcome out of support", &lp)) return lp; if (!check_finite(function, beta, "log odds parameter", &lp)) return lp; if (!include_summand<propto,T_prob>::value) return 0.0; // FIXME: wasteful vs. creating term (n-1) if not vectorized return beta(n-1) - log_sum_exp(beta); // == log_softmax(beta)(n-1); }
inline Eigen::Matrix<fvar<T>, Eigen::Dynamic, 1> softmax(const Eigen::Matrix<fvar<T>, Eigen::Dynamic, 1>& alpha) { using stan::math::softmax; using Eigen::Matrix; using Eigen::Dynamic; Matrix<T, Dynamic, 1> alpha_t(alpha.size()); for (int k = 0; k < alpha.size(); ++k) alpha_t(k) = alpha(k).val_; Matrix<T, Dynamic, 1> softmax_alpha_t = softmax(alpha_t); Matrix<fvar<T>, Dynamic, 1> softmax_alpha(alpha.size()); for (int k = 0; k < alpha.size(); ++k) { softmax_alpha(k).val_ = softmax_alpha_t(k); softmax_alpha(k).d_ = 0; } // for each input position for (int m = 0; m < alpha.size(); ++m) { // for each output position T negative_alpha_m_d_times_softmax_alpha_t_m = - alpha(m).d_ * softmax_alpha_t(m); for (int k = 0; k < alpha.size(); ++k) { // chain from input to output if (m == k) { softmax_alpha(k).d_ += softmax_alpha_t(k) * (alpha(m).d_ + negative_alpha_m_d_times_softmax_alpha_t_m); } else { softmax_alpha(k).d_ += negative_alpha_m_d_times_softmax_alpha_t_m * softmax_alpha_t(k); } } } return softmax_alpha; }
inline Eigen::Matrix<fvar<T>, Eigen::Dynamic, 1> softmax(const Eigen::Matrix<fvar<T>, Eigen::Dynamic, 1>& alpha) { using Eigen::Matrix; using Eigen::Dynamic; Matrix<T, Dynamic, 1> alpha_t(alpha.size()); for (int k = 0; k < alpha.size(); ++k) alpha_t(k) = alpha(k).val_; Matrix<T, Dynamic, 1> softmax_alpha_t = softmax(alpha_t); Matrix<fvar<T>, Dynamic, 1> softmax_alpha(alpha.size()); for (int k = 0; k < alpha.size(); ++k) { softmax_alpha(k).val_ = softmax_alpha_t(k); softmax_alpha(k).d_ = 0; } for (int m = 0; m < alpha.size(); ++m) { T negative_alpha_m_d_times_softmax_alpha_t_m = - alpha(m).d_ * softmax_alpha_t(m); for (int k = 0; k < alpha.size(); ++k) { if (m == k) { softmax_alpha(k).d_ += softmax_alpha_t(k) * (alpha(m).d_ + negative_alpha_m_d_times_softmax_alpha_t_m); } else { softmax_alpha(k).d_ += negative_alpha_m_d_times_softmax_alpha_t_m * softmax_alpha_t(k); } } } return softmax_alpha; }
inline typename Eigen::Matrix<var, R, C> sort_desc(Eigen::Matrix<var, R, C> xs) { std::sort(xs.data(), xs.data()+xs.size(), std::greater<var>()); return xs; }
double GaussianFullCov<ScalarType>::calculateNoMeanValue(const Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>& dataVector) { assert(dataVector.size() == mean.size()); return preFactor * std::exp(-0.5 * (dataVector.transpose() * ldlt.solve(dataVector))(0)); }
double GaussianFullCov<ScalarType>::calculateValueWithoutWeights(const Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>& dataVector) { assert(dataVector.size() == mean.size()); Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> dist = dataVector - mean; return preFactorWithoutWeights * std::exp(-0.5 * (dist.transpose() * ldlt.solve(dist))(0)); }
TEST(Matrices,fromMatlabStringFormat) { const char* mat1 = "[1 2 3;-3 -6 -5]"; const double vals1[] = {1,2,3,-3,-6,-5}; const char* mat2 = " [ -8.2 9.232 ; -2e+2 +6 ; 1.000 7 ] "; // With tabs and spaces... const double vals2[] = {-8.2, 9.232, -2e+2, +6, 1.000 ,7}; const char* mat3 = "[9]"; const char* mat4 = "[1 2 3 4 5 6 7 9 10 ; 1 2 3 4 5 6 7 8 9 10 11]"; // An invalid matrix const char* mat5 = "[ ]"; // Empty const char* mat6 = "[ -405.200 42.232 ; 1219.600 -98.696 ]"; // M1 * M2 const char* mat13 = "[9 8 7]"; const char* mat31 = "[9; 8; 7]"; CMatrixDouble M1,M2,M3, M4, M5, M6; if (! M1.fromMatlabStringFormat(mat1) || (CMatrixFixedNumeric<double,2,3>(vals1)-M1).array().abs().sum() > 1e-4 ) GTEST_FAIL() << mat1; { CMatrixFixedNumeric<double,2,3> M1b; if (! M1b.fromMatlabStringFormat(mat1) || (CMatrixFixedNumeric<double,2,3>(vals1)-M1b).array().abs().sum() > 1e-4 ) GTEST_FAIL() << mat1; } if (! M2.fromMatlabStringFormat(mat2) || M2.cols()!=2 || M2.rows()!=3 || (CMatrixFixedNumeric<double,3,2>(vals2)-M2).array().abs().sum() > 1e-4 ) GTEST_FAIL() << mat2; { CMatrixFixedNumeric<double,3,2> M2b; if (! M2b.fromMatlabStringFormat(mat2) || (CMatrixFixedNumeric<double,3,2>(vals2)-M2b).array().abs().sum() > 1e-4 ) GTEST_FAIL() << mat2; } if (! M3.fromMatlabStringFormat(mat3) ) GTEST_FAIL() << mat3; { CVectorDouble m; if (! m.fromMatlabStringFormat(mat3) || m.size()!=1 ) GTEST_FAIL() << "CVectorDouble:" << mat3; } { CArrayDouble<1> m; if (! m.fromMatlabStringFormat(mat3) ) GTEST_FAIL() << "CArrayDouble<1>:" << mat3; } { CVectorDouble m; if (! m.fromMatlabStringFormat(mat31) || m.size()!=3 ) GTEST_FAIL() << "CVectorDouble:" << mat31; } { CArrayDouble<3> m; if (! m.fromMatlabStringFormat(mat31) ) GTEST_FAIL() << "CArrayDouble<3>:" << mat31; } { Eigen::Matrix<double,1,3> m; if (! m.fromMatlabStringFormat(mat13) ) GTEST_FAIL() << "Matrix<double,1,3>:" << mat13; } { Eigen::Matrix<double,1,Eigen::Dynamic> m; if (! m.fromMatlabStringFormat(mat13) || m.size()!=3 ) GTEST_FAIL() << "Matrix<double,1,Dynamic>:" << mat13; } // This one MUST BE detected as WRONG: if ( M4.fromMatlabStringFormat(mat4, NULL /*dont dump errors to cerr*/) ) GTEST_FAIL() << mat4; if (! M5.fromMatlabStringFormat(mat5) || size(M5,1)!=0 || size(M5,2)!=0 ) GTEST_FAIL() << mat5; if (! M6.fromMatlabStringFormat(mat6) ) GTEST_FAIL() << mat6; // Check correct values loaded: CMatrixDouble RES = M1*M2; EXPECT_NEAR(0,(M6 - M1*M2).array().square().sum(), 1e-3); }
typename return_type< T_y, typename return_type<T_F,T_G,T_V,T_W,T_m0,T_C0>::type >::type gaussian_dlm_obs_log(const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y, const Eigen::Matrix<T_F,Eigen::Dynamic,Eigen::Dynamic>& F, const Eigen::Matrix<T_G,Eigen::Dynamic,Eigen::Dynamic>& G, const Eigen::Matrix<T_V,Eigen::Dynamic,Eigen::Dynamic>& V, const Eigen::Matrix<T_W,Eigen::Dynamic,Eigen::Dynamic>& W, const Eigen::Matrix<T_m0,Eigen::Dynamic,1>& m0, const Eigen::Matrix<T_C0,Eigen::Dynamic,Eigen::Dynamic>& C0) { static const char* function = "stan::prob::gaussian_dlm_obs_log(%1%)"; typedef typename return_type< T_y, typename return_type<T_F,T_G,T_V,T_W,T_m0,T_C0>::type >::type T_lp; T_lp lp(0.0); using stan::math::add; using stan::math::check_cov_matrix; using stan::math::check_finite; using stan::math::check_not_nan; using stan::math::check_size_match; using stan::math::check_spsd_matrix; using stan::math::inverse_spd; using stan::math::log_determinant_spd; using stan::math::multiply; using stan::math::quad_form_sym; using stan::math::subtract; using stan::math::trace_quad_form; using stan::math::transpose; int r = y.rows(); // number of variables int T = y.cols(); // number of observations int n = G.rows(); // number of states // check y if (!check_finite(function, y, "y", &lp)) return lp; if (!check_not_nan(function, y, "y", &lp)) return lp; // check F if (!check_size_match(function, F.cols(), "columns of F", y.rows(), "rows of y", &lp)) return lp; if (!check_size_match(function, F.rows(), "rows of F", G.rows(), "rows of G", &lp)) return lp; if (!check_finite(function, F, "F", &lp)) return lp; if (!check_not_nan(function, F, "F", &lp)) return lp; // check G if (!check_size_match(function, G.rows(), "rows of G", G.cols(), "columns of G", &lp)) return lp; if (!check_finite(function, G, "G", &lp)) return lp; if (!check_not_nan(function, G, "G", &lp)) return lp; // check V if (!check_size_match(function, V.rows(), "rows of V", y.rows(), "rows of y", &lp)) return lp; // TODO: incorporate support for infinite V if (!check_finite(function, V, "V", &lp)) return lp; if (!check_not_nan(function, V, "V", &lp)) return lp; if (!check_spsd_matrix(function, V, "V", &lp)) return lp; // check W if (!check_size_match(function, W.rows(), "rows of W", G.rows(), "rows of G", &lp)) return lp; // TODO: incorporate support for infinite W if (!check_finite(function, W, "W", &lp)) return lp; if (!check_not_nan(function, W, "W", &lp)) return lp; if (!check_spsd_matrix(function, W, "W", &lp)) return lp; // check m0 if (!check_size_match(function, m0.size(), "size of m0", G.rows(), "rows of G", &lp)) return lp; if (!check_finite(function, m0, "m0", &lp)) return lp; if (!check_not_nan(function, m0, "m0", &lp)) return lp; // check C0 if (!check_size_match(function, C0.rows(), "rows of C0", G.rows(), "rows of G", &lp)) return lp; if (!check_cov_matrix(function, C0, "C0", &lp)) return lp; if (!check_finite(function, C0, "C0", &lp)) return lp; if (!check_not_nan(function, C0, "C0", &lp)) return lp; if (y.cols() == 0 || y.rows() == 0) return lp; if (include_summand<propto>::value) { lp -= 0.5 * LOG_TWO_PI * r * T; } if (include_summand<propto,T_y,T_F,T_G,T_V,T_W,T_m0,T_C0>::value) { Eigen::Matrix<T_lp,Eigen::Dynamic, 1> m(n); Eigen::Matrix<T_lp,Eigen::Dynamic, Eigen::Dynamic> C(n, n); // TODO: how to recast matrices for (int i = 0; i < m0.size(); i ++ ) { m(i) = m0(i); } for (int i = 0; i < C0.rows(); i ++ ) { for (int j = 0; j < C0.cols(); j ++ ) { C(i, j) = C0(i, j); } } Eigen::Matrix<typename return_type<T_y>::type, Eigen::Dynamic, 1> yi(r); Eigen::Matrix<T_lp,Eigen::Dynamic, 1> a(n); Eigen::Matrix<T_lp,Eigen::Dynamic, Eigen::Dynamic> R(n, n); Eigen::Matrix<T_lp,Eigen::Dynamic, 1> f(r); Eigen::Matrix<T_lp,Eigen::Dynamic, Eigen::Dynamic> Q(r, r); Eigen::Matrix<T_lp,Eigen::Dynamic, Eigen::Dynamic> Q_inv(r, r); Eigen::Matrix<T_lp,Eigen::Dynamic, 1> e(r); Eigen::Matrix<T_lp,Eigen::Dynamic, Eigen::Dynamic> A(n, r); for (int i = 0; i < y.cols(); i++) { yi = y.col(i); // // Predict state // a_t = G_t m_{t-1} a = multiply(G, m); // R_t = G_t C_{t-1} G_t' + W_t R = add(quad_form_sym(C, transpose(G)), W); // // predict observation // f_t = F_t' a_t f = multiply(transpose(F), a); // Q_t = F'_t R_t F_t + V_t Q = add(quad_form_sym(R, F), V); Q_inv = inverse_spd(Q); // // filtered state // e_t = y_t - f_t e = subtract(yi, f); // A_t = R_t F_t Q^{-1}_t A = multiply(multiply(R, F), Q_inv); // m_t = a_t + A_t e_t m = add(a, multiply(A, e)); // C = R_t - A_t Q_t A_t' C = subtract(R, quad_form_sym(Q, transpose(A))); lp -= 0.5 * (log_determinant_spd(Q) + trace_quad_form(Q_inv, e)); } } return lp; }
void dims(Eigen::Matrix<T,1,Eigen::Dynamic> rv, std::vector<size_t> ds) { ds.push_back(rv.size()); }
inline void initialize_variable(Eigen::Matrix<var,R,C>& matrix, const var& value) { for (int i = 0; i < matrix.size(); ++i) matrix(i) = value; }
typename boost::math::tools::promote_args<T_y,T_loc,T_covar>::type multi_normal_cholesky_log(const Eigen::Matrix<T_y,Eigen::Dynamic,1>& y, const Eigen::Matrix<T_loc,Eigen::Dynamic,1>& mu, const Eigen::Matrix<T_covar,Eigen::Dynamic,Eigen::Dynamic>& L) { static const char* function = "stan::prob::multi_normal_cholesky_log(%1%)"; using stan::math::mdivide_left_tri_low; using stan::math::dot_self; using stan::math::multiply; using stan::math::subtract; using stan::math::sum; using stan::math::check_size_match; using stan::math::check_finite; using stan::math::check_not_nan; using stan::math::check_cov_matrix; using boost::math::tools::promote_args; typename promote_args<T_y,T_loc,T_covar>::type lp(0.0); if (!check_size_match(function, y.size(), "Size of random variable", mu.size(), "size of location parameter", &lp)) return lp; if (!check_size_match(function, y.size(), "Size of random variable", L.rows(), "rows of covariance parameter", &lp)) return lp; if (!check_size_match(function, y.size(), "Size of random variable", L.cols(), "columns of covariance parameter", &lp)) return lp; if (!check_finite(function, mu, "Location parameter", &lp)) return lp; if (!check_not_nan(function, y, "Random variable", &lp)) return lp; if (y.rows() == 0) return lp; if (include_summand<propto>::value) lp += NEG_LOG_SQRT_TWO_PI * y.rows(); if (include_summand<propto,T_covar>::value) { Eigen::Matrix<T_covar,Eigen::Dynamic,1> L_log_diag = L.diagonal().array().log().matrix(); lp -= sum(L_log_diag); } if (include_summand<propto,T_y,T_loc,T_covar>::value) { Eigen::Matrix<typename boost::math::tools::promote_args<T_y,T_loc>::type, Eigen::Dynamic, 1> y_minus_mu(y.size()); for (int i = 0; i < y.size(); i++) y_minus_mu(i) = y(i)-mu(i); Eigen::Matrix<typename boost::math::tools::promote_args<T_covar,T_loc,T_y>::type, Eigen::Dynamic, 1> half(mdivide_left_tri_low(L,y_minus_mu)); // FIXME: this code does not compile. revert after fixing subtract() // Eigen::Matrix<typename // boost::math::tools::promote_args<T_covar,T_loc,T_y>::type, // Eigen::Dynamic, 1> // half(mdivide_left_tri_low(L,subtract(y,mu))); lp -= 0.5 * dot_self(half); } return lp; }
var variance(const Eigen::Matrix<var,R,C>& m) { stan::math::validate_nonzero_size(m,"variance"); if (m.size() == 1) return 0; return calc_variance(m.size(), &m(0)); }
IGL_INLINE void igl::slice( const Eigen::SparseMatrix<TX>& X, const Eigen::Matrix<int,Eigen::Dynamic,1> & R, const Eigen::Matrix<int,Eigen::Dynamic,1> & C, Eigen::SparseMatrix<TY>& Y) { #if 1 int xm = X.rows(); int xn = X.cols(); int ym = R.size(); int yn = C.size(); // special case when R or C is empty if(ym == 0 || yn == 0) { Y.resize(ym,yn); return; } assert(R.minCoeff() >= 0); assert(R.maxCoeff() < xm); assert(C.minCoeff() >= 0); assert(C.maxCoeff() < xn); // Build reindexing maps for columns and rows, -1 means not in map std::vector<std::vector<int> > RI; RI.resize(xm); for(int i = 0;i<ym;i++) { RI[R(i)].push_back(i); } std::vector<std::vector<int> > CI; CI.resize(xn); // initialize to -1 for(int i = 0;i<yn;i++) { CI[C(i)].push_back(i); } // Resize output Eigen::DynamicSparseMatrix<TY, Eigen::RowMajor> dyn_Y(ym,yn); // Take a guess at the number of nonzeros (this assumes uniform distribution // not banded or heavily diagonal) dyn_Y.reserve((X.nonZeros()/(X.rows()*X.cols())) * (ym*yn)); // Iterate over outside for(int k=0; k<X.outerSize(); ++k) { // Iterate over inside for(typename Eigen::SparseMatrix<TX>::InnerIterator it (X,k); it; ++it) { std::vector<int>::iterator rit, cit; for(rit = RI[it.row()].begin();rit != RI[it.row()].end(); rit++) { for(cit = CI[it.col()].begin();cit != CI[it.col()].end(); cit++) { dyn_Y.coeffRef(*rit,*cit) = it.value(); } } } } Y = Eigen::SparseMatrix<TY>(dyn_Y); #else // Alec: This is _not_ valid for arbitrary R,C since they don't necessary // representation a strict permutation of the rows and columns: rows or // columns could be removed or replicated. The removal of rows seems to be // handled here (although it's not clear if there is a performance gain when // the #removals >> #remains). If this is sufficiently faster than the // correct code above, one could test whether all entries in R and C are // unique and apply the permutation version if appropriate. // int xm = X.rows(); int xn = X.cols(); int ym = R.size(); int yn = C.size(); // special case when R or C is empty if(ym == 0 || yn == 0) { Y.resize(ym,yn); return; } assert(R.minCoeff() >= 0); assert(R.maxCoeff() < xm); assert(C.minCoeff() >= 0); assert(C.maxCoeff() < xn); // initialize row and col permutation vectors Eigen::VectorXi rowIndexVec = Eigen::VectorXi::LinSpaced(xm,0,xm-1); Eigen::VectorXi rowPermVec = Eigen::VectorXi::LinSpaced(xm,0,xm-1); for(int i=0;i<ym;i++) { int pos = rowIndexVec.coeffRef(R(i)); if(pos != i) { int& val = rowPermVec.coeffRef(i); std::swap(rowIndexVec.coeffRef(val),rowIndexVec.coeffRef(R(i))); std::swap(rowPermVec.coeffRef(i),rowPermVec.coeffRef(pos)); } } Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic,int> rowPerm(rowIndexVec); Eigen::VectorXi colIndexVec = Eigen::VectorXi::LinSpaced(xn,0,xn-1); Eigen::VectorXi colPermVec = Eigen::VectorXi::LinSpaced(xn,0,xn-1); for(int i=0;i<yn;i++) { int pos = colIndexVec.coeffRef(C(i)); if(pos != i) { int& val = colPermVec.coeffRef(i); std::swap(colIndexVec.coeffRef(val),colIndexVec.coeffRef(C(i))); std::swap(colPermVec.coeffRef(i),colPermVec.coeffRef(pos)); } } Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic,int> colPerm(colPermVec); Eigen::SparseMatrix<T> M = (rowPerm * X); Y = (M * colPerm).block(0,0,ym,yn); #endif }
void mrpt::math::ransac_detect_3D_planes( const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &x, const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &y, const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &z, vector<pair<size_t,TPlane> > &out_detected_planes, const double threshold, const size_t min_inliers_for_valid_plane ) { MRPT_START ASSERT_(x.size()==y.size() && x.size()==z.size()) out_detected_planes.clear(); if (x.empty()) return; // The running lists of remaining points after each plane, as a matrix: CMatrixTemplateNumeric<NUMTYPE> remainingPoints( 3, x.size() ); remainingPoints.insertRow(0,x); remainingPoints.insertRow(1,y); remainingPoints.insertRow(2,z); // --------------------------------------------- // For each plane: // --------------------------------------------- for (;;) { mrpt::vector_size_t this_best_inliers; CMatrixTemplateNumeric<NUMTYPE> this_best_model; math::RANSAC_Template<NUMTYPE>::execute( remainingPoints, ransac3Dplane_fit, ransac3Dplane_distance, ransac3Dplane_degenerate, threshold, 3, // Minimum set of points this_best_inliers, this_best_model, true, // Verbose 0.999 // Prob. of good result ); // Is this plane good enough? if (this_best_inliers.size()>=min_inliers_for_valid_plane) { // Add this plane to the output list: out_detected_planes.push_back( std::make_pair<size_t,TPlane>( this_best_inliers.size(), TPlane( this_best_model(0,0), this_best_model(0,1),this_best_model(0,2),this_best_model(0,3) ) ) ); out_detected_planes.rbegin()->second.unitarize(); // Discard the selected points so they are not used again for finding subsequent planes: remainingPoints.removeColumns(this_best_inliers); } else { break; // Do not search for more planes. } } MRPT_END }
typename return_type< T_y, typename return_type<T_F,T_G,T_V,T_W,T_m0,T_C0>::type >::type gaussian_dlm_obs_log(const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y, const Eigen::Matrix<T_F,Eigen::Dynamic,Eigen::Dynamic>& F, const Eigen::Matrix<T_G,Eigen::Dynamic,Eigen::Dynamic>& G, const Eigen::Matrix<T_V,Eigen::Dynamic,1>& V, const Eigen::Matrix<T_W,Eigen::Dynamic,Eigen::Dynamic>& W, const Eigen::Matrix<T_m0,Eigen::Dynamic,1>& m0, const Eigen::Matrix<T_C0,Eigen::Dynamic,Eigen::Dynamic>& C0) { static const char* function = "stan::prob::gaussian_dlm_obs_log(%1%)"; typedef typename return_type< T_y, typename return_type<T_F,T_G,T_V,T_W,T_m0,T_C0>::type >::type T_lp; T_lp lp(0.0); using stan::math::add; using stan::math::check_cov_matrix; using stan::math::check_finite; using stan::math::check_nonnegative; using stan::math::check_not_nan; using stan::math::check_size_match; using stan::math::check_spsd_matrix; using stan::math::dot_product; using stan::math::multiply; using stan::math::quad_form_sym; using stan::math::subtract; using stan::math::tcrossprod; using stan::math::trace_quad_form; using stan::math::transpose; int r = y.rows(); // number of variables int T = y.cols(); // number of observations int n = G.rows(); // number of states // check y if (!check_finite(function, y, "y", &lp)) return lp; if (!check_not_nan(function, y, "y", &lp)) return lp; // check F if (!check_size_match(function, F.cols(), "columns of F", y.rows(), "rows of y", &lp)) return lp; if (!check_size_match(function, F.rows(), "rows of F", G.rows(), "rows of G", &lp)) return lp; if (!check_finite(function, F, "F", &lp)) return lp; if (!check_not_nan(function, F, "F", &lp)) return lp; // check G if (!check_size_match(function, G.rows(), "rows of G", G.cols(), "columns of G", &lp)) return lp; if (!check_finite(function, G, "G", &lp)) return lp; if (!check_not_nan(function, G, "G", &lp)) return lp; // check V if (!check_nonnegative(function, V, "V", &lp)) return lp; if (!check_size_match(function, V.size(), "size of V", y.rows(), "rows of y", &lp)) return lp; // TODO: support infinite V if (!check_finite(function, V, "V", &lp)) return lp; if (!check_not_nan(function, V, "V", &lp)) return lp; // check W if (!check_spsd_matrix(function, W, "W", &lp)) return lp; if (!check_size_match(function, W.rows(), "rows of W", G.rows(), "rows of G", &lp)) return lp; // TODO: support infinite W if (!check_finite(function, W, "W", &lp)) return lp; if (!check_not_nan(function, W, "W", &lp)) return lp; // check m0 if (!check_size_match(function, m0.size(), "size of m0", G.rows(), "rows of G", &lp)) return lp; if (!check_finite(function, m0, "m0", &lp)) return lp; if (!check_not_nan(function, m0, "m0", &lp)) return lp; // check C0 if (!check_cov_matrix(function, C0, "C0", &lp)) return lp; if (!check_size_match(function, C0.rows(), "rows of C0", G.rows(), "rows of G", &lp)) return lp; if (!check_finite(function, C0, "C0", &lp)) return lp; if (!check_not_nan(function, C0, "C0", &lp)) return lp; if (y.cols() == 0 || y.rows() == 0) return lp; if (include_summand<propto>::value) { lp += 0.5 * NEG_LOG_TWO_PI * r * T; } if (include_summand<propto,T_y,T_F,T_G,T_V,T_W,T_m0,T_C0>::value) { T_lp f; T_lp Q; T_lp Q_inv; T_lp e; Eigen::Matrix<T_lp, Eigen::Dynamic, 1> A(n); Eigen::Matrix<T_lp, Eigen::Dynamic, 1> Fj(n); Eigen::Matrix<T_lp, Eigen::Dynamic, 1> m(n); Eigen::Matrix<T_lp, Eigen::Dynamic, Eigen::Dynamic> C(n, n); // TODO: how to recast matrices for (int i = 0; i < m0.size(); i ++ ) { m(i) = m0(i); } for (int i = 0; i < C0.rows(); i ++ ) { for (int j = 0; j < C0.cols(); j ++ ) { C(i, j) = C0(i, j); } } for (int i = 0; i < y.cols(); i++) { // Predict state // reuse m and C instead of using a and R m = multiply(G, m); C = add(quad_form_sym(C, transpose(G)), W); for (int j = 0; j < y.rows(); ++j) { // predict observation T_lp yij(y(j, i)); // dim Fj = (n, 1) for (int k = 0; k < F.rows(); ++k) { Fj(k) = F(k, j); } // // f_{t,i} = F_{t,i}' m_{t,i-1} f = dot_product(Fj, m); Q = trace_quad_form(C, Fj) + V(j); Q_inv = 1.0 / Q; // // filtered observation // // e_{t,i} = y_{t,i} - f_{t,i} e = yij - f; // // A_{t,i} = C_{t,i-1} F_{t,i} Q_{t,i}^{-1} A = multiply(multiply(C, Fj), Q_inv); // // m_{t,i} = m_{t,i-1} + A_{t,i} e_{t,i} m += multiply(A, e); // // c_{t,i} = C_{t,i-1} - Q_{t,i} A_{t,i} A_{t,i}' // // // tcrossprod throws an error (ambiguous) // C = subtract(C, multiply(Q, tcrossprod(A))); C -= multiply(Q, multiply(A, transpose(A))); C = 0.5 * add(C, transpose(C)); lp -= 0.5 * (log(Q) + pow(e, 2) * Q_inv); } } } return lp; }
inline T max(const Eigen::Matrix<T, R, C>& m) { if (m.size() == 0) return -std::numeric_limits<double>::infinity(); return m.maxCoeff(); }
typename boost::math::tools::promote_args<T_y,T_loc,T_covar>::type multi_normal_cholesky_log(const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y, const Eigen::Matrix<T_loc,Eigen::Dynamic,1>& mu, const Eigen::Matrix<T_covar,Eigen::Dynamic,Eigen::Dynamic>& L) { static const char* function = "stan::prob::multi_normal_cholesky_log(%1%)"; using stan::math::mdivide_left_tri_low; using stan::math::columns_dot_self; using stan::math::multiply; using stan::math::subtract; using stan::math::sum; using stan::math::log; using stan::math::check_size_match; using stan::math::check_finite; using stan::math::check_not_nan; using stan::math::check_cov_matrix; using boost::math::tools::promote_args; typename promote_args<T_y,T_loc,T_covar>::type lp(0.0); if (!check_size_match(function, y.cols(), "Columns of random variable", mu.rows(), "rows of location parameter", &lp)) return lp; if (!check_size_match(function, y.cols(), "Columns of random variable", L.rows(), "rows of covariance parameter", &lp)) return lp; if (!check_size_match(function, y.cols(), "Columns of random variable", L.cols(), "columns of covariance parameter", &lp)) return lp; if (!check_finite(function, mu, "Location parameter", &lp)) return lp; if (!check_not_nan(function, y, "Random variable", &lp)) return lp; if (y.cols() == 0) return lp; if (include_summand<propto>::value) lp += NEG_LOG_SQRT_TWO_PI * y.cols() * y.rows(); if (include_summand<propto,T_covar>::value) { Eigen::Matrix<T_covar,Eigen::Dynamic,1> L_log_diag = L.diagonal().array().log().matrix(); lp -= sum(L_log_diag) * y.rows(); } if (include_summand<propto,T_y,T_loc,T_covar>::value) { Eigen::Matrix<T_loc, Eigen::Dynamic, Eigen::Dynamic> MU(y.rows(),y.cols()); for (typename Eigen::Matrix<T_loc, Eigen::Dynamic, Eigen::Dynamic>::size_type i = 0; i < y.rows(); i++) MU.row(i) = mu; Eigen::Matrix<typename boost::math::tools::promote_args<T_loc,T_y>::type, Eigen::Dynamic,Eigen::Dynamic> y_minus_MU(y.rows(), y.cols()); for (int i = 0; i < y.size(); i++) y_minus_MU(i) = y(i)-MU(i); Eigen::Matrix<typename boost::math::tools::promote_args<T_loc,T_y>::type, Eigen::Dynamic,Eigen::Dynamic> z(y_minus_MU.transpose()); // was = // FIXME: revert this code when subtract() is fixed. // Eigen::Matrix<typename // boost::math::tools::promote_args<T_loc,T_y>::type, // Eigen::Dynamic,Eigen::Dynamic> // z(subtract(y,MU).transpose()); // was = Eigen::Matrix<typename boost::math::tools::promote_args<T_covar,T_loc,T_y>::type, Eigen::Dynamic,Eigen::Dynamic> half(mdivide_left_tri_low(L,z)); lp -= 0.5 * sum(columns_dot_self(half)); } return lp; }