void Filter::addPole (float frequency, float resonance)
{
if (frequency <= 0.0f)
{
addPole(polToCar(newComplex(resonance, 0.0f)));
}
else
{
float theta = frequency / sampleRate * 2*M_PI;
addPole(polToCar(newComplex(resonance, theta)));
addPole(polToCar(newComplex(resonance, -theta)));
}
}
// LOWPASS
pzkContainer * t2lp(pzkContainer * pzk, real w0) {
pzkContainer * f = createPzkContainer(pzk->nextPole, pzk->nextZero);
uint i;
complex tmp;
f->no_wz = pzk->no_wz;
f->amp *= pzk->amp * pow(w0,(real)(-f->no_wz));
for( i = 0; i < pzk->nextPole; i++ ) {
tmp = cmul2(w0, pzk->poles[i]);
addPole(f, tmp);
f->amp *= w0;
if( !cisreal(tmp) ) {
f->amp *= w0;
}
}
for( i = 0; i < pzk->nextZero; i++ ) {
tmp = cmul2(w0, pzk->zeros[i]);
addZero(f, tmp);
if( cisreal(tmp) ) {
f->amp /= w0;
} else {
f->amp /= w0*w0;
}
}
f->type = lowpass;
return f;
}
void PointCloudModelGenerator::flatPole(pcl::PointCloud<PointT>& output, double hole_rate)
{
boost::mt19937 gen( static_cast<unsigned long>(time(0)) );
boost::uniform_real<> dst( 0, 100 );
boost::variate_generator<
boost::mt19937&, boost::uniform_real<>
> rand( gen, dst );
for (double y = -4; y < 4; y = y + 0.01) {
for (double x = -4; x < 4; x = x + 0.01) {
if (rand() >= hole_rate) {
pcl::PointNormal p;
p.x = x;
p.y = y;
output.points.push_back(p);
}
}
}
for (double y = -4; y < 4; y = y + 2.0) {
for (double x = -4; x < 4; x = x + 2.0) {
if (x != 0.0 || y != 0.0) {
addPole(output, Eigen::Vector3f(x, y, 0), 0.2, 2.0);
}
}
}
}
// HIGHPASS
pzkContainer * t2hp(pzkContainer * pzk, real w0) {
pzkContainer * f = createPzkContainer(pzk->nextPole, pzk->nextZero);
uint i;
complex tmp;
f->amp = pzk->amp * pow(w0,(real)pzk->no_wz);
f->no_wz = -pzk->no_wz;
for( i = 0; i < pzk->nextPole; i++ ) {
tmp.re = w0;
tmp.im = 0;
f->no_wz++;
if( cisreal(pzk->poles[i]) ) {
f->amp /= -pzk->poles[i].re;
tmp.re /= pzk->poles[i].re;
} else {
tmp = cdiv(tmp, pzk->poles[i]);
f->amp /= cabs2(pzk->poles[i]);
f->no_wz++;
}
addPole(f, tmp);
}
for( i = 0; i < pzk->nextZero; i++ ) {
tmp.re = w0;
tmp.im = 0;
f->no_wz--;
if( cisreal(pzk->zeros[i]) ) {
f->amp *= -pzk->zeros[i].re;
tmp.re /= pzk->zeros[i].re;
} else {
tmp = cdiv(tmp, pzk->zeros[i]);
f->amp *= cabs2(pzk->zeros[i]);
f->no_wz--;
}
addZero(f, tmp);
}
f->type = highpass;
return f;
}
// BANDSTOP
pzkContainer * t2bs(pzkContainer * pzk, real w0, real dw) {
uint nop, noz, i;
pzkContainer * f;
const real w02 = w0*w0;
complex tmp, beta, gamma, cw0, cdwp2;
cw0.re = w0;
cw0.im = 0;
cdwp2.re = dw / 2;
cdwp2.im = 0;
noz = 2*pzk->nextZero;
nop = 2*pzk->nextPole;
if( pzk->no_wz > 0 ) {
noz += pzk->no_wz;
}
if( pzk->no_wz < 0 ) {
nop -= pzk->no_wz;
}
f = createPzkContainer(nop, noz);
f->wz = w0;
f->no_wz = -pzk->no_wz;
f->amp = pzk->amp * pow(dw, (real)(pzk->no_wz));
gamma.re = 0;
gamma.im = 0;
if( pzk->no_wz > 0 ) {
for( i = 0; i < pzk->no_wz; i++ ) {
addZero(f, gamma);
}
}
if( pzk->no_wz < 0 ) {
for( i = 0; i < -pzk->no_wz; i++ ) {
addPole(f, gamma);
}
}
for( i = 0; i < pzk->nextPole; i++ ) {
if( cisreal(pzk->poles[i]) ) {
beta.re = cdwp2.re / pzk->poles[i].re;
tmp.re = 1 - ( w02/(beta.re*beta.re) );
if( tmp.re >= 0 ) {
tmp.re = sqrt( tmp.re );
gamma.im = 0;
gamma.re = beta.re * (1 + tmp.re);
addPole(f, gamma);
gamma.re = beta.re * (1 - tmp.re);
addPole(f, gamma);
} else {
tmp.im = sqrt( -tmp.re );
tmp.re = 1;
gamma = cmul2( beta.re, tmp );
addPole(f, gamma);
}
f->amp /= -pzk->poles[i].re;
f->no_wz++;
}
else {
beta = cdiv(cdwp2, pzk->poles[i]);
tmp = cdiv(cw0, beta);
tmp = csqrt( csub2(1, cmlt(tmp, tmp)) );
gamma = cmlt(beta, cadd2(1, tmp));
addPole(f, gamma);
gamma = cmlt(beta, csub2(1, tmp));
addPole(f, gamma);
f->amp /= cabs2(pzk->poles[i]);
f->no_wz += 2;
}
}
for( i = 0; i < pzk->nextZero; i++ ) {
if( cisreal(pzk->zeros[i]) ) {
beta.re = cdwp2.re / pzk->zeros[i].re;
tmp.re = 1 - ( w02/(beta.re*beta.re) );
if( tmp.re >= 0 ) {
tmp.re = sqrt( tmp.re );
gamma.im = 0;
gamma.re = beta.re * (1 + tmp.re);
addZero(f, gamma);
gamma.re = beta.re * (1 - tmp.re);
addZero(f, gamma);
} else {
tmp.im = sqrt( -tmp.re );
tmp.re = 1;
gamma = cmul2( beta.re, tmp );
addZero(f, gamma);
}
f->amp *= -pzk->zeros[i].re;
f->no_wz--;
}
else {
beta = cdiv(cdwp2, pzk->zeros[i]);
tmp = cdiv(cw0, beta);
tmp = csqrt( csub2(1, cmlt(tmp, tmp)) );
gamma = cmlt(beta, cadd2(1, tmp));
addZero(f, gamma);
gamma = cmlt(beta, csub2(1, tmp));
addZero(f, gamma);
f->amp *= cabs2(pzk->zeros[i]);
f->no_wz -= 2;
}
}
return f;
}
