/** Constructor
*
* returns a Particle filter object.
*
* @param process The process model object that the PF is defined for
* @param resampler The resampling algorithm
* @param particles_num Number of particles \f$M\f$
*/
Particle(Process process, Resampler resampler, unsigned long particles_num)
: process_{process}, resampler_{resampler}, num_{particles_num} {
// initialized w_ and state_par_
w_.resize(num_);
// take one sample to find out dimension
auto tmp = process_.template getProcess<0>().getInitialPDF().random();
state_par_.resize(tmp.size(), num_);
}
void Static_grid_filter::create_cube(arma::mat& points,float l, float w, float h, float bin_w,
const arma::colvec3 position, const arma::vec3& rpy){
std::cout<< "-1" << std::endl;
std::size_t num_l = l/bin_w;
std::size_t num_w = w/bin_w;
std::size_t num_h = h/bin_w;
if(num_h == 0){
num_h = 1;
}
if(num_w == 0){
num_w = 1;
}
if(num_l == 0){
num_l = 1;
}
tf::Matrix3x3 R;
tf::Vector3 p;
std::cout<< "-2" << std::endl;
std::cout<< "num_l: " << num_l << std::endl;
std::cout<< "num_w: " << num_w << std::endl;
std::cout<< "nu_h: " << num_h << std::endl;
//R.setRPY(rpy(0),rpy(1),rpy(2));
R.setIdentity();
tf::Vector3 t(-l/2.0 + position(0),-w/2.0 + position(1),-h/2.0 + position(2));
std::cout<< "-3" << std::endl;
std::cout<< "before resize" << std::endl;
std::cout<< "total num points: " << num_l * num_w * num_h << std::endl;
points.resize(num_l * num_w * num_h,3);
std::cout<< "after resize" << std::endl;
std::size_t index = 0;
float step = bin_w/2;
for(std::size_t i = 0; i < num_l; i++){
for(std::size_t j = 0; j < num_w; j++){
for(std::size_t k=0; k < num_h; k++){
p.setX((float)i * bin_w + step);
p.setY((float)j * bin_w + step);
p.setZ((float)k * bin_w + step);
p = R * p + t;
points(index,0) = p.x();
points(index,1) = p.y();
points(index,2) = p.z();
index++;
}
}
}
std::cout<< "finished creating cube" << std::endl;
}
void Koornwinder2D::EvaluatePolynomial(const int p, const arma::mat& xin,
arma::mat &f, arma::cube &df){
using namespace arma;
const int ndof = (p+1)*(p+2)/2;
const int npt = xin.n_rows;
arma::mat x = 2.*xin-1.;
vec xi = x.col(0);
vec eta = x.col(1);
f.resize(ndof,npt);
df.resize(ndof,npt,2);
const umat pind = pascalindex(p);
arma::vec r = 2.*(1.0+xi)/(1.-eta)-1.;
const arma::vec s = eta;
for(int i=0; i<npt; i++){
if(x(i,1)==1){
r(i) = -1.0;
}
}
const arma::vec dr_dxi = 2./(1.0-x.col(1));
const arma::vec dr_deta = 2.*(1.0+x.col(0))/pow((1.-x.col(1)),2.0);
const arma::vec2 cv = {0.5,-0.5};
arma::vec pval(npt), qval(npt), dpval(npt), dqval(npt);
arma::vec dpp,dqp;
arma::vec pp, qp;
for(int i=0; i<ndof; i++){
pp = jacobi(pind(i,0),0.0,0.0);
qp = jacobi(pind(i,1),2.*pind(i,0)+1.,0.0);
for(int j=0; j<pind(i,0); j++){
qp = conv(cv,qp);
}
polyval(pp,r,pval);
polyval(qp,s,qval);
const double fc = sqrt((2.*pind(i,0)+1.)*2.*(pind(i,0)+pind(i,1)+1.));
polyder(pp,dpp);
polyder(qp,dqp);
polyval(dpp,r,dpval);
polyval(dqp,s,dqval);
for(int j=0; j<npt; j++){
f(i,j) = fc*pval(j)*qval(j);
df(i,j,0) = 2.0*fc*dpval(j)*qval(j)*dr_dxi(j);
df(i,j,1) = 2.0*fc*(dpval(j)*qval(j)*dr_deta(j) + pval(j)*dqval(j));
}
}
}
void Koornwinder3D::EvaluatePolynomial(const int p, const arma::mat& xin,
arma::mat &f, arma::cube &df){
using namespace arma;
const int ndof = (p+1)*(p+2)*(p+3)/6;
const int npts = xin.n_rows;
f.resize(ndof,npts);
df.resize(ndof,npts,3);
const mat x = 2.*xin-1.;
const vec xi = x.unsafe_col(0);
const vec eta = x.unsafe_col(1);
const vec zeta = x.unsafe_col(2);
vec r = -2.*(1.+xi)/(eta+zeta) - 1.;
vec s = 2.*(1.+eta)/(1.-zeta) - 1.;
vec t = zeta;
for(int i=0; i<r.n_rows; i++){
if(std::abs(zeta(i)-1.0) < 1.0e-10) s(i) = -1.0;
if(std::abs(eta(i)+zeta(i)) < 1.0e-10) r(i) = -1.0;
}
const vec dr_dxi = -2./(eta+zeta);
const vec dr_deta = 2.*(1.+xi)/pow(eta+zeta,2.0);
const vec dr_dzeta = 2.*(1.+xi)/pow(eta+zeta,2.0);
const vec ds_deta = 2./(1. - zeta);
const vec ds_dzeta = -2.*(1. + eta)/pow(1. - zeta,2.0);
const imat mnl = tetindex(p);
const vec2 pm_half = {0.5, -0.5};
vec pm, pn, pl;
vec dpm, dpn, dpl;
vec mval(npts), nval(npts), lval(npts);
vec dmval(npts), dnval(npts), dlval(npts);
for(int i=0; i<ndof; i++){
const int m = mnl(i,0);
const int n = mnl(i,1);
const int l = mnl(i,2);
pm = jacobi(m,0.,0.);
pn = jacobi(n,2.*m+1.,0.);
pl = jacobi(l,2.*(m+n+1.),0.);
for(int j=0; j<m; j++){
pn = conv(pm_half,pn);
}
for(int j=0; j<(m+n); j++){
pl = conv(pm_half,pl);
}
polyder(pm,dpm);
polyder(pn,dpn);
polyder(pl,dpl);
polyval(pm,r,mval);
polyval(pn,s,nval);
polyval(pl,t,lval);
polyval(dpm,r,dmval);
polyval(dpn,s,dnval);
polyval(dpl,t,dlval);
double fc = sqrt((2.*m+1.)*(2.*n+1+2.*m+1.)*(2.*l+1+2.*(m+n+1.)));
for(int j=0; j<npts; j++){
f(i,j) = fc*mval(j)*nval(j)*lval(j);
df(i,j,0) = 2.0*fc*dmval(j)*dr_dxi(j)*nval(j)*lval(j);
df(i,j,1) = 2.0*fc*(dmval(j)*dr_deta(j)*nval(j)*lval(j) + mval(j)*
dnval(j)*ds_deta(j)*lval(j));
df(i,j,2) = 2.0*fc*(dmval(j)*dr_dzeta(j)*nval(j)*lval(j) + mval(j)*
(dnval(j)*ds_dzeta(j)*lval(j) + nval(j)*dlval(j)));
}
}
}
