bool configure(ResourceFinder &rf)
{
string name=rf.check("name",Value("imuFilter")).asString();
string robot=rf.check("robot",Value("icub")).asString();
size_t gyro_order=(size_t)rf.check("gyro-order",Value(5)).asInt();
size_t mag_order=(size_t)rf.check("mag-order",Value(51)).asInt();
mag_vel_thres_up=rf.check("mag-vel-thres-up",Value(0.04)).asDouble();
mag_vel_thres_down=rf.check("mag-vel-thres-down",Value(0.02)).asDouble();
bias_gain=rf.check("bias-gain",Value(0.001)).asDouble();
verbose=rf.check("verbose");
gyroFilt.setOrder(gyro_order);
magFilt.setOrder(mag_order);
biasInt.setTs(bias_gain);
gyroBias.resize(3,0.0);
adaptGyroBias=false;
iPort.open("/"+name+"/inertial:i");
oPort.open("/"+name+"/inertial:o");
bPort.open("/"+name+"/bias:o");
string imuPortName=("/"+robot+"/inertial");
if (!Network::connect(imuPortName,iPort.getName()))
yWarning("Unable to connect to %s",imuPortName.c_str());
return true;
}
void Window::startMedian()
{
MedianDialog dialog;
if (dialog.exec() == QDialog::Accepted) {
MedianFilter *filter = new MedianFilter(Settings::getUserName(), dialog.getAperture());
filter->Launch();
//QMessageBox *message = new QMessageBox("Filtering", "Finished filtering.", 0, )
}
}
void MainWindow::applyMedianFilter()
{
int filterRadius = ui->lineEdit->text().toInt();
if (filterRadius > 30 || filterRadius <= 0)
{
QMessageBox::warning(this, tr("Incorrect radius"), tr("Please choose radius between 1 and 30"), QMessageBox::Ok);
return;
}
if(!ui->labelImage->pixmap())
{
QMessageBox::warning(this, tr("No image"), tr("Please select image."), QMessageBox::Ok);
return;
}
int imageHeight = image.height(), imageWeight = image.width();
//qDebug()<<"image size"<<image.height()<<image.width();
if (filterRadius > imageHeight/2 || filterRadius >imageWeight/2)
{
QMessageBox::warning(this, tr("Incorrect radius"), tr("The chosen filter radius is too big for this image!"), QMessageBox::Ok);
return;
}
MedianFilter medianFilter;
element* resImageBits;
resImageBits = new element[imageHeight * imageWeight];
if (!resImageBits)
return;
medianFilter.applyMedianFilter((element*)image.bits(), resImageBits, imageHeight, imageWeight, filterRadius);
if(resImageBits)
{
QImage destImage((uchar*)resImageBits, imageWeight, imageHeight, image.format());
QPixmap pixRes;
pixRes.convertFromImage(destImage);
ui->labelMedianFilterResult->setPixmap(pixRes.scaled(MAXSIDE, MAXSIDE, Qt::KeepAspectRatio));
}
else
qDebug()<<"median filter failed";
delete [] resImageBits;
/*
QImage imageTestGray8 = ui->labelImage->pixmap()->toImage().convertToFormat(QImage::Format_Grayscale8);
QImage destImageTestGray8;
qDebug()<<imageTestGray8<<imageTestGray8.pixelFormat().channelCount() <<imageTestGray8.pixelFormat().bitsPerPixel();
QPixmap pixmapRezGray8;
pixmapRezGray8.convertFromImage(destImageTestGray8);
ui->labelMedianFilterResult->setPixmap(pixmapRezGray8);
// ctmf(image.bits(), destImage.bits(), image.width(), image.height(),
// image.pixelFormat().bitsPerPixel(), image.pixelFormat().bitsPerPixel(), image.pixelFormat().channelCount(), filterRadius, 256*1024);
*/
}
bool updateModule()
{
Vector *imuData=iPort.read();
if (imuData==NULL)
return false;
Stamp stamp;
iPort.getEnvelope(stamp);
double t0=Time::now();
Vector gyro=imuData->subVector(6,8);
Vector gyro_filt=gyroFilt.filt(gyro);
gyro-=gyroBias;
gyro_filt-=gyroBias;
Vector mag_filt=magFilt.filt(imuData->subVector(9,11));
double magVel=norm(velEst.estimate(AWPolyElement(mag_filt,stamp.getTime())));
adaptGyroBias=adaptGyroBias?(magVel<mag_vel_thres_up):(magVel<mag_vel_thres_down);
gyroBias=biasInt.integrate(adaptGyroBias?gyro_filt:Vector(3,0.0));
double dt=Time::now()-t0;
if (oPort.getOutputCount()>0)
{
Vector &outData=oPort.prepare();
outData=*imuData;
outData.setSubvector(6,gyro);
oPort.setEnvelope(stamp);
oPort.write();
}
if (bPort.getOutputCount()>0)
{
bPort.prepare()=gyroBias;
bPort.setEnvelope(stamp);
bPort.write();
}
if (verbose)
{
yInfo("magVel = %g => [%s]",magVel,adaptGyroBias?"adapt-gyroBias":"no-adaption");
yInfo("gyro = %s",gyro.toString(3,3).c_str());
yInfo("gyroBias = %s",gyroBias.toString(3,3).c_str());
yInfo("dt = %.0f [us]",dt*1e6);
yInfo("\n");
}
return true;
}
void DespeckleCommand::onExecute(Context* context)
{
IDocumentSettings* docSettings = context->settings()->getDocumentSettings(context->activeDocument());
MedianFilter filter;
filter.setTiledMode(docSettings->getTiledMode());
filter.setSize(get_config_int(ConfigSection, "Width", 3),
get_config_int(ConfigSection, "Height", 3));
FilterManagerImpl filterMgr(context, &filter);
filterMgr.setTarget(TARGET_RED_CHANNEL |
TARGET_GREEN_CHANNEL |
TARGET_BLUE_CHANNEL |
TARGET_GRAY_CHANNEL);
DespeckleWindow window(filter, filterMgr);
if (window.doModal()) {
set_config_int(ConfigSection, "Width", filter.getWidth());
set_config_int(ConfigSection, "Height", filter.getHeight());
}
}
void DespeckleCommand::onExecute(Context* context)
{
DocumentPreferences& docPref = App::instance()->preferences().document(context->activeDocument());
MedianFilter filter;
filter.setTiledMode((filters::TiledMode)docPref.tiled.mode());
filter.setSize(get_config_int(ConfigSection, "Width", 3),
get_config_int(ConfigSection, "Height", 3));
FilterManagerImpl filterMgr(context, &filter);
filterMgr.setTarget(TARGET_RED_CHANNEL |
TARGET_GREEN_CHANNEL |
TARGET_BLUE_CHANNEL |
TARGET_GRAY_CHANNEL);
DespeckleWindow window(filter, filterMgr);
if (window.doModal()) {
set_config_int(ConfigSection, "Width", filter.getWidth());
set_config_int(ConfigSection, "Height", filter.getHeight());
}
}
DespeckleWindow(MedianFilter& filter, FilterManagerImpl& filterMgr)
: FilterWindow("Median Blur", ConfigSection, &filterMgr,
WithChannelsSelector,
WithTiledCheckBox,
filter.getTiledMode())
, m_filter(filter)
, m_controlsWidget(app::load_widget<ui::Widget>("despeckle.xml", "controls"))
, m_widthEntry(app::find_widget<ui::Entry>(m_controlsWidget, "width"))
, m_heightEntry(app::find_widget<ui::Entry>(m_controlsWidget, "height"))
{
getContainer()->addChild(m_controlsWidget);
m_widthEntry->setTextf("%d", m_filter.getWidth());
m_heightEntry->setTextf("%d", m_filter.getHeight());
m_widthEntry->EntryChange.connect(&DespeckleWindow::onSizeChange, this);
m_heightEntry->EntryChange.connect(&DespeckleWindow::onSizeChange, this);
}
static void process(const MedianFilter& filter) { filter.process(); }
