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