00001 #include "FLIRTDemo.h"
00002
00003 #include "FLIRTDemo.moc"
00004
00005 #include <typeinfo>
00006 #include <iostream>
00007 #include <string>
00008 #include <sstream>
00009
00010 FLIRTDemo::FLIRTDemo(QWidget* parent, SensorStream& sensor):
00011 QWidget(parent),
00012 m_sensor(sensor),
00013 m_currentPoint(NULL),
00014 m_reading(NULL)
00015 {
00016 m_sensorWidget = new SensorStreamWidget(this);
00017 m_rendererWidget = new RendererWidget(this);
00018 m_descriptorRendererWidget = new RendererWidget(this);
00019 m_peakMR = new SimpleMinMaxPeakFinder(0.2, 0.04);
00020 m_peakR = new SimplePeakFinder(0.2, 0.04);
00021 m_peakC = new SimplePeakFinder(0.3, 0.004);
00022 m_peakMNE = new SimpleMinMaxPeakFinder(0.2, 0.04);
00023 m_peakNE = new SimplePeakFinder(0.2, 0.04);
00024 m_peakMNB = new SimpleMinMaxPeakFinder(0.2, 0.04);
00025 m_peakNB = new SimplePeakFinder(0.2, 0.04);
00026 m_detectorR = new RangeDetector(m_peakMR,1);
00027 m_detectorC = new CurvatureDetector(m_peakC,1);
00028 m_detectorNB = new NormalBlobDetector(m_peakMNB,3,1);
00029 m_detectorNE = new NormalEdgeDetector(m_peakMNE,3,1);
00030 m_layout = new QGridLayout(this);
00031 m_parameter = new QTabWidget(this);
00032 m_chooserW = new QWidget(0);
00033 m_parameter->insertTab(1,m_chooserW,"Chooser");
00034 QVBoxLayout *chooserLayout = new QVBoxLayout(m_chooserW);
00035
00036 m_plotWidget = new MultiScaleDetectorPlotWidget(this, m_detectorR->getScaleNumber());
00037
00038 m_detectorPresenterR = new MultiScaleDetectorPresenter(m_detectorR, new TabbedParameterWidget("RangeDetector", m_parameter,0));
00039 m_peakMRPresenter = new SimplePeakFinderPresenter(m_peakMR, new TabbedParameterWidget("PeakFinder", m_parameter,2));
00040 m_peakRPresenter = new SimplePeakFinderPresenter(m_peakR, new TabbedParameterWidget("PeakFinder", m_parameter,2));
00041 m_detectorPresenterR->insertPeakFinder("SimpleMinMax", m_peakMRPresenter);
00042 m_detectorPresenterR->insertPeakFinder("Simple", m_peakRPresenter);
00043
00044 m_detectorPresenterNB = new MultiScaleNormalDetectorPresenter(m_detectorNB, new TabbedParameterWidget("NormalBlobDetector", m_parameter,1));
00045 m_peakMNBPresenter = new SimplePeakFinderPresenter(m_peakMNB, new TabbedParameterWidget("PeakFinder", m_parameter,2));
00046 m_peakNBPresenter = new SimplePeakFinderPresenter(m_peakNB, new TabbedParameterWidget("PeakFinder", m_parameter,2));
00047 m_detectorPresenterNB->insertPeakFinder("SimpleMinMax", m_peakMNBPresenter);
00048 m_detectorPresenterNB->insertPeakFinder("Simple", m_peakNBPresenter);
00049
00050 m_detectorPresenterNE = new MultiScaleNormalDetectorPresenter(m_detectorNE, new TabbedParameterWidget("NormalEdgeDetector", m_parameter,1));
00051 m_peakMNEPresenter = new SimplePeakFinderPresenter(m_peakMNE, new TabbedParameterWidget("PeakFinder", m_parameter,2));
00052 m_peakNEPresenter = new SimplePeakFinderPresenter(m_peakNE, new TabbedParameterWidget("PeakFinder", m_parameter,2));
00053 m_detectorPresenterNE->insertPeakFinder("SimpleMinMax", m_peakMNEPresenter);
00054 m_detectorPresenterNE->insertPeakFinder("Simple", m_peakNEPresenter);
00055
00056 m_detectorPresenterC = new MultiScaleCurvatureDetectorPresenter(m_detectorC, new TabbedParameterWidget("CurvatureDetector", m_parameter,1));
00057 m_peakCPresenter = new SimplePeakFinderPresenter(m_peakC, new TabbedParameterWidget("PeakFinder", m_parameter,2));
00058 m_detectorPresenterC->insertPeakFinder("Simple", m_peakCPresenter);
00059
00060 m_chooser = new DetectorChooserPresenter(new ParameterWidget("DetectorChooser", m_chooserW));
00061 m_chooser->insertDetector("CurvatureDetector", m_detectorPresenterC);
00062 m_chooser->insertDetector("RangeDetector", m_detectorPresenterR);
00063 m_chooser->insertDetector("NormalBlobDetector", m_detectorPresenterNB);
00064 m_chooser->insertDetector("NormalEdgeDetector", m_detectorPresenterNE);
00065
00066 m_descriptorGeneratorS = new ShapeContextGenerator(0.02, 0.5, 4, 12);
00067 m_descriptorPresenterS = new ShapeContextPresenter(m_descriptorGeneratorS, new TabbedParameterWidget("ShapeContext", m_parameter,3));
00068
00069 m_descriptorGeneratorB = new BetaGridGenerator(0.02, 0.5, 4, 12);
00070 m_descriptorPresenterB = new BetaGridPresenter(m_descriptorGeneratorB, new TabbedParameterWidget("BetaGrid", m_parameter,3));
00071
00072 m_chooserD = new DescriptorChooserPresenter(new ParameterWidget("DescriptorChooser", m_chooserW));
00073 m_chooserD->insertDescriptor("BetaGrid", m_descriptorPresenterB);
00074 m_chooserD->insertDescriptor("ShapeContext", m_descriptorPresenterS);
00075
00076 m_interestRenderer = 0;
00077 m_laserRenderer = 0;
00078 m_supportRenderer = 0;
00079 m_polarRenderer = 0;
00080
00081 m_layout->setSpacing(3);
00082 m_layout->setMargin(3);
00083 m_layout->addWidget(m_sensorWidget,0,0);
00084 m_layout->addWidget(m_rendererWidget,1,0);
00085 m_layout->addWidget(m_descriptorRendererWidget,2,1);
00086 m_layout->setRowStretch(1,1);
00087 m_layout->addWidget(m_plotWidget,2,0);
00088 m_layout->addWidget(m_parameter,1,1);
00089 chooserLayout->addWidget(m_chooser->getChooserParameter());
00090 chooserLayout->addWidget(m_chooserD->getChooserParameter());
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105 m_sensor.seek(0,END);
00106 unsigned int end = m_sensor.tell();
00107 m_sensor.seek(0,BEGIN);
00108 m_sensorWidget->seekable(m_sensor.isSeekable(), end);
00109 m_sensorWidget->streamReady();
00110 m_sensorWidget->setMaximumHeight(32);
00111 m_plotWidget->setMaximumHeight(256);
00112 m_plotWidget->setMinimumHeight(256);
00113 m_parameter->setMaximumWidth(350);
00114 m_parameter->setMinimumWidth(350);
00115
00116 m_parameter->show();
00117
00118 connect(m_sensorWidget, SIGNAL(newReading()), this, SLOT(next()));
00119 connect(m_sensorWidget, SIGNAL(newReading(int)), this, SLOT(next(int)));
00120 connect(m_chooser, SIGNAL(detectorChanged()), this, SLOT(update()));
00121 connect(m_chooserD, SIGNAL(descriptorChanged()), this, SLOT(updateDescriptor()));
00122 connect(m_descriptorPresenterS, SIGNAL(descriptorChanged()), this, SLOT(updateDescriptor()));
00123 connect(m_descriptorPresenterB, SIGNAL(descriptorChanged()), this, SLOT(updateDescriptor()));
00124 connect(m_rendererWidget, SIGNAL(mousePressedGL(int, int, int)), this, SLOT(mousePressedGL(int, int, int)));
00125
00126 const AbstractReading* read = m_sensor.current();
00127 processReading(read);
00128 }
00129
00130 FLIRTDemo::~FLIRTDemo() {
00131 delete m_sensorWidget;
00132 delete m_rendererWidget;
00133 delete m_peakR;
00134 delete m_detectorR;
00135 delete m_layout;
00136 delete m_plotWidget;
00137 if(m_interestRenderer) delete m_interestRenderer;
00138 if(m_laserRenderer) delete m_laserRenderer;
00139 if(m_supportRenderer) delete m_supportRenderer;
00140 delete m_sensorWidget;
00141 }
00142
00143 void FLIRTDemo::next() {
00144 m_sensor.next();
00145 const AbstractReading* read = m_sensor.current();
00146 processReading(read);
00147 }
00148
00149 void FLIRTDemo::next(int pos) {
00150 m_sensor.seek((unsigned int) pos);
00151 const AbstractReading* read = m_sensor.current();
00152 processReading(read);
00153 }
00154
00155 void FLIRTDemo::update(){
00156 if(dynamic_cast<const MultiScaleDetector*>(m_chooser->getCurrentDetector())){
00157 m_plotWidget->setScales(static_cast<const MultiScaleDetector*>(m_chooser->getCurrentDetector())->getScaleNumber());
00158 } else if(dynamic_cast<const CurvatureDetector*>(m_chooser->getCurrentDetector())){
00159 m_plotWidget->setScales(static_cast<const CurvatureDetector*>(m_chooser->getCurrentDetector())->getScaleNumber());
00160 }
00161 const AbstractReading* read = m_sensor.current();
00162 processReading(read);
00163 }
00164
00165 void FLIRTDemo::mousePressedGL(int button, int x, int y){
00166 if(button & Qt::LeftButton){
00167 GLdouble worldX, worldY, worldZ;
00168 m_rendererWidget->unprojectCoordinates(x, y, &worldX, &worldY, &worldZ);
00169
00170 if(!m_points.size()) return;
00171 double minDistance = 10e16;
00172 unsigned int minIndex = 0;
00173 Point2D clickedPoint(worldX, worldY);
00174 for(unsigned int i = 0; i < m_points.size(); i ++){
00175 Point2D difference = clickedPoint - m_points[i]->getPosition();
00176 double distance = hypot(difference.x, difference.y);
00177 if(distance < minDistance){
00178 minDistance = distance;
00179 minIndex = i;
00180 }
00181 }
00182 if(minDistance < 0.2){
00183 m_currentPoint = m_points[minIndex];
00184 m_supportRenderer->setLaserPoints(&m_currentPoint->getSupport());
00185 m_supportRenderer->setColor(m_colors[minIndex]);
00186 m_polarRenderer->setColor(m_colors[minIndex]);
00187 } else {
00188 m_currentPoint = NULL;
00189 }
00190 }
00191 drawDescriptor();
00192 }
00193
00194 void FLIRTDemo::updateDescriptor(){
00195 for(unsigned int i = 0; i < m_points.size(); i++){
00196 m_points[i]->setDescriptor(m_chooserD->getCurrentDescriptor()->describe(*m_points[i], *m_reading));
00197 }
00198 drawDescriptor();
00199 }
00200
00201 void FLIRTDemo::drawDescriptor(){
00202 if(m_currentPoint){
00203 if(const ShapeContext * desc = dynamic_cast<const ShapeContext *>(m_currentPoint->getDescriptor())){
00204 m_polarRenderer->setGrid(&desc->getHistogram(), &m_descriptorGeneratorS->getPhiEdges(), &m_descriptorGeneratorS->getRhoEdges());
00205 m_polarRenderer->setPosition(&m_currentPoint->getPosition());
00206 m_descriptorRendererWidget->setLaserPose(m_currentPoint->getPosition().x, m_currentPoint->getPosition().y);
00207 } else if(const BetaGrid * desc = dynamic_cast<const BetaGrid *>(m_currentPoint->getDescriptor())){
00208 m_polarRenderer->setGrid(&desc->getHistogram(), &m_descriptorGeneratorB->getPhiEdges(), &m_descriptorGeneratorB->getRhoEdges());
00209 m_polarRenderer->setPosition(&m_currentPoint->getPosition());
00210 m_descriptorRendererWidget->setLaserPose(m_currentPoint->getPosition().x, m_currentPoint->getPosition().y);
00211 }
00212 } else {
00213 m_polarRenderer->setGrid(NULL, NULL, NULL);
00214 m_polarRenderer->setPosition(NULL);
00215 m_supportRenderer->setLaserPoints(NULL);
00216 }
00217 m_descriptorRendererWidget->updateGL();
00218 m_rendererWidget->updateGL();
00219 }
00220
00221
00222 void FLIRTDemo::processReading(const AbstractReading* _read){
00223 const LaserReading* lread = dynamic_cast<const LaserReading*>(_read);
00224 if (lread){
00225 const std::vector<Point2D>& points = lread->getCartesian();
00226 std::vector< double > signal;
00227 std::vector< std::vector<double> > smooth;
00228 std::vector< std::vector<double> > diff;
00229 std::vector< std::vector<unsigned int> > idx;
00230 QVector<double> xpoints(points.size());
00231 for(int i = 0; i < xpoints.size(); i++) xpoints[i] = (double)i;
00232 m_chooser->getCurrentDetector()->detect(*lread, m_points, signal, smooth, diff, idx);
00233 const std::vector<QColor>& colors = m_plotWidget->getColors();
00234 for(unsigned int i = 0; i < smooth.size(); i++){
00235 QVector<double> smoothV = QVector<double>::fromStdVector(smooth[i]);
00236 QVector<double> diffV = QVector<double>::fromStdVector(diff[i]);
00237 m_plotWidget->setSmoothData(xpoints, smoothV, i);
00238 m_plotWidget->setDifferentialData(xpoints, diffV, i);
00239 QVector<double> smoothMarker(idx[i].size());
00240 QVector<double> diffMarker(idx[i].size());
00241 QVector<double> indexes(idx[i].size());
00242 for(unsigned int j = 0; j < idx[i].size(); j++){
00243 smoothMarker[j] = smooth[i][idx[i][j]];
00244 diffMarker[j] = diff[i][idx[i][j]];
00245 indexes[j] = idx[i][j];
00246 }
00247 m_plotWidget->setSmoothMarker(indexes, smoothMarker, i);
00248 m_plotWidget->setDifferentialMarker(indexes, diffMarker, i);
00249 }
00250 m_interestPoints.resize(m_points.size());
00251 m_scales.resize(m_points.size());
00252 m_colors.resize(m_points.size(),Color(1.,1.,1.,1.));
00253 for(unsigned int i = 0; i < m_points.size(); i++){
00254 m_points[i]->setDescriptor(m_chooserD->getCurrentDescriptor()->describe(*m_points[i], *lread));
00255 m_interestPoints[i] = &(m_points[i]->getPosition());
00256 m_scales[i] = m_points[i]->getScale();
00257 m_colors[i] = toColor(colors[m_points[i]->getScaleLevel()]);
00258 }
00259 if(!m_laserRenderer){
00260 m_interestRenderer = new InterestPointRenderer(&m_interestPoints, &m_scales);
00261 m_laserRenderer = new LaserReadingRenderer(&lread->getCartesian());
00262 m_supportRenderer = new LaserReadingRenderer(NULL);
00263 m_polarRenderer = new PolarGridRenderer(NULL, NULL, NULL);
00264 m_supportRenderer->setSize(m_laserRenderer->getSize() + 0.01f);
00265 m_descriptorRendererWidget->addRenderer(m_polarRenderer);
00266 m_descriptorRendererWidget->addRenderer(m_laserRenderer);
00267 m_rendererWidget->addRenderer(m_laserRenderer);
00268 m_rendererWidget->addRenderer(m_supportRenderer);
00269 m_rendererWidget->addRenderer(m_interestRenderer);
00270 m_laserRenderer->setLaserPose(&lread->getLaserPose());
00271 }else {
00272 m_laserRenderer->setLaserPoints(&points);
00273 m_interestRenderer->setInterestPoints(&m_interestPoints, &m_scales);
00274 m_laserRenderer->setLaserPose(&lread->getLaserPose());
00275 }
00276 m_polarRenderer->setGrid(NULL, NULL, NULL);
00277 m_polarRenderer->setPosition(NULL);
00278 m_supportRenderer->setLaserPoints(NULL);
00279 m_interestRenderer->setColors(m_colors);
00280 m_rendererWidget->setLaserPose(lread->getLaserPose().x, lread->getLaserPose().y);
00281 m_reading = lread;
00282 }
00283 m_plotWidget->replot();
00284 m_rendererWidget->updateGL();
00285 m_descriptorRendererWidget->updateGL();
00286 }
00287
00288
00289 int main(int argc, char **argv){
00290
00291 if(argc < 2){
00292 std::cerr << "Usage : flirtDemo <logfile>" << std::endl;
00293 exit(-1);
00294 }
00295 std::string filename(argv[1]);
00296
00297 QApplication app(argc, argv);
00298
00299 CarmenLogWriter writer;
00300 CarmenLogReader reader;
00301
00302 LogSensorStream log(&reader, &writer);
00303
00304 log.load(filename);
00305
00306 FLIRTDemo* widget = new FLIRTDemo(0, log);
00307 widget->setGeometry(100, 100, 500, 500);
00308 widget->show();
00309
00310 return app.exec();
00311 }
