feat(slam): add rtabmap_ros

rtabmap/guilib/src/3rdParty/QMultiComboBox.cpp

@@ -0,0 +1,237 @@
+/****************************************************************************
+**
+** Copyright (c) 2012 Richard Steffen and/or its subsidiary(-ies).
+** All rights reserved.
+** Contact: rsteffen@messbild.de, rsteffen@uni-bonn.de
+**
+** QMultiComboBox is free to use unter the terms of the LGPL 2.1 License in
+** Free and Commercial Products.
+****************************************************************************/
+
+#include "QMultiComboBox.h"
+#include <QApplication>
+#include <QCoreApplication>
+#include <QWindow>
+#include <QScreen>
+
+#include "rtabmap/utilite/ULogger.h"
+
+QMultiComboBox::QMultiComboBox(QWidget *widget ) :
+    QComboBox(widget),
+    popheight_(0),
+    screenbound_(50),
+    popframe_(NULL, Qt::Popup)
+{
+
+    SetDisplayText("Not Set");
+
+    // setup the popup list
+    vlist_.setSelectionMode(QAbstractItemView::MultiSelection);
+    vlist_.setSelectionBehavior(QAbstractItemView::SelectItems);
+    vlist_.clearSelection();
+    popframe_.setLayout(new QVBoxLayout());
+    popframe_.layout()->addWidget(&vlist_);
+    popframe_.layout()->setContentsMargins(0,0,0,0);
+
+    connect(&vlist_, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(scanItemSelect(QListWidgetItem*)));
+
+}
+
+
+QMultiComboBox::~QMultiComboBox()
+{
+    disconnect(&vlist_,0,0,0);
+}
+
+
+void QMultiComboBox::SetDisplayText(QString text)
+{
+    m_DisplayText_ = text;
+#if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0)
+    const int textWidth = fontMetrics().horizontalAdvance(text);
+#else
+    const int textWidth = fontMetrics().width(text);
+#endif
+    setMinimumWidth(textWidth + 30);
+    updateGeometry();
+    repaint();
+}
+
+
+QString QMultiComboBox::GetDisplayText() const
+{
+    return m_DisplayText_;
+}
+
+
+void QMultiComboBox::setPopupHeight(int h)
+{
+    popheight_ = h;
+}
+
+
+void QMultiComboBox::paintEvent(QPaintEvent *e)
+{
+    QStylePainter painter(this);
+    painter.setPen(palette().color(QPalette::Text));
+    // draw the combobox frame, focusrect and selected etc.
+    QStyleOptionComboBox opt;
+
+    initStyleOption(&opt);
+    opt.currentText = m_DisplayText_;
+    painter.drawComplexControl(QStyle::CC_ComboBox, opt);
+    // draw the icon and text
+    painter.drawControl(QStyle::CE_ComboBoxLabel, opt);
+}
+
+
+void QMultiComboBox::showPopup()
+{
+    QRect rec = QRect(geometry());
+
+    //QPoint p = this->mapToGlobal(QPoint(0,rec.height()));
+    //QRect rec2(p , p + QPoint(rec.width(), rec.height()));
+
+    // get the two possible list points and height
+    QRect screen = this->window()->windowHandle()->screen()->availableGeometry();
+    QPoint above = this->mapToGlobal(QPoint(0,0));
+    int aboveHeight = above.y() - screen.y();
+    QPoint below = this->mapToGlobal(QPoint(0,rec.height()));
+    int belowHeight = screen.bottom() - below.y();
+
+	// compute width
+	int textWidth = vlist_.sizeHint().width();
+
+    // first activate it with height 1px to get all the items initialized
+    QRect rec2;
+    rec2.setTopLeft(below);
+    rec2.setWidth(textWidth>rec.width()? textWidth:rec.width());
+    rec2.setHeight(rec.height());
+    popframe_.setGeometry(rec2);
+    popframe_.raise();
+    popframe_.show();
+    QCoreApplication::processEvents();
+
+    // determine rect
+    int contheight = vlist_.count()*vlist_.sizeHintForRow(0) + 4; // +4 - should be determined by margins?
+    belowHeight = std::min(abs(belowHeight)-screenbound_, contheight);
+    aboveHeight = std::min(abs(aboveHeight)-screenbound_, contheight);
+    if (popheight_ > 0) // fixed
+    {
+        rec2.setHeight(popheight_);
+    }
+    else // dynamic
+    {
+        // do we use below or above
+        if (belowHeight==contheight || belowHeight>aboveHeight)
+        {
+            rec2.setTopLeft(below);
+            rec2.setHeight(belowHeight);
+        }
+        else
+        {
+            rec2.setTopLeft(above - QPoint(0,aboveHeight));
+            rec2.setHeight(aboveHeight);
+        }
+    }
+    popframe_.setGeometry(rec2);
+    popframe_.raise();
+    popframe_.show();
+}
+
+
+void QMultiComboBox::hidePopup()
+{
+    popframe_.hide();
+}
+
+
+void QMultiComboBox::addItem ( const QString & text, const QVariant & userData)
+{
+    QListWidgetItem* wi = new QListWidgetItem(text);
+    wi->setFlags(wi->flags() | Qt::ItemIsUserCheckable);
+    if (userData.toBool())
+        wi->setCheckState(Qt::Checked);
+    else
+        wi->setCheckState(Qt::Unchecked);
+    vlist_.addItem(wi);
+	vlist_.setMinimumWidth(vlist_.sizeHintForColumn(0));
+}
+
+
+int QMultiComboBox::count()
+{
+    return vlist_.count();
+}
+
+
+void QMultiComboBox::setCurrentIndex(int index)
+{
+  //  cout << __FUNCTION__ << "DONT USE THIS ................" << endl;
+}
+
+
+QString QMultiComboBox::currentText()
+{
+    return vlist_.currentItem()->text();
+}
+
+
+QString QMultiComboBox::itemText(int row)
+{
+    return vlist_.item(row)->text();
+}
+
+
+QVariant QMultiComboBox::itemData(int row)
+{
+    QListWidgetItem* item = vlist_.item(row);
+    if (item->checkState() == Qt::Checked) return QVariant(true);
+    return QVariant(false);
+}
+
+void QMultiComboBox::setItemChecked(int row, bool checked)
+{
+	QListWidgetItem* item = vlist_.item(row);
+	bool wasChecked = item->checkState() == Qt::Checked;
+	if (wasChecked != checked)
+	{
+		item->setCheckState(checked?Qt::Checked:Qt::Unchecked);
+		Q_EMIT itemChanged();
+	}
+}
+
+
+void QMultiComboBox::scanItemSelect(QListWidgetItem* item)
+{
+
+    QList<QListWidgetItem*> list = vlist_.selectedItems();
+    for (int i = 0; i < list.count(); i++)
+    {
+        if (item->checkState() == Qt::Checked)
+        {
+            list[i]->setCheckState(Qt::Checked);
+        }
+        else
+        {
+            list[i]->setCheckState(Qt::Unchecked);
+        }
+        list[i]->setSelected(false);
+    }
+    Q_EMIT itemChanged();
+}
+
+void QMultiComboBox::initStyleOption(QStyleOptionComboBox *option) const
+{
+    //Initializes the state, direction, rect, palette, and fontMetrics member variables based on the specified widget.
+    //This is a convenience function; the member variables can also be initialized manually.
+    option->initFrom(this);
+
+}
+
+void QMultiComboBox::clear()
+{
+	vlist_.clear();
+	QComboBox::clear();
+}
+

rtabmap/guilib/src/3rdParty/QMultiComboBox.h

@@ -0,0 +1,91 @@
+/****************************************************************************
+**
+** Copyright (C) 2010 Richard Steffen and/or its subsidiary(-ies).
+** All rights reserved.
+** Contact: rsteffen@messbild.de, rsteffen@uni-bonn.de
+**
+** Observe the License Information
+**
+****************************************************************************/
+
+#ifndef __MULTIBOXCOMBO_H__
+#define __MULTIBOXCOMBO_H__
+
+#include <iostream>
+
+#include <QComboBox>
+#include <QListWidget>
+#include <QVBoxLayout>
+#include <QStylePainter>
+
+class QMultiComboBox: public QComboBox
+{
+    Q_OBJECT
+public:
+
+    /// Constructor
+    QMultiComboBox(QWidget *widget = 0);
+
+    virtual ~QMultiComboBox();
+
+    /// the main display text
+    void SetDisplayText(QString text);
+
+    /// get the main display text
+    QString GetDisplayText() const;
+
+    /// add a item to the list
+    void addItem(const QString& text, const QVariant& userData = QVariant());
+
+    /// custom paint
+    virtual void paintEvent(QPaintEvent *e);
+
+    /// set the height of the popup
+    void setPopupHeight(int h);
+
+    /// replace standard QComboBox Popup
+    void showPopup();
+    void hidePopup();
+
+    /// replace neccessary data access
+    int count();
+    void setCurrentIndex(int index);
+    QString currentText();
+    QString itemText(int row);
+    QVariant itemData(int row);
+    void setItemChecked(int row, bool checked);
+
+Q_SIGNALS:
+    /// item changed
+    void itemChanged();
+
+public Q_SLOTS:
+
+    /// react on changes of the item checkbox
+    void scanItemSelect(QListWidgetItem* item);
+
+    /// the init style
+    void initStyleOption(QStyleOptionComboBox *option) const;
+
+    void clear();
+
+protected:
+
+    /// the height of the popup
+    int popheight_;
+
+    /// lower/upper screen bound
+    int screenbound_;
+
+    /// hold the main display text
+    QString m_DisplayText_;
+
+    /// popup frame
+    QFrame popframe_;
+
+    /// multi selection list in the popup frame
+    QListWidget vlist_;
+
+};
+
+#endif

rtabmap/guilib/src/3rdParty/lgpl-2.1.txt

@@ -0,0 +1,504 @@
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+		     END OF TERMS AND CONDITIONS
+
+           How to Apply These Terms to Your New Libraries
+
+  If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change.  You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+  To apply these terms, attach the following notices to the library.  It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the library's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+    This library is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the
+  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+  <signature of Ty Coon>, 1 April 1990
+  Ty Coon, President of Vice
+
+That's all there is to it!
+
+

rtabmap/guilib/src/AboutDialog.cpp

@@ -0,0 +1,307 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/AboutDialog.h"
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraStereo.h"
+#include "rtabmap/core/Optimizer.h"
+#include "ui_aboutDialog.h"
+#include <opencv2/core/version.hpp>
+#include <pcl/pcl_config.h>
+#include <vtkVersion.h>
+
+namespace rtabmap {
+
+AboutDialog::AboutDialog(QWidget * parent) :
+	QDialog(parent)
+{
+	_ui = new Ui_aboutDialog();
+	_ui->setupUi(this);
+	QString version = Parameters::getVersion().c_str();
+	QString cv_version = CV_VERSION;
+#if CV_MAJOR_VERSION < 3
+  #ifdef RTABMAP_NONFREE
+	_ui->label_opencv_license->setText("Not Commercial [With nonfree module]");
+  #else
+	_ui->label_opencv_license->setText("BSD [Without nonfree module]");
+  #endif
+#elif defined(HAVE_OPENCV_XFEATURES2D)
+  #ifdef RTABMAP_NONFREE
+	_ui->label_opencv_license->setText("Not Commercial [With xfeatures2d and nonfree modules]");
+  #else
+    #if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION<5)
+	  _ui->label_opencv_license->setText("BSD [With xfeatures2d module]");
+    #else
+	  _ui->label_opencv_license->setText("Apache 3 [With xfeatures2d module]");
+    #endif
+  #endif
+#else
+  #if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION<5)
+	_ui->label_opencv_license->setText("BSD [Without xfeatures2d and nonfree modules]");
+  #else
+	_ui->label_opencv_license->setText("Apache 3 [Without xfeatures2d and nonfree modules]");
+  #endif
+#endif
+	_ui->label_version->setText(version);
+	_ui->label_opencv_version->setText(cv_version);
+	_ui->label_pcl_version->setText(PCL_VERSION_PRETTY);
+	_ui->label_vtk_version->setText(vtkVersion::GetVTKVersion());
+	_ui->label_qt_version->setText(qVersion());
+#ifdef RTABMAP_ORB_OCTREE
+	_ui->label_orboctree->setText("Yes");
+	_ui->label_orboctree_license->setEnabled(true);
+#else
+	_ui->label_orboctree->setText("No");
+	_ui->label_orboctree_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_TORCH
+	_ui->label_sptorch->setText("Yes");
+	_ui->label_sptorch_license->setEnabled(true);
+#else
+	_ui->label_sptorch->setText("No");
+	_ui->label_sptorch_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_PYTHON
+	_ui->label_pymatcher->setText("Yes");
+	_ui->label_pymatcher_license->setEnabled(true);
+#else
+	_ui->label_pymatcher->setText("No");
+	_ui->label_pymatcher_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_FASTCV
+	_ui->label_fastcv->setText("Yes");
+	_ui->label_fastcv_license->setEnabled(true);
+#else
+	_ui->label_fastcv->setText("No");
+	_ui->label_fastcv_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_PDAL
+	_ui->label_pdal->setText("Yes");
+	_ui->label_pdal_license->setEnabled(true);
+#else
+	_ui->label_pdal->setText("No");
+	_ui->label_pdal_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_LIBLAS
+	_ui->label_liblas->setText("Yes");
+	_ui->label_liblas_license->setEnabled(true);
+#else
+	_ui->label_liblas->setText("No");
+	_ui->label_liblas_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_CUDASIFT
+	_ui->label_cudasift->setText("Yes");
+	_ui->label_cudasift_license->setEnabled(true);
+#else
+	_ui->label_cudasift->setText("No");
+	_ui->label_cudasift_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_OCTOMAP
+	_ui->label_octomap->setText("Yes");
+	_ui->label_octomap_license->setEnabled(true);
+#else
+	_ui->label_octomap->setText("No");
+	_ui->label_octomap_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_GRIDMAP
+	_ui->label_gridmap->setText("Yes");
+	_ui->label_gridmap_license->setEnabled(true);
+#else
+	_ui->label_gridmap->setText("No");
+	_ui->label_gridmap_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_CPUTSDF
+	_ui->label_cputsdf->setText("Yes");
+	_ui->label_cputsdf_license->setEnabled(true);
+#else
+	_ui->label_cputsdf->setText("No");
+	_ui->label_cputsdf_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_OPENCHISEL
+	_ui->label_openchisel->setText("Yes");
+#else
+	_ui->label_openchisel->setText("No");
+#endif
+#ifdef RTABMAP_ALICE_VISION
+	_ui->label_aliceVision->setText("Yes");
+	_ui->label_aliceVision_license->setEnabled(true);
+#else
+	_ui->label_aliceVision->setText("No");
+	_ui->label_aliceVision_license->setEnabled(false);
+#endif
+
+	_ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No");
+	_ui->label_freenect_license->setEnabled(CameraFreenect::available());
+	_ui->label_openni->setText(CameraOpenni::available()?"Yes":"No");
+	_ui->label_openni_license->setEnabled(CameraOpenni::available());
+	_ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No");
+	_ui->label_openni2_license->setEnabled(CameraOpenNI2::available());
+	_ui->label_freenect2->setText(CameraFreenect2::available()?"Yes":"No");
+	_ui->label_freenect2_license->setEnabled(CameraFreenect2::available());
+	_ui->label_realsense->setText(CameraRealSense::available() ? "Yes" : "No");
+	_ui->label_realsense_license->setEnabled(CameraRealSense::available());
+	_ui->label_realsense2->setText(CameraRealSense2::available() ? "Yes" : "No");
+	_ui->label_realsense2_license->setEnabled(CameraRealSense2::available());
+	_ui->label_dc1394->setText(CameraStereoDC1394::available()?"Yes":"No");
+	_ui->label_dc1394_license->setEnabled(CameraStereoDC1394::available());
+	_ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No");
+	_ui->label_zed->setText(CameraStereoZed::available()?"Yes":"No");
+	_ui->label_zedOC->setText(CameraStereoZedOC::available()?"Yes":"No");
+	_ui->label_zedOC_license->setEnabled(CameraStereoZedOC::available());
+	_ui->label_k4w2->setText(CameraK4W2::available() ? "Yes" : "No");
+	_ui->label_k4a->setText(CameraK4A::available() ? "Yes" : "No");
+	_ui->label_mynteye->setText(CameraMyntEye::available() ? "Yes" : "No");
+	_ui->label_depthai->setText(CameraDepthAI::available() ? "Yes" : "No");
+	_ui->label_depthai_license->setEnabled(CameraDepthAI::available());
+	_ui->label_xvsdk->setText(CameraSeerSense::available() ? "Yes" : "No");
+
+	_ui->label_toro->setText(Optimizer::isAvailable(Optimizer::kTypeTORO)?"Yes":"No");
+	_ui->label_toro_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeTORO)?true:false);
+	_ui->label_g2o->setText(Optimizer::isAvailable(Optimizer::kTypeG2O)?"Yes":"No");
+	_ui->label_g2o_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeG2O)?true:false);
+	_ui->label_gtsam->setText(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?"Yes":"No");
+	_ui->label_gtsam_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?true:false);
+	_ui->label_cvsba->setText(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?"Yes":"No");
+	_ui->label_cvsba_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeCVSBA)?true:false);
+	_ui->label_ceres->setText(Optimizer::isAvailable(Optimizer::kTypeCeres)?"Yes":"No");
+	_ui->label_ceres_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeCeres)?true:false);
+
+#ifdef RTABMAP_MRPT
+	_ui->label_mrpt->setText("Yes");
+	_ui->label_mrpt_license->setEnabled(true);
+#else
+	_ui->label_mrpt->setText("No");
+	_ui->label_mrpt_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_POINTMATCHER
+	_ui->label_libpointmatcher->setText("Yes");
+	_ui->label_libpointmatcher_license->setEnabled(true);
+#else
+	_ui->label_libpointmatcher->setText("No");
+	_ui->label_libpointmatcher_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_CCCORELIB
+	_ui->label_cccorelib->setText("Yes");
+	_ui->label_cccorelib_license->setEnabled(true);
+#else
+	_ui->label_cccorelib->setText("No");
+	_ui->label_cccorelib_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_OPEN3D
+	_ui->label_open3d->setText("Yes");
+	_ui->label_open3d_license->setEnabled(true);
+#else
+	_ui->label_open3d->setText("No");
+	_ui->label_open3d_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_FOVIS
+	_ui->label_fovis->setText("Yes");
+	_ui->label_fovis_license->setEnabled(true);
+#else
+	_ui->label_fovis->setText("No");
+	_ui->label_fovis_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_VISO2
+	_ui->label_viso2->setText("Yes");
+	_ui->label_viso2_license->setEnabled(true);
+#else
+	_ui->label_viso2->setText("No");
+	_ui->label_viso2_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_DVO
+	_ui->label_dvo->setText("Yes");
+	_ui->label_dvo_license->setEnabled(true);
+#else
+	_ui->label_dvo->setText("No");
+	_ui->label_dvo_license->setEnabled(false);
+#endif
+#ifdef RTABMAP_ORB_SLAM
+#if RTABMAP_ORB_SLAM == 3
+	_ui->label_orbslam_title->setText("With ORB SLAM3 :");
+#elif RTABMAP_ORB_SLAM == 2
+	_ui->label_orbslam_title->setText("With ORB SLAM2 :");
+#endif
+	_ui->label_orbslam->setText("Yes");
+	_ui->label_orbslam_license->setEnabled(true);
+#else
+	_ui->label_orbslam->setText("No");
+	_ui->label_orbslam_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_OKVIS
+	_ui->label_okvis->setText("Yes");
+	_ui->label_okvis_license->setEnabled(true);
+#else
+	_ui->label_okvis->setText("No");
+	_ui->label_okvis_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_LOAM
+	_ui->label_loam->setText("Yes");
+	_ui->label_loam_license->setEnabled(true);
+#else
+	_ui->label_loam->setText("No");
+	_ui->label_loam_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_MSCKF_VIO
+	_ui->label_msckf->setText("Yes");
+	_ui->label_msckf_license->setEnabled(true);
+#else
+	_ui->label_msckf->setText("No");
+	_ui->label_msckf_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_VINS
+	_ui->label_vins_fusion->setText("Yes");
+	_ui->label_vins_fusion_license->setEnabled(true);
+#else
+	_ui->label_vins_fusion->setText("No");
+	_ui->label_vins_fusion_license->setEnabled(false);
+#endif
+
+#ifdef RTABMAP_OPENVINS
+	_ui->label_openvins->setText("Yes");
+	_ui->label_openvins_license->setEnabled(true);
+#else
+	_ui->label_openvins->setText("No");
+	_ui->label_openvins_license->setEnabled(false);
+#endif
+
+}
+
+AboutDialog::~AboutDialog()
+{
+	delete _ui;
+}
+
+}

rtabmap/guilib/src/CMakeLists.txt

@@ -0,0 +1,261 @@
+
+
+### Qt Gui stuff ###
+SET(headers_ui 
+	../include/${PROJECT_PREFIX}/gui/MainWindow.h
+	../include/${PROJECT_PREFIX}/gui/PreferencesDialog.h
+	../include/${PROJECT_PREFIX}/gui/DatabaseViewer.h
+	../include/${PROJECT_PREFIX}/gui/AboutDialog.h
+	../include/${PROJECT_PREFIX}/gui/ConsoleWidget.h
+	../include/${PROJECT_PREFIX}/gui/ImageView.h
+	../include/${PROJECT_PREFIX}/gui/PdfPlot.h
+	../include/${PROJECT_PREFIX}/gui/StatsToolBox.h
+	../include/${PROJECT_PREFIX}/gui/ProgressDialog.h
+	../include/${PROJECT_PREFIX}/utilite/UPlot.h
+	../include/${PROJECT_PREFIX}/utilite/UImageView.h
+	../include/${PROJECT_PREFIX}/gui/CloudViewer.h
+	../include/${PROJECT_PREFIX}/gui/OdometryViewer.h
+	../include/${PROJECT_PREFIX}/gui/LoopClosureViewer.h
+	../include/${PROJECT_PREFIX}/gui/DataRecorder.h
+	../include/${PROJECT_PREFIX}/gui/CameraViewer.h
+	../include/${PROJECT_PREFIX}/gui/CalibrationDialog.h
+	../include/${PROJECT_PREFIX}/gui/ExportDialog.h
+	../include/${PROJECT_PREFIX}/gui/PostProcessingDialog.h
+	../include/${PROJECT_PREFIX}/gui/ExportCloudsDialog.h
+	../include/${PROJECT_PREFIX}/gui/ExportBundlerDialog.h
+	../include/${PROJECT_PREFIX}/gui/MapVisibilityWidget.h
+	../include/${PROJECT_PREFIX}/gui/GraphViewer.h
+	../include/${PROJECT_PREFIX}/gui/CreateSimpleCalibrationDialog.h
+	../include/${PROJECT_PREFIX}/gui/ParametersToolBox.h
+	../include/${PROJECT_PREFIX}/gui/DepthCalibrationDialog.h
+	../include/${PROJECT_PREFIX}/gui/EditConstraintDialog.h
+	./3rdParty/QMultiComboBox.h
+	../include/${PROJECT_PREFIX}/gui/TexturingState.h
+	../include/${PROJECT_PREFIX}/gui/RecoveryState.h
+	../include/${PROJECT_PREFIX}/gui/EditDepthArea.h
+	../include/${PROJECT_PREFIX}/gui/EditMapArea.h
+	../include/${PROJECT_PREFIX}/gui/MultiSessionLocWidget.h
+	../include/${PROJECT_PREFIX}/gui/MultiSessionLocSubView.h
+	../include/${PROJECT_PREFIX}/gui/LinkRefiningDialog.h
+)
+
+SET(qrc 
+    ./GuiLib.qrc
+)
+
+IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
+    set(CMAKE_AUTOUIC_SEARCH_PATHS ./ui/)
+ENDIF()
+
+
+SET(SRC_FILES 
+	./MainWindow.cpp
+	./PreferencesDialog.cpp
+	./KeypointItem.cpp
+	./ImageView.cpp
+	./PdfPlot.cpp
+	./StatsToolBox.cpp
+	./ProgressDialog.cpp
+	./AboutDialog.cpp
+	./ConsoleWidget.cpp
+	./DatabaseViewer.cpp
+	./utilite/UPlot.cpp
+	./CloudViewer.cpp
+	./CloudViewerCellPicker.cpp
+	./CloudViewerInteractorStyle.cpp
+	./OdometryViewer.cpp
+	./LoopClosureViewer.cpp
+	./DataRecorder.cpp
+	./CameraViewer.cpp
+	./CalibrationDialog.cpp
+	./ExportDialog.cpp
+	./ExportBundlerDialog.cpp
+	./PostProcessingDialog.cpp
+	./ExportCloudsDialog.cpp
+	./EditConstraintDialog.cpp
+	./MapVisibilityWidget.cpp
+	./GraphViewer.cpp
+	./EditDepthArea.cpp
+	./EditMapArea.cpp
+	./MultiSessionLocWidget.cpp
+	./MultiSessionLocSubView.cpp
+	./CreateSimpleCalibrationDialog.cpp
+	./ParametersToolBox.cpp
+	./DepthCalibrationDialog.cpp
+	./LinkRefiningDialog.cpp
+	./3rdParty/QMultiComboBox.cpp
+	./opencv/vtkImageMatSource.cpp
+	${qrc}
+	${headers_ui}
+)
+
+# to get includes in visual studio
+IF(MSVC)
+    FILE(GLOB HEADERS
+        ../include/${PROJECT_PREFIX}/gui/*.h
+    )
+    SET(SRC_FILES ${SRC_FILES} ${HEADERS})
+ENDIF(MSVC)
+
+SET(INCLUDE_DIRS
+	${CMAKE_CURRENT_SOURCE_DIR}/../include
+	${CMAKE_CURRENT_SOURCE_DIR}
+	${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir
+)
+
+IF(QT4_FOUND)
+    INCLUDE(${QT_USE_FILE})
+ENDIF(QT4_FOUND)
+
+SET(LIBRARIES "")
+SET(PUBLIC_LIBRARIES "")
+SET(PUBLIC_INCLUDE_DIRS "")
+
+IF(Qt6_FOUND)
+    SET(PUBLIC_LIBRARIES ${PUBLIC_LIBRARIES} Qt6::Widgets Qt6::Core Qt6::Gui Qt6::OpenGL)
+    SET(LIBRARIES ${LIBRARIES} Qt6::PrintSupport)
+    IF(Qt6Svg_FOUND)
+        SET(LIBRARIES ${LIBRARIES} Qt6::Svg)
+    ENDIF()
+    SET(PUBLIC_INCLUDE_DIRS
+        ${PUBLIC_INCLUDE_DIRS}
+        ${Qt6Widgets_INCLUDE_DIRS}
+        ${Qt6Core_INCLUDE_DIRS}
+        ${Qt6Gui_INCLUDE_DIRS}
+        ${Qt6OpenGL_INCLUDE_DIRS})
+ELSEIF(Qt5_FOUND)
+    SET(PUBLIC_LIBRARIES ${PUBLIC_LIBRARIES} Qt5::Widgets Qt5::Core Qt5::Gui Qt5::OpenGL)
+    SET(LIBRARIES ${LIBRARIES} Qt5::PrintSupport)
+    IF(Qt5Svg_FOUND)
+        SET(LIBRARIES ${LIBRARIES} Qt5::Svg)
+    ENDIF()
+    SET(PUBLIC_INCLUDE_DIRS
+        ${PUBLIC_INCLUDE_DIRS}
+        ${Qt5Widgets_INCLUDE_DIRS}
+        ${Qt5Core_INCLUDE_DIRS}
+        ${Qt5Gui_INCLUDE_DIRS}
+        ${Qt5OpenGL_INCLUDE_DIRS})
+ELSE()
+    SET(PUBLIC_LIBRARIES ${PUBLIC_LIBRARIES} ${QTCORE_LIBRARY} ${QTGUI_LIBRARY})
+    IF(QTSVG_FOUND)
+        SET(LIBRARIES ${LIBRARIES} ${QTSVG_LIBRARY})
+    ENDIF()
+    SET(PUBLIC_INCLUDE_DIRS
+        ${QTCORE_INCLUDE_DIRS}
+        ${QTGUI_INCLUDE_DIRS})
+ENDIF()
+
+IF(CPUTSDF_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${CPUTSDF_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${CPUTSDF_LIBRARIES}
+	)
+ENDIF(CPUTSDF_FOUND)
+
+IF(open_chisel_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${open_chisel_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${open_chisel_LIBRARIES}
+	)
+ENDIF(open_chisel_FOUND)
+
+IF(VTK_USE_QVTK)
+	SET(INCLUDE_DIRS ${INCLUDE_DIRS} ${QVTK_INCLUDE_DIR})
+	SET(LIBRARIES ${LIBRARIES} ${QVTK_LIBRARY})
+ENDIF(VTK_USE_QVTK)
+
+#include files
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+add_definitions(${PCL_DEFINITIONS})
+
+# Include presets
+SET(RESOURCES
+	${PROJECT_SOURCE_DIR}/data/presets/camera_tof_icp.ini
+	${PROJECT_SOURCE_DIR}/data/presets/lidar3d_icp.ini
+)
+
+foreach(arg ${RESOURCES})
+   get_filename_component(filename ${arg} NAME)
+   string(REPLACE "." "_" output ${filename})
+   set(RESOURCES_HEADERS "${RESOURCES_HEADERS}" "${CMAKE_CURRENT_BINARY_DIR}/${output}.h")
+   set_property(SOURCE "${CMAKE_CURRENT_BINARY_DIR}/${output}.h" PROPERTY SKIP_AUTOGEN ON)
+endforeach(arg ${RESOURCES})
+
+ADD_CUSTOM_COMMAND(
+	OUTPUT ${RESOURCES_HEADERS}
+	COMMAND res_tool -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
+	COMMENT "[Creating resources]"
+	DEPENDS ${RESOURCES}
+)
+
+# create a library from the source files
+ADD_LIBRARY(rtabmap_gui ${SRC_FILES} ${RESOURCES_HEADERS})
+ADD_LIBRARY(rtabmap::gui ALIAS rtabmap_gui)
+
+generate_export_header(rtabmap_gui
+ DEPRECATED_MACRO_NAME RTABMAP_DEPRECATED)
+
+target_include_directories(rtabmap_gui PUBLIC 
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR}/include;${PUBLIC_INCLUDE_DIRS}>"
+  "$<INSTALL_INTERFACE:${INSTALL_INCLUDE_DIR};${PUBLIC_INCLUDE_DIRS}>")
+
+TARGET_LINK_LIBRARIES(rtabmap_gui 
+  PUBLIC
+    rtabmap_core ${PUBLIC_LIBRARIES}
+  PRIVATE
+    ${LIBRARIES}
+)
+
+SET_TARGET_PROPERTIES(
+     rtabmap_gui
+   PROPERTIES
+     VERSION ${RTABMAP_VERSION}
+     SOVERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}
+     EXPORT_NAME "gui"
+	 AUTOUIC ON
+     AUTOMOC ON
+     AUTORCC ON
+)
+
+INSTALL(TARGETS rtabmap_gui EXPORT rtabmap_guiTargets
+        RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+        LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel
+        ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel)
+
+configure_file(
+  ${CMAKE_CURRENT_BINARY_DIR}/rtabmap_gui_export.h
+  ${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_PREFIX}/gui/rtabmap_gui_export.h
+  COPYONLY)
+  
+install(
+  DIRECTORY
+    ${CMAKE_CURRENT_SOURCE_DIR}/../include/${PROJECT_PREFIX}
+    ${CMAKE_CURRENT_BINARY_DIR}/include/${PROJECT_PREFIX}
+  DESTINATION
+    ${INSTALL_INCLUDE_DIR}
+  COMPONENT
+    devel
+  FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+)
+export(EXPORT rtabmap_guiTargets
+  FILE "${CMAKE_CURRENT_BINARY_DIR}/../../${PROJECT_NAME}_guiTargets.cmake"
+  NAMESPACE rtabmap::
+)
+install(EXPORT rtabmap_guiTargets
+  FILE
+    ${PROJECT_NAME}_guiTargets.cmake
+  DESTINATION
+    ${INSTALL_CMAKE_DIR}
+  NAMESPACE rtabmap::
+  COMPONENT
+    devel
+)

rtabmap/guilib/src/CameraViewer.cpp

@@ -0,0 +1,324 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/SensorEvent.h>
+#include "rtabmap/gui/CameraViewer.h"
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/MarkerDetector.h>
+#include <rtabmap/gui/ImageView.h>
+#include <rtabmap/gui/CloudViewer.h>
+#include <rtabmap/utilite/UCv2Qt.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <QtCore/QMetaType>
+#include <QHBoxLayout>
+#include <QVBoxLayout>
+#include <QLabel>
+#include <QSpinBox>
+#include <QDialogButtonBox>
+#include <QCheckBox>
+#include <QPushButton>
+
+namespace rtabmap {
+
+
+CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
+		QDialog(parent),
+	imageView_(new ImageView(this)),
+	cloudView_(new CloudViewer(this)),
+	processingImages_(false),
+	parameters_(parameters),
+	markerDetector_(0)
+{
+	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
+
+	imageView_->setImageDepthShown(true);
+	imageView_->setMinimumSize(320, 240);
+	imageView_->setVisible(false);
+	QHBoxLayout * layout = new QHBoxLayout();
+	layout->setContentsMargins(0,0,0,0);
+	layout->addWidget(imageView_,1);
+	layout->addWidget(cloudView_,1);
+
+	QLabel * decimationLabel = new QLabel("Decimation", this);
+	decimationSpin_ = new QSpinBox(this);
+	decimationSpin_->setMinimum(-16);
+	decimationSpin_->setMaximum(16);
+	decimationSpin_->setValue(2);
+
+	pause_ = new QPushButton("Pause", this);
+	pause_->setCheckable(true);
+	showCloudCheckbox_ = new QCheckBox("Show RGB-D cloud", this);
+	showCloudCheckbox_->setEnabled(false);
+	showCloudCheckbox_->setChecked(true);
+	showScanCheckbox_ = new QCheckBox("Show scan", this);
+	showScanCheckbox_->setEnabled(false);
+	showScanCheckbox_->setChecked(true);
+
+	markerCheckbox_ = new QCheckBox("Detect markers", this);
+#ifdef HAVE_OPENCV_ARUCO
+	markerCheckbox_->setEnabled(true);
+	markerDetector_ = new MarkerDetector(parameters);
+#else
+	markerCheckbox_->setEnabled(false);
+	markerCheckbox_->setToolTip("Disabled: RTAB-Map is not built with OpenCV's aruco module.");
+#endif
+	markerCheckbox_->setChecked(false);
+
+	imageSizeLabel_ = new QLabel(this);
+
+	QDialogButtonBox * buttonBox = new QDialogButtonBox(this);
+	buttonBox->setStandardButtons(QDialogButtonBox::Close);
+	connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
+
+	QHBoxLayout * layout2 = new QHBoxLayout();
+	layout2->addWidget(pause_);
+	layout2->addWidget(decimationLabel);
+	layout2->addWidget(decimationSpin_);
+	layout2->addWidget(showCloudCheckbox_);
+	layout2->addWidget(showScanCheckbox_);
+	layout2->addWidget(markerCheckbox_);
+	layout2->addWidget(imageSizeLabel_);
+	layout2->addStretch(1);
+	layout2->addWidget(buttonBox);
+
+	QVBoxLayout * vlayout = new QVBoxLayout(this);
+	vlayout->setContentsMargins(0,0,0,0);
+	vlayout->setSpacing(0);
+	vlayout->addLayout(layout, 1);
+	vlayout->addLayout(layout2);
+
+	this->setLayout(vlayout);
+}
+
+CameraViewer::~CameraViewer()
+{
+	this->unregisterFromEventsManager();
+	delete markerDetector_;
+}
+
+void CameraViewer::setDecimation(int value)
+{
+	decimationSpin_->setValue(value);
+}
+
+void CameraViewer::showImage(const rtabmap::SensorData & data)
+{
+	processingImages_ = true;
+	QString sizes;
+
+	cv::Mat left;
+	cv::Mat depthOrRight;
+	LaserScan scan;
+	if( !data.imageRaw().empty() || !data.imageCompressed().empty() ||
+		!data.depthOrRightRaw().empty() || !data.depthOrRightCompressed().empty() ||
+		!data.laserScanRaw().empty() || !data.laserScanCompressed().empty())
+	{
+		data.uncompressDataConst(
+			!data.imageRaw().empty() || !data.imageCompressed().empty()?&left:0, 
+			!data.depthOrRightRaw().empty() || !data.depthOrRightCompressed().empty()?&depthOrRight:0,
+			!data.laserScanRaw().empty() || !data.laserScanCompressed().empty()?&scan:0);
+	}
+
+	imageView_->setVisible(!left.empty() || !left.empty());
+	std::map<int, MarkerInfo> detections;
+	if(!left.empty())
+	{
+		std::vector<CameraModel> models;
+		if(markerCheckbox_->isEnabled() && markerCheckbox_->isChecked())
+		{
+			models = data.cameraModels();
+			if(models.empty())
+			{
+				for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
+				{
+					models.push_back(data.stereoCameraModels()[i].left());
+				}
+			}
+		}
+			
+		if(!models.empty() && models[0].isValidForProjection())
+		{
+			cv::Mat imageWithDetections;
+			detections = markerDetector_->detect(left, models, depthOrRight, std::map<int, float>(), &imageWithDetections);
+			imageView_->setImage(uCvMat2QImage(imageWithDetections));
+		}
+		else
+		{
+			imageView_->setImage(uCvMat2QImage(left));
+		}
+		sizes.append(QString("Color=%1x%2").arg(left.cols).arg(left.rows));
+	}
+	if(!depthOrRight.empty())
+	{
+		imageView_->setImageDepth(depthOrRight);
+		sizes.append(QString(" Depth=%1x%2").arg(depthOrRight.cols).arg(depthOrRight.rows));
+	}
+	imageSizeLabel_->setText(sizes);
+
+	if(!depthOrRight.empty() &&
+	   ((data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()) || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())))
+	{
+		if(showCloudCheckbox_->isChecked())
+		{
+			if(!left.empty() && !depthOrRight.empty())
+			{
+				showCloudCheckbox_->setEnabled(true);
+				if(data.imageRaw().empty())
+				{
+					if(!data.stereoCameraModels().empty())
+					{
+						cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(SensorData(left, depthOrRight, data.stereoCameraModels()), decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
+					}
+					else
+					{
+						cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(SensorData(left, depthOrRight, data.cameraModels()), decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
+					}
+					
+				}
+				else
+				{
+					cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
+				}
+			}
+			else if(!depthOrRight.empty())
+			{
+				showCloudCheckbox_->setEnabled(true);
+				if(data.depthOrRightRaw().empty())
+				{
+					cloudView_->addCloud("cloud", util3d::cloudFromSensorData(SensorData(cv::Mat(), depthOrRight, data.cameraModels()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
+				}
+				else
+				{
+					cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
+				}
+			}
+
+			// Add landmarks to 3D Map view
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+			cloudView_->removeAllCoordinates("landmark_");
+#endif
+			cloudView_->removeAllTexts();
+			if(!detections.empty())
+			{
+				for(std::map<int, MarkerInfo>::const_iterator iter=detections.begin(); iter!=detections.end(); ++iter)
+				{
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+					cloudView_->addOrUpdateCoordinate(uFormat("landmark_%d", iter->first), iter->second.pose(), iter->second.length(), false);
+#endif
+					std::string num = uNumber2Str(iter->first);
+					cloudView_->addOrUpdateText(
+							std::string("landmark_str_") + num,
+							num,
+							iter->second.pose(),
+							0.05,
+							Qt::yellow);
+				}
+			}
+		}
+	}
+
+	if(!scan.isEmpty())
+	{
+		showScanCheckbox_->setEnabled(true);
+		if(showScanCheckbox_->isChecked())
+		{
+			if(scan.hasNormals())
+			{
+				if(scan.hasIntensity())
+				{
+					cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudINormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+				}
+				else if(scan.hasRGB())
+				{
+					cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGBNormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+				}
+				else
+				{
+					cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudNormal(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+				}
+			}
+			else if(scan.hasIntensity())
+			{
+				cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudI(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+			}
+			else if(scan.hasRGB())
+			{
+				cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGB(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+			}
+			else
+			{
+				cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(scan), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), scan.localTransform(), Qt::yellow);
+			}
+		}
+	}
+
+	cloudView_->setVisible((showCloudCheckbox_->isEnabled() && showCloudCheckbox_->isChecked()) ||
+						   (showScanCheckbox_->isEnabled() && showScanCheckbox_->isChecked()));
+	if(cloudView_->isVisible())
+	{
+		cloudView_->refreshView();
+	}
+	if(cloudView_->getAddedClouds().contains("cloud"))
+	{
+		cloudView_->setCloudVisibility("cloud", showCloudCheckbox_->isChecked());
+	}
+	if(cloudView_->getAddedClouds().contains("scan"))
+	{
+		cloudView_->setCloudVisibility("scan", showScanCheckbox_->isChecked());
+	}
+
+	processingImages_ = false;
+}
+
+bool CameraViewer::handleEvent(UEvent * event)
+{
+	if(!pause_->isChecked())
+	{
+		if(event->getClassName().compare("SensorEvent") == 0)
+		{
+			SensorEvent * camEvent = (SensorEvent*)event;
+			if(camEvent->getCode() == SensorEvent::kCodeData)
+			{
+				if(camEvent->data().isValid())
+				{
+					if(!processingImages_ && this->isVisible() && camEvent->data().isValid())
+					{
+						processingImages_ = true;
+						QMetaObject::invokeMethod(this, "showImage",
+								Q_ARG(rtabmap::SensorData, camEvent->data()));
+					}
+				}
+			}
+		}
+	}
+	return false;
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/CloudViewerCellPicker.cpp

@@ -0,0 +1,387 @@
+/*
+ * CloudViewerCellPicker.cpp
+ *
+ *  Created on: Aug 21, 2018
+ *      Author: mathieu
+ */
+
+#include "rtabmap/gui/CloudViewerCellPicker.h"
+
+#include <vtkImageData.h>
+#include <vtkRenderer.h>
+#include <vtkAbstractPicker.h>
+#include <vtkPicker.h>
+#include <vtkAbstractCellLocator.h>
+#include <vtkIdList.h>
+#include <vtkCellPicker.h>
+#include <vtkLODProp3D.h>
+#include <vtkMapper.h>
+#include <vtkGenericCell.h>
+#include <vtkMath.h>
+#include <vtkTexture.h>
+#include <vtkObjectFactory.h>
+#include <vtkSmartPointer.h>
+#include <vtkPoints.h>
+#include <vtkProperty.h>
+
+namespace rtabmap {
+
+// Standard VTK macro for *New ()
+vtkStandardNewMacro (CloudViewerCellPicker);
+
+CloudViewerCellPicker::CloudViewerCellPicker()
+{
+	cell_ = vtkGenericCell::New();
+	pointIds_ = vtkIdList::New();
+}
+
+CloudViewerCellPicker::~CloudViewerCellPicker()
+{
+	cell_->Delete();
+	pointIds_->Delete();
+}
+
+double CloudViewerCellPicker::IntersectActorWithLine(const double p1[3],
+		const double p2[3],
+		double t1, double t2,
+		double tol,
+		vtkProp3D *prop,
+		vtkMapper *mapper)
+{
+	// This code was taken from the original CellPicker with almost no
+	// modification except for the locator and texture additions.
+
+	// Intersect each cell with ray.  Keep track of one closest to
+	// the eye (within the tolerance tol) and within the clipping range).
+	// Note that we fudge the "closest to" (tMin+this->Tolerance) a little and
+	// keep track of the cell with the best pick based on parametric
+	// coordinate (pick the minimum, maximum parametric distance). This
+	// breaks ties in a reasonable way when cells are the same distance
+	// from the eye (like cells laying on a 2D plane).
+
+	vtkDataSet *data = mapper->GetInput();
+	double tMin = VTK_DOUBLE_MAX;
+	double minPCoords[3];
+	double pDistMin = VTK_DOUBLE_MAX;
+	vtkIdType minCellId = -1;
+	int minSubId = -1;
+	double minXYZ[3];
+	minXYZ[0] = minXYZ[1] = minXYZ[2] = 0.0;
+	double ray[3] = {p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]};
+	vtkMath::Normalize(ray);
+	vtkActor * actor = vtkActor::SafeDownCast(prop);
+
+	// Polydata has no 3D cells
+	int isPolyData = data->IsA("vtkPolyData");
+
+	vtkCollectionSimpleIterator iter;
+	vtkAbstractCellLocator *locator = 0;
+	this->Locators->InitTraversal(iter);
+	while ( (locator = static_cast<vtkAbstractCellLocator *>(
+			this->Locators->GetNextItemAsObject(iter))) )
+	{
+		if (locator->GetDataSet() == data)
+		{
+			break;
+		}
+	}
+
+	// Make a new p1 and p2 using the clipped t1 and t2
+	double q1[3], q2[3];
+	q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
+	q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
+	if (t1 != 0.0 || t2 != 1.0)
+	{
+		for (int j = 0; j < 3; j++)
+		{
+			q1[j] = p1[j]*(1.0 - t1) + p2[j]*t1;
+			q2[j] = p1[j]*(1.0 - t2) + p2[j]*t2;
+		}
+	}
+
+	// Use the locator if one exists for this data
+	if (locator)
+	{
+		vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
+		vtkSmartPointer<vtkIdList> intersectCells = vtkSmartPointer<vtkIdList>::New();
+
+		locator->IntersectWithLine(q1, q2, intersectPoints, intersectCells);
+		for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
+		{
+			double intersection[3];
+			intersectPoints->GetPoint(i, intersection);
+		}
+
+		if (!locator->IntersectWithLine(q1, q2, tol, tMin, minXYZ,
+				minPCoords, minSubId, minCellId,
+				this->cell_))
+		{
+			return VTK_DOUBLE_MAX;
+		}
+
+		// Stretch tMin out to the original range
+		if (t1 != 0.0 || t2 != 1.0)
+		{
+			tMin = t1*(1.0 - tMin) + t2*tMin;
+		}
+
+		// If cell is a strip, then replace cell with a sub-cell
+		this->SubCellFromCell(this->cell_, minSubId);
+	}
+	else
+	{
+		vtkIdList *pointIds = this->pointIds_;
+		vtkIdType numCells = data->GetNumberOfCells();
+
+		for (vtkIdType cellId = 0; cellId < numCells; cellId++)
+		{
+			double t;
+			double x[3];
+			double pcoords[3];
+			pcoords[0] = pcoords[1] = pcoords[2] = 0;
+			int newSubId = -1;
+			int numSubIds = 1;
+
+			// If it is a strip, we need to iterate over the subIds
+			int cellType = data->GetCellType(cellId);
+			int useSubCells = this->HasSubCells(cellType);
+			if (useSubCells)
+			{
+				// Get the pointIds for the strip and the length of the strip
+				data->GetCellPoints(cellId, pointIds);
+				numSubIds = this->GetNumberOfSubCells(pointIds, cellType);
+			}
+
+			// This will only loop once unless we need to deal with a strip
+			for (int subId = 0; subId < numSubIds; subId++)
+			{
+				if (useSubCells)
+				{
+					// Get a sub-cell from a the strip
+					this->GetSubCell(data, pointIds, subId, cellType, this->cell_);
+				}
+				else
+				{
+					data->GetCell(cellId, this->cell_);
+				}
+
+				int cellPicked = 0;
+				if (isPolyData)
+				{
+					// Polydata can always be picked with original endpoints
+					cellPicked = this->cell_->IntersectWithLine(
+							const_cast<double *>(p1), const_cast<double *>(p2),
+							tol, t, x, pcoords, newSubId);
+				}
+				else
+				{
+					// Any 3D cells need to be intersected with a line segment that
+					// has been clipped with the clipping planes, in case one end is
+					// actually inside the cell.
+					cellPicked = this->cell_->IntersectWithLine(
+							q1, q2, tol, t, x, pcoords, newSubId);
+
+					// Stretch t out to the original range
+					if (t1 != 0.0 || t2 != 1.0)
+					{
+						t = t1*(1.0 - t) + t2*t;
+					}
+				}
+
+				if (cellPicked && t <= (tMin + this->Tolerance) && t >= t1 && t <= t2)
+				{
+					double pDist = this->cell_->GetParametricDistance(pcoords);
+					if (pDist < pDistMin || (pDist == pDistMin && t < tMin))
+					{
+////////////////////////////////////////////////////////////////////////////////////
+// BEGIN: Modifications from VTK 6.2
+////////////////////////////////////////////////////////////////////////////////////
+						bool visible = true;
+						if(actor->GetProperty()->GetBackfaceCulling() ||
+						   actor->GetProperty()->GetFrontfaceCulling())
+						{
+							// Get the cell weights
+							vtkIdType numPoints = this->cell_->GetNumberOfPoints();
+							double *weights = new double[numPoints];
+							for (vtkIdType i = 0; i < numPoints; i++)
+							{
+								weights[i] = 0;
+							}
+
+							// Get the interpolation weights (point is thrown away)
+							double point[3] = {0.0,0.0,0.0};
+							this->cell_->EvaluateLocation(minSubId, minPCoords, point, weights);
+
+							double normal[3] = {0.0,0.0,0.0};
+
+							if (this->ComputeSurfaceNormal(data, this->cell_, weights, normal))
+							{
+								if(actor->GetProperty()->GetBackfaceCulling())
+								{
+									visible = ray[0]*normal[0] + ray[1]*normal[1] + ray[2]*normal[2] <= 0;
+								}
+								else
+								{
+									visible = ray[0]*normal[0] + ray[1]*normal[1] + ray[2]*normal[2] >= 0;
+								}
+							}
+							delete [] weights;
+						}
+						if(visible)
+						{
+							tMin = t;
+							pDistMin = pDist;
+							// save all of these
+							minCellId = cellId;
+							minSubId = newSubId;
+							if (useSubCells)
+							{
+								minSubId = subId;
+							}
+							for (int k = 0; k < 3; k++)
+							{
+								minXYZ[k] = x[k];
+								minPCoords[k] = pcoords[k];
+							}
+						}
+////////////////////////////////////////////////////////////////////////////////////
+// END: Modifications from VTK 6.2
+////////////////////////////////////////////////////////////////////////////////////
+					} // for all subIds
+				} // if minimum, maximum
+			} // if a close cell
+		} // for all cells
+	}
+
+	// Do this if a cell was intersected
+	if (minCellId >= 0 && tMin < this->GlobalTMin)
+	{
+		this->ResetPickInfo();
+
+		// Get the cell, convert to triangle if it is a strip
+		vtkGenericCell *cell = this->cell_;
+
+		// If we used a locator, we already have the picked cell
+		if (!locator)
+		{
+			int cellType = data->GetCellType(minCellId);
+
+			if (this->HasSubCells(cellType))
+			{
+				data->GetCellPoints(minCellId, this->pointIds_);
+				this->GetSubCell(data, this->pointIds_, minSubId, cellType, cell);
+			}
+			else
+			{
+				data->GetCell(minCellId, cell);
+			}
+		}
+
+		// Get the cell weights
+		vtkIdType numPoints = cell->GetNumberOfPoints();
+		double *weights = new double[numPoints];
+		for (vtkIdType i = 0; i < numPoints; i++)
+		{
+			weights[i] = 0;
+		}
+
+		// Get the interpolation weights (point is thrown away)
+		double point[3];
+		cell->EvaluateLocation(minSubId, minPCoords, point, weights);
+
+		this->Mapper = mapper;
+
+		// Get the texture from the actor or the LOD
+		vtkActor *actor = 0;
+		vtkLODProp3D *lodActor = 0;
+		if ( (actor = vtkActor::SafeDownCast(prop)) )
+		{
+			this->Texture = actor->GetTexture();
+		}
+		else if ( (lodActor = vtkLODProp3D::SafeDownCast(prop)) )
+		{
+			int lodId = lodActor->GetPickLODID();
+			lodActor->GetLODTexture(lodId, &this->Texture);
+		}
+
+		if (this->PickTextureData && this->Texture)
+		{
+			// Return the texture's image data to the user
+			vtkImageData *image = this->Texture->GetInput();
+			this->DataSet = image;
+
+			// Get and check the image dimensions
+			int extent[6];
+			image->GetExtent(extent);
+			int dimensionsAreValid = 1;
+			int dimensions[3];
+			for (int i = 0; i < 3; i++)
+			{
+				dimensions[i] = extent[2*i + 1] - extent[2*i] + 1;
+				dimensionsAreValid = (dimensionsAreValid && dimensions[i] > 0);
+			}
+
+			// Use the texture coord to set the information
+			double tcoord[3];
+			if (dimensionsAreValid &&
+					this->ComputeSurfaceTCoord(data, cell, weights, tcoord))
+			{
+				// Take the border into account when computing coordinates
+				double x[3];
+				x[0] = extent[0] + tcoord[0]*dimensions[0] - 0.5;
+				x[1] = extent[2] + tcoord[1]*dimensions[1] - 0.5;
+				x[2] = extent[4] + tcoord[2]*dimensions[2] - 0.5;
+
+				this->SetImageDataPickInfo(x, extent);
+			}
+		}
+		else
+		{
+			// Return the polydata to the user
+			this->DataSet = data;
+			this->CellId = minCellId;
+			this->SubId = minSubId;
+			this->PCoords[0] = minPCoords[0];
+			this->PCoords[1] = minPCoords[1];
+			this->PCoords[2] = minPCoords[2];
+
+			// Find the point with the maximum weight
+			double maxWeight = 0;
+			vtkIdType iMaxWeight = -1;
+			for (vtkIdType i = 0; i < numPoints; i++)
+			{
+				if (weights[i] > maxWeight)
+				{
+					iMaxWeight = i;
+				}
+			}
+
+			// If maximum weight is found, use it to get the PointId
+			if (iMaxWeight != -1)
+			{
+				this->PointId = cell->PointIds->GetId(iMaxWeight);
+			}
+		}
+
+		// Set the mapper position
+		this->MapperPosition[0] = minXYZ[0];
+		this->MapperPosition[1] = minXYZ[1];
+		this->MapperPosition[2] = minXYZ[2];
+
+		// Compute the normal
+		if (!this->ComputeSurfaceNormal(data, cell, weights, this->MapperNormal))
+		{
+			// By default, the normal points back along view ray
+			this->MapperNormal[0] = p1[0] - p2[0];
+			this->MapperNormal[1] = p1[1] - p2[1];
+			this->MapperNormal[2] = p1[2] - p2[2];
+			vtkMath::Normalize(this->MapperNormal);
+		}
+
+		delete [] weights;
+	}
+
+	return tMin;
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/CloudViewerInteractorStyle.cpp

@@ -0,0 +1,341 @@
+/*
+ * CloudViewerInteractorStyl.cpp
+ *
+ *  Created on: Aug 21, 2018
+ *      Author: mathieu
+ */
+
+#include "rtabmap/gui/CloudViewerInteractorStyle.h"
+#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/gui/CloudViewerCellPicker.h"
+
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UMath.h"
+
+#include <vtkRenderer.h>
+#include <vtkRenderWindow.h>
+#include <vtkObjectFactory.h>
+#include <vtkOBBTree.h>
+#include <vtkCamera.h>
+
+namespace rtabmap {
+
+// Standard VTK macro for *New ()
+vtkStandardNewMacro (CloudViewerInteractorStyle);
+
+CloudViewerInteractorStyle::CloudViewerInteractorStyle() :
+	pcl::visualization::PCLVisualizerInteractorStyle(),
+	viewer_(0),
+	NumberOfClicks(0),
+	ResetPixelDistance(0),
+	pointsHolder_(new pcl::PointCloud<pcl::PointXYZRGB>),
+	orthoMode_(false)
+{
+	PreviousPosition[0] = PreviousPosition[1] = 0;
+	PreviousMeasure[0] = PreviousMeasure[1] =  PreviousMeasure[2] = 0.0f;
+
+	this->MotionFactor = 5;
+}
+
+void CloudViewerInteractorStyle::Rotate()
+{
+	if (this->CurrentRenderer == NULL)
+	{
+		return;
+	}
+
+	vtkRenderWindowInteractor *rwi = this->Interactor;
+
+	int dx = rwi->GetEventPosition()[0] - rwi->GetLastEventPosition()[0];
+	int dy = orthoMode_?0:rwi->GetEventPosition()[1] - rwi->GetLastEventPosition()[1];
+
+	int *size = this->CurrentRenderer->GetRenderWindow()->GetSize();
+
+	double delta_elevation = -20.0 / size[1];
+	double delta_azimuth = -20.0 / size[0];
+
+	double rxf = dx * delta_azimuth * this->MotionFactor;
+	double ryf = dy * delta_elevation * this->MotionFactor;
+
+	vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
+	UASSERT(camera);
+	if(!orthoMode_)
+	{
+		camera->Azimuth(rxf);
+		camera->Elevation(ryf);
+		camera->OrthogonalizeViewUp();
+	}
+	else
+	{
+		camera->Roll(-rxf);
+	}
+
+	if (this->AutoAdjustCameraClippingRange)
+	{
+		this->CurrentRenderer->ResetCameraClippingRange();
+	}
+
+	if (rwi->GetLightFollowCamera())
+	{
+		this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
+	}
+
+	//rwi->Render();
+}
+
+void CloudViewerInteractorStyle::setOrthoMode(bool enabled)
+{
+	if (this->CurrentRenderer == NULL)
+	{
+		return;
+	}
+
+	vtkCamera *camera = CurrentRenderer->GetActiveCamera ();
+	UASSERT(camera);
+	camera->SetParallelProjection (enabled);
+	if(enabled)
+	{
+		double x,y,z;
+		camera->GetFocalPoint(x, y, z);
+		camera->SetPosition(x, y, z+(camera->GetDistance()<=5?5:camera->GetDistance()));
+		camera->SetViewUp(1, 0, 0);
+	}
+	CurrentRenderer->SetActiveCamera (camera);
+	orthoMode_ = enabled;
+}
+
+void CloudViewerInteractorStyle::OnMouseMove()
+{
+	if(this->CurrentRenderer &&
+		this->CurrentRenderer->GetLayer() == 1 &&
+		this->GetInteractor()->GetShiftKey() && this->GetInteractor()->GetControlKey() &&
+		viewer_ &&
+		viewer_->getLocators().size())
+	{
+		CloudViewerCellPicker * cellPicker = dynamic_cast<CloudViewerCellPicker*>(this->Interactor->GetPicker());
+		if(cellPicker)
+		{
+			int pickPosition[2];
+			this->GetInteractor()->GetEventPosition(pickPosition);
+			int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+					0,  // always zero.
+					this->CurrentRenderer);
+			if(result)
+			{
+				double picked[3];
+				this->Interactor->GetPicker()->GetPickPosition(picked);
+
+				UDEBUG("Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+
+				float textSize = 0.05;
+
+				viewer_->removeCloud("interactor_points_alt");
+				pointsHolder_->resize(2);
+				pcl::PointXYZRGB pt;
+				pt.r = 255;
+				pt.x = picked[0];
+				pt.y = picked[1];
+				pt.z = picked[2];
+				pointsHolder_->at(0) = pt;
+
+				viewer_->removeLine("interactor_ray_alt");
+				viewer_->removeText("interactor_ray_text_alt");
+
+				// Intersect the locator with the line
+				double length = 5.0;
+				double pickedNormal[3];
+				cellPicker->GetPickNormal(pickedNormal);
+				double lineP0[3] = {picked[0], picked[1], picked[2]};
+				double lineP1[3] = {picked[0]+pickedNormal[0]*length, picked[1]+pickedNormal[1]*length, picked[2]+pickedNormal[2]*length};
+				vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
+
+				viewer_->getLocators().begin()->second->IntersectWithLine(lineP0, lineP1, intersectPoints, NULL);
+
+				// Display list of intersections
+				double intersection[3];
+				double previous[3] = {picked[0], picked[1], picked[2]};
+				for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
+				{
+					intersectPoints->GetPoint(i, intersection);
+
+					Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
+					float n = v.norm();
+					if(n  > 0.01f)
+					{
+						v/=n;
+						v *= n/2.0f;
+						pt.r = 125;
+						pt.g = 125;
+						pt.b = 125;
+						pt.x = intersection[0];
+						pt.y = intersection[1];
+						pt.z = intersection[2];
+						pointsHolder_->at(1) = pt;
+						viewer_->addOrUpdateText("interactor_ray_text_alt", uFormat("%.2f m", n),
+								Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
+								textSize,
+								Qt::gray);
+						viewer_->addOrUpdateLine("interactor_ray_alt",
+								Transform(previous[0], previous[1], previous[2], 0, 0, 0),
+								Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
+								Qt::gray);
+
+						previous[0] = intersection[0];
+						previous[1] = intersection[1];
+						previous[2] = intersection[2];
+						break;
+					}
+				}
+				viewer_->addCloud("interactor_points_alt", pointsHolder_);
+				viewer_->setCloudPointSize("interactor_points_alt", 15);
+				viewer_->setCloudOpacity("interactor_points_alt", 0.5);
+			}
+		}
+	}
+	// Forward events
+	PCLVisualizerInteractorStyle::OnMouseMove();
+}
+
+void CloudViewerInteractorStyle::OnLeftButtonDown()
+{
+	// http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/DoubleClick
+	// http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/PointPicker
+	if(this->CurrentRenderer && this->CurrentRenderer->GetLayer() == 1)
+	{
+		this->NumberOfClicks++;
+		int pickPosition[2];
+		this->GetInteractor()->GetEventPosition(pickPosition);
+		int xdist = pickPosition[0] - this->PreviousPosition[0];
+		int ydist = pickPosition[1] - this->PreviousPosition[1];
+
+		this->PreviousPosition[0] = pickPosition[0];
+		this->PreviousPosition[1] = pickPosition[1];
+
+		int moveDistance = (int)sqrt((double)(xdist*xdist + ydist*ydist));
+
+		// Reset numClicks - If mouse moved further than resetPixelDistance
+		if(moveDistance > this->ResetPixelDistance)
+		{
+			this->NumberOfClicks = 1;
+		}
+
+		if(this->NumberOfClicks >= 2)
+		{
+			this->NumberOfClicks = 0;
+			int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+					0,  // always zero.
+					this->CurrentRenderer);
+			if(result && this->GetInteractor()->GetControlKey()==0)
+			{
+				double picked[3];
+				this->Interactor->GetPicker()->GetPickPosition(picked);
+				UDEBUG("Double clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+				vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
+				UASSERT(camera);
+				double position[3];
+				double focal[3];
+				camera->GetPosition(position[0], position[1], position[2]);
+				camera->GetFocalPoint(focal[0], focal[1], focal[2]);
+				//camera->SetPosition (position[0] + (picked[0]-focal[0]), position[1] + (picked[1]-focal[1]), position[2] + (picked[2]-focal[2]));
+				camera->SetFocalPoint (picked[0], picked[1], picked[2]);
+				camera->OrthogonalizeViewUp();
+
+				if (this->AutoAdjustCameraClippingRange)
+				{
+					this->CurrentRenderer->ResetCameraClippingRange();
+				}
+
+				if (this->Interactor->GetLightFollowCamera())
+				{
+					this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
+				}
+			}
+			else if(viewer_)
+			{
+				viewer_->removeText("interactor_pose");
+				viewer_->removeLine("interactor_line");
+				viewer_->removeCloud("interactor_points");
+				viewer_->removeLine("interactor_ray");
+				viewer_->removeText("interactor_ray_text");
+				viewer_->removeCloud("interactor_points_alt");
+				viewer_->removeLine("interactor_ray_alt");
+				viewer_->removeText("interactor_ray_text_alt");
+				PreviousMeasure[0] = 0.0f;
+				PreviousMeasure[1] = 0.0f;
+				PreviousMeasure[2] = 0.0f;
+			}
+		}
+		else if(this->GetInteractor()->GetControlKey() && viewer_)
+		{
+			int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+					0,  // always zero.
+					this->CurrentRenderer);
+			if(result)
+			{
+				double picked[3];
+				this->Interactor->GetPicker()->GetPickPosition(picked);
+
+				UDEBUG("Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+
+				float textSize = 0.05;
+
+				viewer_->removeCloud("interactor_points");
+				pointsHolder_->clear();
+				pcl::PointXYZRGB pt;
+				pt.r = 255;
+				pt.x = picked[0];
+				pt.y = picked[1];
+				pt.z = picked[2];
+				pointsHolder_->push_back(pt);
+
+				viewer_->removeLine("interactor_ray");
+				viewer_->removeText("interactor_ray_text");
+
+				if(	PreviousMeasure[0] != 0.0f && PreviousMeasure[1] != 0.0f && PreviousMeasure[2] != 0.0f &&
+					viewer_->getAddedLines().find("interactor_line") == viewer_->getAddedLines().end())
+				{
+					viewer_->addOrUpdateLine("interactor_line",
+							Transform(PreviousMeasure[0], PreviousMeasure[1], PreviousMeasure[2], 0, 0, 0),
+							Transform(picked[0], picked[1], picked[2], 0, 0, 0),
+							Qt::red);
+					pt.x = PreviousMeasure[0];
+					pt.y = PreviousMeasure[1];
+					pt.z = PreviousMeasure[2];
+					pointsHolder_->push_back(pt);
+
+					Eigen::Vector3f v(picked[0]-PreviousMeasure[0], picked[1]-PreviousMeasure[1], picked[2]-PreviousMeasure[2]);
+					float n = v.norm();
+					v/=n;
+					v *= n/2.0f;
+					viewer_->addOrUpdateText("interactor_pose", uFormat("%.2f m", n),
+							Transform(PreviousMeasure[0]+v[0], PreviousMeasure[1]+v[1],PreviousMeasure[2]+v[2], 0, 0, 0),
+							textSize,
+							Qt::red);
+				}
+				else
+				{
+					viewer_->removeText("interactor_pose");
+					viewer_->removeLine("interactor_line");
+				}
+				PreviousMeasure[0] = picked[0];
+				PreviousMeasure[1] = picked[1];
+				PreviousMeasure[2] = picked[2];
+
+				viewer_->addCloud("interactor_points", pointsHolder_);
+				viewer_->setCloudPointSize("interactor_points", 15);
+				viewer_->setCloudOpacity("interactor_points", 0.5);
+			}
+		}
+	}
+
+	// Forward events
+	PCLVisualizerInteractorStyle::OnLeftButtonDown();
+}
+
+void CloudViewerInteractorStyle::OnRightButtonDown()
+{
+	// ignore
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/ConsoleWidget.cpp

@@ -0,0 +1,161 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/ConsoleWidget.h"
+#include "ui_consoleWidget.h"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <QMessageBox>
+#include <QtGui/QTextCursor>
+#include <QtCore/QTimer>
+
+namespace rtabmap {
+
+ConsoleWidget::ConsoleWidget(QWidget * parent) :
+	QWidget(parent)
+{
+	_ui = new Ui_consoleWidget();
+	_ui->setupUi(this);
+	UEventsManager::addHandler(this);
+	_ui->textEdit->document()->setMaximumBlockCount(_ui->spinBox_lines->value());
+	_textCursor = new QTextCursor(_ui->textEdit->document());
+	_ui->textEdit->setFontPointSize(10);
+	QPalette p(_ui->textEdit->palette());
+	p.setColor(QPalette::Base, Qt::black);
+	_ui->textEdit->setPalette(p);
+	_errorMessage = new QMessageBox(QMessageBox::Critical, tr("Fatal error occurred"), "", QMessageBox::Ok, this);
+	_errorMessageMutex.lock();
+	_time.start();
+	_timer.setSingleShot(true);
+	connect(_ui->pushButton_clear, SIGNAL(clicked()), _ui->textEdit, SLOT(clear()));
+	connect(_ui->spinBox_lines, SIGNAL(valueChanged(int)), this, SLOT(updateTextEditBufferSize()));
+	connect(this, SIGNAL(msgReceived(const QString &, int)), this, SLOT(appendMsg(const QString &, int)));
+	connect(&_timer, SIGNAL(timeout()), this, SLOT(flushConsole()));
+}
+
+ConsoleWidget::~ConsoleWidget()
+{
+	delete _ui;
+	_errorMessageMutex.unlock();
+}
+
+bool ConsoleWidget::handleEvent(UEvent * anEvent)
+{
+	// WARNING, don't put a log message here! otherwise it could be recursively called.
+	if(anEvent->getClassName().compare("ULogEvent") == 0)
+	{
+		ULogEvent * logEvent = (ULogEvent*)anEvent;
+		_msgListMutex.lock();
+		_msgList.append(QPair<QString, int>(logEvent->getMsg().c_str(), logEvent->getCode()));
+		while(_ui->spinBox_lines->value()>0 && _msgList.size()>_ui->spinBox_lines->value())
+		{
+			_msgList.pop_front();
+		}
+		_msgListMutex.unlock();
+
+		if(_ui->spinBox_time->value()>0 && _time.restart() < _ui->spinBox_time->value())
+		{
+			if(logEvent->getCode() == ULogger::kFatal)
+			{
+				QMetaObject::invokeMethod(&_timer, "start", Q_ARG(int, 0));
+			}
+			else
+			{
+				QMetaObject::invokeMethod(&_timer, "start", Q_ARG(int, _ui->spinBox_time->value()));
+			}
+		}
+		else
+		{
+			QMetaObject::invokeMethod(&_timer, "start", Q_ARG(int, 0));
+		}
+
+		if(logEvent->getCode() == ULogger::kFatal)
+		{
+			//This thread will wait until the message box is closed...
+			// Assuming that error messages come from a different thread.
+			_errorMessageMutex.lock();
+		}
+
+	}
+	return false;
+}
+
+void ConsoleWidget::appendMsg(const QString & msg, int level)
+{
+	switch(level)
+	{
+	case 0:
+		_ui->textEdit->setTextColor(Qt::darkGreen);
+		break;
+	case 2:
+		_ui->textEdit->setTextColor(Qt::yellow);
+		break;
+	case 3:
+	case 4:
+		_ui->textEdit->setTextColor(Qt::red);
+		break;
+	default:
+		_ui->textEdit->setTextColor(Qt::white);
+		break;
+	}
+	_ui->textEdit->append(msg);
+
+	if(level == ULogger::kFatal)
+	{
+		_textCursor->endEditBlock();
+		QTextCursor cursor = _ui->textEdit->textCursor();
+		cursor.movePosition(QTextCursor::End, QTextCursor::MoveAnchor);
+		_ui->textEdit->setTextCursor(cursor);
+		//The application will exit, so warn the user.
+		_errorMessage->setText(tr("Description:\n\n%1\n\nThe application will now exit...").arg(msg));
+		_errorMessage->exec();
+		_errorMessageMutex.unlock();
+	}
+}
+
+void ConsoleWidget::flushConsole()
+{
+	_msgListMutex.lock();
+	_textCursor->beginEditBlock();
+	for(int i=0; i<_msgList.size(); ++i)
+	{
+		appendMsg(_msgList[i].first, _msgList[i].second);
+	}
+	_textCursor->endEditBlock();
+	_msgList.clear();
+	_msgListMutex.unlock();
+	QTextCursor cursor = _ui->textEdit->textCursor();
+	cursor.movePosition(QTextCursor::End, QTextCursor::MoveAnchor);
+	_ui->textEdit->setTextCursor(cursor);
+}
+
+void ConsoleWidget::updateTextEditBufferSize()
+{
+	_ui->textEdit->document()->setMaximumBlockCount(_ui->spinBox_lines->value());
+}
+
+}

rtabmap/guilib/src/CreateSimpleCalibrationDialog.cpp

@@ -0,0 +1,309 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/CreateSimpleCalibrationDialog.h"
+#include "ui_createSimpleCalibrationDialog.h"
+
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/utilite/ULogger.h"
+
+#include <QFileDialog>
+#include <QPushButton>
+#include <QMessageBox>
+
+namespace rtabmap {
+
+CreateSimpleCalibrationDialog::CreateSimpleCalibrationDialog(
+		const QString & savingFolder,
+		const QString & cameraName,
+		QWidget * parent) :
+	QDialog(parent),
+	savingFolder_(savingFolder),
+	cameraName_(cameraName)
+{
+	if(cameraName_.isEmpty())
+	{
+		cameraName_ = "calib";
+	}
+
+	ui_ = new Ui_createSimpleCalibrationDialog();
+	ui_->setupUi(this);
+
+	connect(ui_->buttonBox->button(QDialogButtonBox::Save), SIGNAL(clicked()), this, SLOT(saveCalibration()));
+	connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
+
+	connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), ui_->stackedWidget, SLOT(setCurrentIndex(int)));
+
+	connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateStereoView()));
+	connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStereoView()));
+	connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateSaveStatus()));
+	connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->doubleSpinBox_fx, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_fy, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->doubleSpinBox_fx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_fy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_cx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_cy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->lineEdit_D_l, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->doubleSpinBox_fx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_fy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_cx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_cy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->lineEdit_D_r, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->spinBox_width, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
+	connect(ui_->spinBox_height, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
+
+	connect(ui_->doubleSpinBox_baseline, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->lineEdit_RT, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
+
+	ui_->stackedWidget->setCurrentIndex(ui_->comboBox_advanced->currentIndex());
+
+	ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(false);
+
+	updateStereoView();
+}
+
+CreateSimpleCalibrationDialog::~CreateSimpleCalibrationDialog()
+{
+	delete ui_;
+}
+
+void CreateSimpleCalibrationDialog::updateStereoView()
+{
+	bool checked = ui_->checkBox_stereo->isChecked();
+	ui_->doubleSpinBox_baseline->setVisible(checked);
+	ui_->label_baseline->setVisible(checked);
+	ui_->label_right->setVisible(checked);
+	ui_->doubleSpinBox_fx_r->setVisible(checked);
+	ui_->doubleSpinBox_fy_r->setVisible(checked);
+	ui_->doubleSpinBox_cx_r->setVisible(checked);
+	ui_->doubleSpinBox_cy_r->setVisible(checked);
+	ui_->lineEdit_D_r->setVisible(checked);
+	ui_->groupBox_stereo_extrinsics->setVisible(ui_->comboBox_advanced->currentIndex() == 1 && checked);
+	ui_->label_left->setVisible(checked);
+}
+
+void CreateSimpleCalibrationDialog::updateSaveStatus()
+{
+	bool valid = false;
+	if(ui_->comboBox_advanced->currentIndex() == 0 &&
+	   ui_->doubleSpinBox_fx->value() > 0.0 &&
+	   ui_->doubleSpinBox_fy->value() > 0.0 &&
+	   ui_->spinBox_width->value() > 0 &&
+	   ui_->spinBox_height->value() > 0 &&
+	   (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_baseline->value() != 0.0))
+	{
+		// basic
+		valid = true;
+	}
+	else if(ui_->comboBox_advanced->currentIndex() == 1 &&
+			 ui_->doubleSpinBox_fx_l->value() > 0.0 &&
+			 ui_->doubleSpinBox_fy_l->value() > 0.0 &&
+			 ui_->doubleSpinBox_cx_l->value() > 0.0 &&
+			 ui_->doubleSpinBox_cy_l->value() > 0.0 &&
+			 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fx_r->value() > 0.0) &&
+			 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fy_r->value() > 0.0) &&
+			 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cx_r->value() > 0.0) &&
+			 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cy_r->value() > 0.0) &&
+			 ui_->spinBox_width->value() > 0 &&
+			 ui_->spinBox_height->value() > 0 &&
+			 !ui_->lineEdit_D_l->text().isEmpty() &&
+			 (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_D_r->text().isEmpty()) &&
+			 (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_RT->text().isEmpty()))
+	{
+		//advanced
+		QStringList distorsionsStrListL = ui_->lineEdit_D_l->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
+		QStringList distorsionsStrListR = ui_->lineEdit_D_r->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
+		std::string RT = ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().toStdString();
+
+		if((distorsionsStrListL.size() == 4 || distorsionsStrListL.size() == 5 || distorsionsStrListL.size() == 8) &&
+				(!ui_->checkBox_stereo->isChecked() || (distorsionsStrListR.size() == 4 || distorsionsStrListR.size() == 5 || distorsionsStrListR.size() == 8)) &&
+				(!ui_->checkBox_stereo->isChecked() || (!RT.empty() && Transform::canParseString(RT))))
+		{
+			valid = true;
+		}
+	}
+	ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(valid);
+}
+
+void CreateSimpleCalibrationDialog::saveCalibration()
+{
+	QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_+"/"+cameraName_+".yaml", "*.yaml");
+	QString name = QFileInfo(filePath).baseName();
+	QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
+	if(!filePath.isEmpty())
+	{
+		cameraName_ = name;
+		CameraModel modelLeft;
+		float width = ui_->spinBox_width->value();
+		float height = ui_->spinBox_height->value();
+		if(ui_->comboBox_advanced->currentIndex() == 0)
+		{
+			//basic
+			modelLeft = CameraModel(
+					name.toStdString(),
+					ui_->doubleSpinBox_fx->value(),
+					ui_->doubleSpinBox_fy->value(),
+					ui_->doubleSpinBox_cx->value(),
+					ui_->doubleSpinBox_cy->value(),
+					CameraModel::opticalRotation(),
+					0,
+					cv::Size(width, height));
+			UASSERT(modelLeft.isValidForProjection());
+		}
+		else
+		{
+			//advanced
+			cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+			K.at<double>(0,0) = ui_->doubleSpinBox_fx_l->value();
+			K.at<double>(1,1) = ui_->doubleSpinBox_fy_l->value();
+			K.at<double>(0,2) = ui_->doubleSpinBox_cx_l->value();
+			K.at<double>(1,2) = ui_->doubleSpinBox_cy_l->value();
+			cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+			cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
+			K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
+
+			QStringList distorsionCoeffs = ui_->lineEdit_D_l->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
+			UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
+			cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
+			bool ok;
+			for(int i=0; i<distorsionCoeffs.size(); ++i)
+			{
+				D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
+				if(!ok)
+				{
+					QMessageBox::warning(this, tr("Save"), tr("Error parsing left distortion coefficients \"%1\".").arg(ui_->lineEdit_D_l->text()));
+					return;
+				}
+			}
+
+			modelLeft = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
+			UASSERT(modelLeft.isValidForRectification());
+		}
+
+		if(ui_->checkBox_stereo->isChecked())
+		{
+			StereoCameraModel stereoModel;
+			if(ui_->comboBox_advanced->currentIndex() == 0)
+			{
+				CameraModel modelRight(
+						name.toStdString(),
+						ui_->doubleSpinBox_fx->value(),
+						ui_->doubleSpinBox_fy->value(),
+						ui_->doubleSpinBox_cx->value(),
+						ui_->doubleSpinBox_cy->value(),
+						Transform::getIdentity(),
+						ui_->doubleSpinBox_baseline->value()*-ui_->doubleSpinBox_fx->value(),
+						cv::Size(width, height));
+				UASSERT(modelRight.isValidForProjection());
+				stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform());
+				UASSERT(stereoModel.isValidForProjection());
+			}
+			else if(ui_->comboBox_advanced->currentIndex() == 1)
+			{
+				CameraModel modelRight;
+				cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+				K.at<double>(0,0) = ui_->doubleSpinBox_fx_r->value();
+				K.at<double>(1,1) = ui_->doubleSpinBox_fy_r->value();
+				K.at<double>(0,2) = ui_->doubleSpinBox_cx_r->value();
+				K.at<double>(1,2) = ui_->doubleSpinBox_cy_r->value();
+				cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+				cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
+				K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
+
+				QStringList distorsionCoeffs = ui_->lineEdit_D_r->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
+				UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
+				cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
+				bool ok;
+				for(int i=0; i<distorsionCoeffs.size(); ++i)
+				{
+					D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
+					if(!ok)
+					{
+						QMessageBox::warning(this, tr("Save"), tr("Error parsing right distortion coefficients \"%1\".").arg(ui_->lineEdit_D_r->text()));
+						return;
+					}
+				}
+
+				modelRight = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
+				UASSERT(modelRight.isValidForRectification());
+
+				UASSERT(Transform::canParseString(ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().toStdString()));
+				stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform::fromString(ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').trimmed().toStdString()));
+				if(stereoModel.baseline() < 0)
+				{
+					QMessageBox::warning(this, tr("Save"), tr("Error parsing the extrinsics \"%1\", resulting baseline (%f) is negative!").arg(ui_->lineEdit_RT->text()).arg(stereoModel.baseline()));
+					return;
+				}
+				UASSERT(stereoModel.isValidForRectification());
+			}
+
+			std::string base = (dir+QDir::separator()+name).toStdString();
+			std::string leftPath = base+"_left.yaml";
+			std::string rightPath = base+"_right.yaml";
+
+			if(stereoModel.save(dir.toStdString(), ui_->comboBox_advanced->currentIndex() != 1))
+			{
+				if(ui_->comboBox_advanced->currentIndex() == 0)
+				{
+					QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\".").
+							arg(leftPath.c_str()).arg(rightPath.c_str()));
+				}
+				else
+				{
+					std::string posePath = base+"_pose.yaml";
+					QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\"\n  \"%3\".").
+							arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
+				}
+				this->accept();
+			}
+			else
+			{
+				QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\" and \"%2\"").arg(leftPath.c_str()).arg(rightPath.c_str()));
+			}
+		}
+		else
+		{
+			if(modelLeft.save(dir.toStdString()))
+			{
+				QMessageBox::information(this, tr("Save"), tr("Calibration file saved to \"%1\".").arg(filePath));
+				this->accept();
+			}
+			else
+			{
+				QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\"").arg(filePath));
+			}
+		}
+	}
+}
+
+}

rtabmap/guilib/src/DataRecorder.cpp

@@ -0,0 +1,206 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/DataRecorder.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/core/SensorEvent.h>
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/gui/ImageView.h>
+#include <rtabmap/utilite/UCv2Qt.h>
+#include <QtCore/QMetaType>
+#include <QMessageBox>
+#include <QtGui/QCloseEvent>
+#include <QLabel>
+#include <QVBoxLayout>
+
+namespace rtabmap {
+
+
+DataRecorder::DataRecorder(QWidget * parent) :
+		QWidget(parent),
+	memory_(0),
+	imageView_(new ImageView(this)),
+	label_(new QLabel(this)),
+	processingImages_(false),
+	count_(0),
+	totalSizeKB_(0)
+{
+	qRegisterMetaType<cv::Mat>("cv::Mat");
+
+	imageView_->setImageDepthShown(true);
+	imageView_->setMinimumSize(320, 240);
+	QVBoxLayout * layout = new QVBoxLayout(this);
+	layout->setContentsMargins(0,0,0,0);
+	layout->addWidget(imageView_);
+	layout->addWidget(label_);
+	layout->setStretch(0,1);
+	this->setLayout(layout);
+}
+bool DataRecorder::init(const QString & path, bool recordInRAM)
+{
+	UScopeMutex scope(memoryMutex_);
+	if(!memory_)
+	{
+		ParametersMap customParameters;
+		customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // deactivate rehearsal
+		customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), "-1")); // deactivate keypoints extraction
+		customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "true")); // to keep images
+		customParameters.insert(ParametersPair(Parameters::kMemMapLabelsAdded(), "false")); // don't create map labels
+		customParameters.insert(ParametersPair(Parameters::kMemBadSignaturesIgnored(), "true")); // make sure memory cleanup is done
+		customParameters.insert(ParametersPair(Parameters::kMemIntermediateNodeDataKept(), "true"));
+		if(!recordInRAM)
+		{
+			customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
+		}
+		memory_ = new Memory();
+		if(!memory_->init(path.toStdString(), true, customParameters))
+		{
+			delete memory_;
+			memory_ = 0;
+			UERROR("Error initializing the memory.");
+			return false;
+		}
+		path_ = path;
+		return true;
+	}
+	else
+	{
+		UERROR("Already initialized, close it first.");
+		return false;
+	}
+}
+
+void DataRecorder::closeRecorder()
+{
+	memoryMutex_.lock();
+	if(memory_)
+	{
+		delete memory_;
+		memory_ = 0;
+		UINFO("Data recorded to \"%s\".", this->path().toStdString().c_str());
+	}
+	memoryMutex_.unlock();
+	processingImages_ = false;
+	count_ = 0;
+	totalSizeKB_ = 0;
+	if(this->isVisible())
+	{
+		QMessageBox::information(this, tr("Data recorder"), tr("Data recorded to \"%1\".").arg(this->path()));
+	}
+}
+
+DataRecorder::~DataRecorder()
+{
+	this->unregisterFromEventsManager();
+	this->closeRecorder();
+}
+
+void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & pose, const cv::Mat & covariance)
+{
+	memoryMutex_.lock();
+	if(memory_)
+	{
+		if(memory_->getStMem().size() == 0 && data.id() > 0)
+		{
+			ParametersMap customParameters;
+			customParameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false")); // use id from data
+			memory_->parseParameters(customParameters);
+		}
+
+		//save to database
+		UTimer time;
+		memory_->update(data, pose, covariance);
+		const Signature * s = memory_->getLastWorkingSignature();
+		totalSizeKB_ += (int)s->sensorData().imageCompressed().total()/1000;
+		totalSizeKB_ += (int)s->sensorData().depthOrRightCompressed().total()/1000;
+		totalSizeKB_ += (int)s->sensorData().laserScanCompressed().data().total()/1000;
+		memory_->cleanup();
+
+		if(++count_ % 30)
+		{
+			memory_->emptyTrash();
+		}
+		UDEBUG("Time to process a message = %f s, totalSizeKB_=%d", time.ticks(), totalSizeKB_);
+	}
+	memoryMutex_.unlock();
+}
+
+void DataRecorder::showImage(const cv::Mat & image, const cv::Mat & depth)
+{
+	processingImages_ = true;
+	if(!image.empty()) {
+		imageView_->setImage(uCvMat2QImage(image));
+	}
+	if(!depth.empty()) {
+		imageView_->setImageDepth(depth);
+	}
+	label_->setText(tr("Images=%1 (~%2 MB)").arg(count_).arg(totalSizeKB_/1000));
+	processingImages_ = false;
+}
+
+void DataRecorder::closeEvent(QCloseEvent* event)
+{
+	this->closeRecorder();
+	event->accept();
+}
+
+bool DataRecorder::handleEvent(UEvent * event)
+{
+	if(memory_)
+	{
+		if(event->getClassName().compare("SensorEvent") == 0)
+		{
+			SensorEvent * camEvent = (SensorEvent*)event;
+			if(camEvent->getCode() == SensorEvent::kCodeData)
+			{
+				if(camEvent->data().isValid())
+				{
+					UINFO("Receiving rate = %f Hz", 1.0f/timer_.ticks());
+					this->addData(
+							camEvent->data(),
+							camEvent->info().odomPose,
+							camEvent->info().odomCovariance.empty()?cv::Mat::eye(6,6,CV_64FC1):camEvent->info().odomCovariance);
+
+					if(!processingImages_ && this->isVisible() && camEvent->data().isValid())
+					{
+						processingImages_ = true;
+						QMetaObject::invokeMethod(this, "showImage",
+								Q_ARG(cv::Mat, camEvent->data().imageRaw()),
+								Q_ARG(cv::Mat, camEvent->data().depthOrRightRaw()));
+					}
+				}
+			}
+		}
+	}
+	return false;
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/DepthCalibrationDialog.cpp

@@ -0,0 +1,562 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/gui/DepthCalibrationDialog.h>
+#include "ui_depthCalibrationDialog.h"
+
+#include "rtabmap/gui/ProgressDialog.h"
+#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/gui/ImageView.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UThread.h"
+#include "rtabmap/utilite/UCv2Qt.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_transforms.h"
+
+#include "rtabmap/core/clams/slam_calibrator.h"
+#include "rtabmap/core/clams/frame_projector.h"
+
+#include <QPushButton>
+#include <QDir>
+#include <QFileInfo>
+#include <QUrl>
+#include <QtGui/QDesktopServices>
+#include <QMessageBox>
+#include <QFileDialog>
+
+namespace rtabmap {
+
+DepthCalibrationDialog::DepthCalibrationDialog(QWidget *parent) :
+			QDialog(parent),
+			_canceled(false),
+			_model(0)
+{
+	_ui = new Ui_DepthCalibrationDialog();
+	_ui->setupUi(this);
+
+	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+	connect(_ui->buttonBox->button(QDialogButtonBox::Save), SIGNAL(clicked()), this, SLOT(saveModel()));
+	connect(_ui->buttonBox->button(QDialogButtonBox::Ok), SIGNAL(clicked()), this, SLOT(accept()));
+	_ui->buttonBox->button(QDialogButtonBox::Ok)->setText("Calibrate");
+
+	restoreDefaults();
+
+	connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_minDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_coneRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_coneStdDevThresh, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_laserScan, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+
+	connect(_ui->spinBox_bin_width, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_bin_height, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_bin_depth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_smoothing, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_maxDepthModel, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+
+	_ui->buttonBox->button(QDialogButtonBox::Ok)->setFocus();
+
+	_progressDialog = new ProgressDialog(this);
+	_progressDialog->setVisible(false);
+	_progressDialog->setAutoClose(true, 2);
+	_progressDialog->setMinimumWidth(600);
+	_progressDialog->setCancelButtonVisible(true);
+
+	connect(_progressDialog, SIGNAL(canceled()), this, SLOT(cancel()));
+}
+
+DepthCalibrationDialog::~DepthCalibrationDialog()
+{
+	delete _ui;
+	delete _model;
+}
+
+void DepthCalibrationDialog::saveSettings(QSettings & settings, const QString & group) const
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+
+	settings.setValue("decimation", _ui->spinBox_decimation->value());
+	settings.setValue("max_depth", _ui->doubleSpinBox_maxDepth->value());
+	settings.setValue("min_depth", _ui->doubleSpinBox_minDepth->value());
+	settings.setValue("voxel",_ui->doubleSpinBox_voxelSize->value());
+	settings.setValue("cone_radius",_ui->doubleSpinBox_coneRadius->value());
+	settings.setValue("cone_stddev_thresh",_ui->doubleSpinBox_coneStdDevThresh->value());
+	settings.setValue("laser_scan",_ui->checkBox_laserScan->isChecked());
+
+	settings.setValue("bin_width",_ui->spinBox_bin_width->value());
+	settings.setValue("bin_height",_ui->spinBox_bin_height->value());
+	settings.setValue("bin_depth",_ui->doubleSpinBox_bin_depth->value());
+	settings.setValue("smoothing",_ui->spinBox_smoothing->value());
+	settings.setValue("max_model_depth",_ui->doubleSpinBox_maxDepthModel->value());
+
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void DepthCalibrationDialog::loadSettings(QSettings & settings, const QString & group)
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+
+	_ui->spinBox_decimation->setValue(settings.value("decimation", _ui->spinBox_decimation->value()).toInt());
+	_ui->doubleSpinBox_maxDepth->setValue(settings.value("max_depth", _ui->doubleSpinBox_maxDepth->value()).toDouble());
+	_ui->doubleSpinBox_minDepth->setValue(settings.value("min_depth", _ui->doubleSpinBox_minDepth->value()).toDouble());
+	_ui->doubleSpinBox_voxelSize->setValue(settings.value("voxel", _ui->doubleSpinBox_voxelSize->value()).toDouble());
+	_ui->doubleSpinBox_coneRadius->setValue(settings.value("cone_radius", _ui->doubleSpinBox_coneRadius->value()).toDouble());
+	_ui->doubleSpinBox_coneStdDevThresh->setValue(settings.value("cone_stddev_thresh", _ui->doubleSpinBox_coneStdDevThresh->value()).toDouble());
+	_ui->checkBox_laserScan->setChecked(settings.value("laser_scan", _ui->checkBox_laserScan->isChecked()).toBool());
+
+	_ui->spinBox_bin_width->setValue(settings.value("bin_width", _ui->spinBox_bin_width->value()).toInt());
+	_ui->spinBox_bin_height->setValue(settings.value("bin_height", _ui->spinBox_bin_height->value()).toInt());
+	_ui->doubleSpinBox_bin_depth->setValue(settings.value("bin_depth", _ui->doubleSpinBox_bin_depth->value()).toDouble());
+	_ui->spinBox_smoothing->setValue(settings.value("smoothing", _ui->spinBox_smoothing->value()).toInt());
+	_ui->doubleSpinBox_maxDepthModel->setValue(settings.value("max_model_depth", _ui->doubleSpinBox_maxDepthModel->value()).toDouble());
+
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void DepthCalibrationDialog::restoreDefaults()
+{
+	_ui->spinBox_decimation->setValue(1);
+	_ui->doubleSpinBox_maxDepth->setValue(3.5);
+	_ui->doubleSpinBox_minDepth->setValue(0);
+	_ui->doubleSpinBox_voxelSize->setValue(0.01);
+	_ui->doubleSpinBox_coneRadius->setValue(0.02);
+	_ui->doubleSpinBox_coneStdDevThresh->setValue(0.1); // 0.03
+	_ui->checkBox_laserScan->setChecked(false);
+	_ui->checkBox_resetModel->setChecked(true);
+
+	_ui->spinBox_bin_width->setValue(8);
+	_ui->spinBox_bin_height->setValue(6);
+	if(_imageSize.width > 0 && _imageSize.height > 0)
+	{
+		size_t bin_width, bin_height;
+		clams::DiscreteDepthDistortionModel::getBinSize(_imageSize.width, _imageSize.height, bin_width, bin_height);
+		_ui->spinBox_bin_width->setValue(bin_width);
+		_ui->spinBox_bin_height->setValue(bin_height);
+	}
+	_ui->doubleSpinBox_bin_depth->setValue(2.0),
+	_ui->spinBox_smoothing->setValue(1);
+	_ui->doubleSpinBox_maxDepthModel->setValue(10.0);
+}
+
+void DepthCalibrationDialog::saveModel()
+{
+	if(_model && _model->getTrainingSamples())
+	{
+		QString path = QFileDialog::getSaveFileName(this, tr("Save distortion model to ..."), _workingDirectory+QDir::separator()+"distortion_model.bin", tr("Distortion model (*.bin *.txt)"));
+		if(!path.isEmpty())
+		{
+			//
+			// Save depth calibration
+			//
+			cv::Mat results = _model->visualize(ULogger::level() == ULogger::kDebug?_workingDirectory.toStdString():"");
+			_model->save(path.toStdString());
+
+			if(!results.empty())
+			{
+				QString name = QString(path).replace(".bin", ".png", Qt::CaseInsensitive).replace(".txt", ".png", Qt::CaseInsensitive);
+				cv::imwrite(name.toStdString(), results);
+				QDesktopServices::openUrl(QUrl::fromLocalFile(name));
+			}
+
+			QMessageBox::information(this, tr("Depth Calibration"), tr("Distortion model saved to \"%1\"!").arg(path));
+		}
+	}
+}
+
+void DepthCalibrationDialog::cancel()
+{
+	_canceled = true;
+	_progressDialog->appendText(tr("Canceled!"));
+}
+
+void DepthCalibrationDialog::calibrate(
+		const std::map<int, Transform> & poses,
+			const QMap<int, Signature> & cachedSignatures,
+			const QString & workingDirectory,
+			const ParametersMap & parameters)
+{
+	_canceled = false;
+	_workingDirectory = workingDirectory;
+	_ui->buttonBox->button(QDialogButtonBox::Save)->setEnabled(_model && _model->getTrainingSamples()>0);
+	if(_model)
+	{
+		_ui->label_trainingSamples->setNum((int)_model->getTrainingSamples());
+	}
+
+	_ui->label_width->setText("NA");
+	_ui->label_height->setText("NA");
+	_imageSize = cv::Size();
+	CameraModel model;
+	if(cachedSignatures.size())
+	{
+		const Signature & s = cachedSignatures.begin().value();
+		const SensorData & data = s.sensorData();
+		cv::Mat depth;
+		data.uncompressDataConst(0, &depth);
+		if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
+		{
+			// use depth image size
+			_imageSize = depth.size();
+			_ui->label_width->setNum(_imageSize.width);
+			_ui->label_height->setNum(_imageSize.height);
+
+			if(_imageSize.width % _ui->spinBox_bin_width->value() != 0 ||
+				_imageSize.height % _ui->spinBox_bin_height->value() != 0)
+			{
+				size_t bin_width, bin_height;
+				clams::DiscreteDepthDistortionModel::getBinSize(_imageSize.width, _imageSize.height, bin_width, bin_height);
+				_ui->spinBox_bin_width->setValue(bin_width);
+				_ui->spinBox_bin_height->setValue(bin_height);
+			}
+		}
+		else if(data.cameraModels().size() > 1)
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"),tr("Multi-camera not supported!"));
+			return;
+		}
+		else if(data.cameraModels().size() != 1)
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"), tr("Camera model not found."));
+			return;
+		}
+		else if(data.cameraModels().size() == 1 && !data.cameraModels()[0].isValidForProjection())
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"), tr("Camera model %1 not valid for projection.").arg(s.id()));
+			return;
+		}
+		else
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"), tr("Depth image cannot be found in the cache, make sure to update cache before doing calibration."));
+			return;
+		}
+	}
+	else
+	{
+		QMessageBox::warning(this, tr("Depth Calibration"), tr("No signatures detected! Map is empty!?"));
+		return;
+	}
+
+	if(this->exec() == QDialog::Accepted)
+	{
+		if(_model && _ui->checkBox_resetModel->isChecked())
+		{
+			delete _model;
+			_model = 0;
+		}
+
+		if(_ui->doubleSpinBox_maxDepthModel->value() < _ui->doubleSpinBox_bin_depth->value())
+		{
+			QMessageBox::warning(this, tr("Wrong parameter"), tr("Maximum model depth should be higher than bin depth, setting to bin depth x5."));
+			_ui->doubleSpinBox_maxDepthModel->setValue(_ui->doubleSpinBox_bin_depth->value() * 5.0);
+		}
+
+		_progressDialog->setMaximumSteps(poses.size()*2 + 3);
+		if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
+		{
+			_progressDialog->setMaximumSteps(_progressDialog->maximumSteps()+1);
+		}
+		_progressDialog->resetProgress();
+		_progressDialog->show();
+
+		std::map<int, rtabmap::SensorData> sequence;
+
+		// Create the map
+		pcl::PointCloud<pcl::PointXYZ>::Ptr  map(new pcl::PointCloud<pcl::PointXYZ>);
+		int index=1;
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end() && !_canceled; ++iter, ++index)
+		{
+			int points = 0;
+			if(!iter->second.isNull())
+			{
+				pcl::IndicesPtr indices(new std::vector<int>);
+				if(cachedSignatures.contains(iter->first))
+				{
+					const Signature & s = cachedSignatures.find(iter->first).value();
+					SensorData data = s.sensorData();
+
+					cv::Mat  depth;
+					LaserScan laserScan;
+					data.uncompressData(0, &depth, _ui->checkBox_laserScan->isChecked()?&laserScan:0);
+					if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
+					{
+						UASSERT(iter->first == data.id());
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+
+						if(_ui->checkBox_laserScan->isChecked())
+						{
+							cloud = util3d::laserScanToPointCloud(laserScan);
+							indices->resize(cloud->size());
+							for(unsigned int i=0; i<indices->size(); ++i)
+							{
+								indices->at(i) = i;
+							}
+						}
+						else
+						{
+							cloud = util3d::cloudFromSensorData(
+									data,
+									_ui->spinBox_decimation->value(),
+									_ui->doubleSpinBox_maxDepth->value(),
+									_ui->doubleSpinBox_minDepth->value(),
+									indices.get(),
+									parameters);
+						}
+
+						if(indices->size())
+						{
+							if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
+							{
+								cloud = util3d::voxelize(cloud, indices, _ui->doubleSpinBox_voxelSize->value());
+							}
+
+							cloud = util3d::transformPointCloud(cloud, iter->second);
+
+							points+=cloud->size();
+
+							*map += *cloud;
+
+							sequence.insert(std::make_pair(iter->first, data));
+
+							cv::Size size = depth.size();
+							if(_model &&
+								(_model->getWidth()!=size.width ||
+								 _model->getHeight()!=size.height))
+							{
+								QString msg = tr("Depth images (%1x%2) in the map don't have the "
+										   "same size then in the current model (%3x%4). You may want "
+										   "to check \"Reset previous model\" before trying again.")
+										   .arg(size.width).arg(size.height)
+										   .arg(_model->getWidth()).arg(_model->getHeight());
+								QMessageBox::warning(this, tr("Depth Calibration"), msg);
+								_progressDialog->appendText(msg, Qt::darkRed);
+								_progressDialog->setAutoClose(false);
+								return;
+							}
+						}
+					}
+					else
+					{
+						_progressDialog->appendText(tr("Not suitable camera model found for node %1, ignoring this node!").arg(iter->first), Qt::darkYellow);
+						_progressDialog->setAutoClose(false);
+					}
+				}
+				else
+				{
+					UERROR("Cloud %d not found in cache!", iter->first);
+				}
+			}
+			else
+			{
+				UERROR("transform is null!?");
+			}
+
+			if(points>0)
+			{
+				_progressDialog->appendText(tr("Generated cloud %1 with %2 points (%3/%4).")
+						.arg(iter->first).arg(points).arg(index).arg(poses.size()));
+			}
+			else
+			{
+				_progressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(index).arg(poses.size()));
+			}
+			_progressDialog->incrementStep();
+			QApplication::processEvents();
+		}
+
+		if(!_canceled && map->size() && sequence.size())
+		{
+			if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
+			{
+				_progressDialog->appendText(tr("Voxel filtering (%1 m) of the merged point cloud (%2 points)")
+						.arg(_ui->doubleSpinBox_voxelSize->value())
+						.arg(map->size()));
+				QApplication::processEvents();
+				QApplication::processEvents();
+
+				map = util3d::voxelize(map, _ui->doubleSpinBox_voxelSize->value());
+				_progressDialog->incrementStep();
+			}
+
+			if(ULogger::level() == ULogger::kDebug)
+			{
+				//
+				// Show 3D map with frustums
+				//
+				QDialog * window = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
+				window->setAttribute(Qt::WA_DeleteOnClose, true);
+				window->setWindowTitle(tr("Map"));
+				window->setMinimumWidth(800);
+				window->setMinimumHeight(600);
+
+				CloudViewer * viewer = new CloudViewer(window);
+				viewer->setCameraLockZ(false);
+				viewer->setFrustumShown(true);
+
+				QVBoxLayout *layout = new QVBoxLayout();
+				layout->addWidget(viewer);
+				window->setLayout(layout);
+				connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
+
+				window->show();
+
+				uSleep(500);
+
+				_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)...").arg(map->size()).arg(sequence.size()));
+				_progressDialog->incrementStep();
+				viewer->addCloud("map", map);
+				for(std::map<int, SensorData>::iterator iter=sequence.begin(); iter!=sequence.end(); ++iter)
+				{
+					Transform baseToCamera = iter->second.cameraModels()[0].localTransform();
+					viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first), baseToCamera, 0.2, QColor(), iter->second.cameraModels()[0].fovX(), iter->second.cameraModels()[0].fovY());
+				}
+				_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)... done.").arg(map->size()).arg(sequence.size()));
+
+				viewer->update();
+			}
+
+			_progressDialog->appendText(tr("CLAMS depth calibration..."));
+			QApplication::processEvents();
+			QApplication::processEvents();
+
+			QDialog * dialog = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
+			dialog->setAttribute(Qt::WA_DeleteOnClose, true);
+			dialog->setWindowTitle(tr("Original/Map"));
+			dialog->setMinimumWidth(_imageSize.width);
+			ImageView * imageView1 = new ImageView(dialog);
+			imageView1->setMinimumSize(320, 240);
+			ImageView * imageView2 = new ImageView(dialog);
+			imageView2->setMinimumSize(320, 240);
+			QVBoxLayout * vlayout = new QVBoxLayout();
+			vlayout->setContentsMargins(0,0,0,0);
+			vlayout->addWidget(imageView1, 1);
+			vlayout->addWidget(imageView2, 1);
+			dialog->setLayout(vlayout);
+			if(ULogger::level() == ULogger::kDebug)
+			{
+				dialog->show();
+			}
+
+			//clams::DiscreteDepthDistortionModel model = clams::calibrate(sequence, poses, map);
+			const cv::Size & imageSize = _imageSize;
+			if(_model == 0)
+			{
+				size_t bin_width = _ui->spinBox_bin_width->value();
+				size_t bin_height = _ui->spinBox_bin_height->value();
+				if(imageSize.width % _ui->spinBox_bin_width->value() != 0 ||
+				   imageSize.height % _ui->spinBox_bin_height->value() != 0)
+				{
+					size_t bin_width, bin_height;
+					clams::DiscreteDepthDistortionModel::getBinSize(imageSize.width, imageSize.height, bin_width, bin_height);
+					_ui->spinBox_bin_width->setValue(bin_width);
+					_ui->spinBox_bin_height->setValue(bin_height);
+				}
+				_model = new clams::DiscreteDepthDistortionModel(
+						imageSize.width,
+						imageSize.height,
+						bin_width,
+						bin_height,
+						_ui->doubleSpinBox_bin_depth->value(),
+						_ui->spinBox_smoothing->value(),
+						_ui->doubleSpinBox_maxDepthModel->value());
+			}
+			UASSERT(_model->getWidth() == imageSize.width && _model->getHeight() == imageSize.height);
+
+			// -- For all selected frames, accumulate training examples
+			//    in the distortion model.
+			size_t counts;
+			index = 0;
+			for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter != poses.end() && !_canceled; ++iter)
+			{
+			  size_t idx = iter->first;
+			  std::map<int, rtabmap::SensorData>::const_iterator ster = sequence.find(idx);
+			  if(ster!=sequence.end())
+			  {
+				  cv::Mat depthImage;
+				  ster->second.uncompressDataConst(0, &depthImage);
+
+				  if(ster->second.cameraModels().size() == 1 && ster->second.cameraModels()[0].isValidForProjection() && !depthImage.empty())
+				  {
+					  cv::Mat mapDepth;
+					  CameraModel model = ster->second.cameraModels()[0];
+					  if(model.imageWidth() != depthImage.cols)
+					  {
+						  UASSERT_MSG(model.imageHeight() % depthImage.rows == 0, uFormat("rgb=%d depth=%d", model.imageHeight(), depthImage.rows).c_str());
+						  model = model.scaled(double(depthImage.rows) / double(model.imageHeight()));
+					  }
+					  clams::FrameProjector projector(model);
+					  mapDepth = projector.estimateMapDepth(
+							  map,
+							  iter->second.inverse(),
+							  depthImage,
+							 _ui->doubleSpinBox_coneRadius->value(),
+							 _ui->doubleSpinBox_coneStdDevThresh->value());
+
+					  if(ULogger::level() == ULogger::kDebug)
+					  {
+						  imageView1->setImage(uCvMat2QImage(depthImage));
+						  imageView2->setImage(uCvMat2QImage(mapDepth));
+					  }
+
+					  counts = _model->accumulate(mapDepth, depthImage);
+					  _progressDialog->appendText(tr("Added %1 training examples from node %2 (%3/%4).").arg(counts).arg(iter->first).arg(++index).arg(sequence.size()));
+				  }
+				 }
+			  _progressDialog->incrementStep();
+			  QApplication::processEvents();
+			}
+
+			_progressDialog->appendText(tr("CLAMS depth calibration... done!"));
+			QApplication::processEvents();
+
+			if(!_canceled)
+			{
+				this->saveModel();
+			}
+		}
+		else
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"), tr("The resulting map is empty!"));
+		}
+		_progressDialog->setValue(_progressDialog->maximumSteps());
+	}
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/EditConstraintDialog.cpp

@@ -0,0 +1,153 @@
+/*
+Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/EditConstraintDialog.h"
+#include "ui_editConstraintDialog.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <iostream>
+
+#ifndef M_PI
+#define M_PI       3.14159265358979323846
+#endif
+
+namespace rtabmap {
+
+EditConstraintDialog::EditConstraintDialog(const Transform & constraint, const cv::Mat & covariance, QWidget * parent) :
+	QDialog(parent)
+{
+	_ui = new Ui_EditConstraintDialog();
+	_ui->setupUi(this);
+
+	float x,y,z,roll,pitch,yaw;
+	constraint.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+	_ui->x->setValue(x);
+	_ui->y->setValue(y);
+	_ui->z->setValue(z);
+	_ui->roll->setValue(roll);
+	_ui->pitch->setValue(pitch);
+	_ui->yaw->setValue(yaw);
+
+	UASSERT(covariance.empty() || (covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1));
+
+	_ui->checkBox_radians->setChecked(true);
+	_ui->linear_sigma_x->setValue(covariance.empty() || covariance.at<double>(0,0)>=9999 || covariance.at<double>(0,0)<=0?0:sqrt(covariance.at<double>(0,0)));
+	_ui->linear_sigma_y->setValue(covariance.empty() || covariance.at<double>(1,1)>=9999 || covariance.at<double>(1,1)<=0?0:sqrt(covariance.at<double>(1,1)));
+	_ui->linear_sigma_z->setValue(covariance.empty() || covariance.at<double>(2,2)>=9999 || covariance.at<double>(2,2)<=0?0:sqrt(covariance.at<double>(2,2)));
+	_ui->angular_sigma_roll->setValue(covariance.empty() || covariance.at<double>(3,3)>=9999 || covariance.at<double>(3,3)<=0?0:sqrt(covariance.at<double>(3,3)));
+	_ui->angular_sigma_pitch->setValue(covariance.empty() || covariance.at<double>(4,4)>=9999 || covariance.at<double>(4,4)<=0?0:sqrt(covariance.at<double>(4,4)));
+	_ui->angular_sigma_yaw->setValue(covariance.empty() || covariance.at<double>(5,5)>=9999 || covariance.at<double>(5,5)<=0?0:sqrt(covariance.at<double>(5,5)));
+
+	connect(_ui->checkBox_radians, SIGNAL(stateChanged(int)), this, SLOT(switchUnits()));
+}
+
+EditConstraintDialog::~EditConstraintDialog()
+{
+	delete _ui;
+}
+
+void EditConstraintDialog::setPoseGroupVisible(bool visible)
+{
+	_ui->groupBox_pose->setVisible(visible);
+}
+void EditConstraintDialog::setCovarianceGroupVisible(bool visible)
+{
+	_ui->groupBox_covariance->setVisible(visible);
+}
+
+void EditConstraintDialog::switchUnits()
+{
+	double conversion = 180.0/M_PI;
+	if(_ui->checkBox_radians->isChecked())
+	{
+		conversion = M_PI/180.0;
+	}
+	QVector<QDoubleSpinBox*> boxes;
+	boxes.push_back(_ui->roll);
+	boxes.push_back(_ui->pitch);
+	boxes.push_back(_ui->yaw);
+	boxes.push_back(_ui->angular_sigma_roll);
+	boxes.push_back(_ui->angular_sigma_pitch);
+	boxes.push_back(_ui->angular_sigma_yaw);
+	for(int i=0; i<boxes.size(); ++i)
+	{
+		double value = boxes[i]->value()*conversion;
+		if(_ui->checkBox_radians->isChecked())
+		{
+			if(boxes[i]!=_ui->angular_sigma_roll && boxes[i]!=_ui->angular_sigma_pitch && boxes[i]!=_ui->angular_sigma_yaw)
+			{
+				boxes[i]->setMinimum(-M_PI);
+			}
+			boxes[i]->setMaximum(M_PI);
+			boxes[i]->setSuffix(" rad");
+			boxes[i]->setSingleStep(0.01);
+		}
+		else
+		{
+			if(boxes[i]!=_ui->angular_sigma_roll && boxes[i]!=_ui->angular_sigma_pitch && boxes[i]!=_ui->angular_sigma_yaw)
+			{
+				boxes[i]->setMinimum(-180);
+			}
+			boxes[i]->setMaximum(180);
+			boxes[i]->setSuffix(" deg");
+			boxes[i]->setSingleStep(1);
+		}
+		boxes[i]->setValue(value);
+	}
+}
+
+Transform EditConstraintDialog::getTransform() const
+{
+	double conversion = 1.0f;
+	if(!_ui->checkBox_radians->isChecked())
+	{
+		conversion = M_PI/180.0;
+	}
+	return Transform(_ui->x->value(), _ui->y->value(), _ui->z->value(), _ui->roll->value()*conversion, _ui->pitch->value()*conversion, _ui->yaw->value()*conversion);
+}
+
+cv::Mat EditConstraintDialog::getCovariance() const
+{
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
+	covariance.at<double>(0,0) = _ui->linear_sigma_x->value()==0?9999:_ui->linear_sigma_x->value()*_ui->linear_sigma_x->value();
+	covariance.at<double>(1,1) = _ui->linear_sigma_y->value()==0?9999:_ui->linear_sigma_y->value()*_ui->linear_sigma_y->value();
+	covariance.at<double>(2,2) = _ui->linear_sigma_z->value()==0?9999:_ui->linear_sigma_z->value()*_ui->linear_sigma_z->value();
+	double conversion = 1.0f;
+	if(!_ui->checkBox_radians->isChecked())
+	{
+		conversion = M_PI/180.0;
+	}
+	double sigma = _ui->angular_sigma_roll->value()*conversion;
+	covariance.at<double>(3,3) = sigma==0?9999:sigma*sigma;
+	sigma = _ui->angular_sigma_pitch->value()*conversion;
+	covariance.at<double>(4,4) = sigma==0?9999:sigma*sigma;
+	sigma = _ui->angular_sigma_yaw->value()*conversion;
+	covariance.at<double>(5,5) = sigma==0?9999:sigma*sigma;
+	return covariance;
+}
+
+}

rtabmap/guilib/src/EditDepthArea.cpp

@@ -0,0 +1,434 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <QWidget>
+#include <QPainter>
+#include <QMouseEvent>
+#include <QMenu>
+#include <QAction>
+#include <QActionGroup>
+#include <QInputDialog>
+
+#include "rtabmap/gui/EditDepthArea.h"
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+EditDepthArea::EditDepthArea(QWidget *parent)
+    : QWidget(parent)
+{
+    setAttribute(Qt::WA_StaticContents);
+    modified_ = false;
+    scribbling_ = false;
+    myPenWidth_ = 10;
+    clusterError_ = 0.02;
+
+    menu_ = new QMenu(tr(""), this);
+    showRGB_ = menu_->addAction(tr("Show RGB Image"));
+    showRGB_->setCheckable(true);
+    showRGB_->setChecked(true);
+    removeCluster_ = menu_->addAction(tr("Remove Cluster"));
+    clusterErrorCluster_ = menu_->addAction(tr("Set Cluster Error"));
+	setPenWidth_ = menu_->addAction(tr("Set Pen Width..."));
+	QMenu * colorMap = menu_->addMenu("Depth color map");
+	colorMapWhiteToBlack_ = colorMap->addAction(tr("White to black"));
+	colorMapWhiteToBlack_->setCheckable(true);
+	colorMapWhiteToBlack_->setChecked(false);
+	colorMapBlackToWhite_ = colorMap->addAction(tr("Black to white"));
+	colorMapBlackToWhite_->setCheckable(true);
+	colorMapBlackToWhite_->setChecked(false);
+	colorMapRedToBlue_ = colorMap->addAction(tr("Red to blue"));
+	colorMapRedToBlue_->setCheckable(true);
+	colorMapRedToBlue_->setChecked(true);
+	colorMapBlueToRed_ = colorMap->addAction(tr("Blue to red"));
+	colorMapBlueToRed_->setCheckable(true);
+	colorMapBlueToRed_->setChecked(false);
+	QActionGroup * group = new QActionGroup(this);
+	group->addAction(colorMapWhiteToBlack_);
+	group->addAction(colorMapBlackToWhite_);
+	group->addAction(colorMapRedToBlue_);
+	group->addAction(colorMapBlueToRed_);
+	resetChanges_ = menu_->addAction(tr("Reset Changes"));
+}
+
+void EditDepthArea::setImage(const cv::Mat &depth, const cv::Mat & rgb)
+{
+	UASSERT(!depth.empty());
+	UASSERT(depth.type() == CV_32FC1 ||
+			depth.type() == CV_16UC1);
+	originalImage_ = depth;
+
+	uCvQtDepthColorMap colorMap = uCvQtDepthWhiteToBlack;
+	if(colorMapBlackToWhite_->isChecked())
+	{
+		colorMap = uCvQtDepthBlackToWhite;
+	}
+	else if(colorMapRedToBlue_->isChecked())
+	{
+		colorMap = uCvQtDepthRedToBlue;
+	}
+	else if(colorMapBlueToRed_->isChecked())
+	{
+		colorMap = uCvQtDepthBlueToRed;
+	}
+
+	image_ = uCvMat2QImage(depth, true, colorMap).convertToFormat(QImage::Format_RGB32);
+
+	imageRGB_ = QImage();
+	if(!rgb.empty())
+	{
+		imageRGB_ = uCvMat2QImage(rgb);
+		if( depth.cols != rgb.cols ||
+			depth.rows != rgb.rows)
+		{
+			// scale rgb to depth
+			imageRGB_ = imageRGB_.scaled(image_.size());
+		}
+	}
+	showRGB_->setEnabled(!imageRGB_.isNull());
+	modified_ = false;
+	update();
+}
+
+cv::Mat EditDepthArea::getModifiedImage() const
+{
+	cv::Mat modifiedImage = originalImage_.clone();
+	if(modified_)
+	{
+		UASSERT(image_.width() == modifiedImage.cols &&
+				image_.height() == modifiedImage.rows);
+		UASSERT(modifiedImage.type() == CV_32FC1 ||
+				modifiedImage.type() == CV_16UC1);
+		for(int j=0; j<image_.height(); ++j)
+		{
+			for(int i=0; i<image_.width(); ++i)
+			{
+				if(qRed(image_.pixel(i, j)) == 0 &&
+				   qGreen(image_.pixel(i, j)) == 0 &&
+				   qBlue(image_.pixel(i, j)) == 0)
+				{
+					if(modifiedImage.type() == CV_32FC1)
+					{
+						modifiedImage.at<float>(j,i) = 0.0f;
+					}
+					else // CV_16UC1
+					{
+						modifiedImage.at<unsigned short>(j,i) = 0;
+					}
+				}
+			}
+		}
+	}
+	return modifiedImage;
+}
+
+void EditDepthArea::setPenWidth(int newWidth)
+{
+    myPenWidth_ = newWidth;
+}
+
+void EditDepthArea::resetChanges()
+{
+    image_ = uCvMat2QImage(originalImage_).convertToFormat(QImage::Format_RGB32);
+    modified_ = false;
+    update();
+}
+
+void EditDepthArea::setColorMap(uCvQtDepthColorMap type)
+{
+	if(type == uCvQtDepthBlackToWhite)
+	{
+		colorMapBlackToWhite_->setChecked(true);
+	}
+	else if(type == uCvQtDepthRedToBlue)
+	{
+		colorMapRedToBlue_->setChecked(true);
+	}
+	else if(type == uCvQtDepthBlueToRed)
+	{
+		colorMapBlueToRed_->setChecked(true);
+	}
+	else
+	{
+		colorMapWhiteToBlack_->setChecked(true);
+	}
+
+	if(!originalImage_.empty())
+	{
+		image_ = uCvMat2QImage(originalImage_, true, type).convertToFormat(QImage::Format_RGB32);
+		update();
+	}
+}
+
+void EditDepthArea::mousePressEvent(QMouseEvent *event)
+{
+    if (event->button() == Qt::LeftButton) {
+    	float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+        lastPoint_.setX((event->pos().x()-offsetX)/scale);
+        lastPoint_.setY((event->pos().y()-offsetY)/scale);
+
+        scribbling_ = true;
+    }
+}
+
+void EditDepthArea::mouseMoveEvent(QMouseEvent *event)
+{
+    if ((event->buttons() & Qt::LeftButton) && scribbling_)
+    {
+    	float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+		QPoint to;
+		to.setX((event->pos().x()-offsetX)/scale);
+		to.setY((event->pos().y()-offsetY)/scale);
+        drawLineTo(to);
+    }
+}
+
+void EditDepthArea::mouseReleaseEvent(QMouseEvent *event)
+{
+    if (event->button() == Qt::LeftButton && scribbling_) {
+    	float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+		QPoint to;
+		to.setX((event->pos().x()-offsetX)/scale);
+		to.setY((event->pos().y()-offsetY)/scale);
+		drawLineTo(to);
+        scribbling_ = false;
+    }
+}
+
+void EditDepthArea::computeScaleOffsets(const QRect & targetRect, float & scale, float & offsetX, float & offsetY) const
+{
+	scale = 1.0f;
+	offsetX = 0.0f;
+	offsetY = 0.0f;
+
+	if(!image_.isNull())
+	{
+		float w = image_.width();
+		float h = image_.height();
+		float widthRatio = float(targetRect.width()) / w;
+		float heightRatio = float(targetRect.height()) / h;
+
+		//printf("w=%f, h=%f, wR=%f, hR=%f, sW=%d, sH=%d\n", w, h, widthRatio, heightRatio, this->rect().width(), this->rect().height());
+		if(widthRatio < heightRatio)
+		{
+			scale = widthRatio;
+		}
+		else
+		{
+			scale = heightRatio;
+		}
+
+		//printf("ratio=%f\n",ratio);
+
+		w *= scale;
+		h *= scale;
+
+		if(w < targetRect.width())
+		{
+			offsetX = (targetRect.width() - w)/2.0f;
+		}
+		if(h < targetRect.height())
+		{
+			offsetY = (targetRect.height() - h)/2.0f;
+		}
+		//printf("offsetX=%f, offsetY=%f\n",offsetX, offsetY);
+	}
+}
+
+void EditDepthArea::paintEvent(QPaintEvent *event)
+{
+	//Scale
+	float ratio, offsetX, offsetY;
+	this->computeScaleOffsets(event->rect(), ratio, offsetX, offsetY);
+	QPainter painter(this);
+
+	painter.translate(offsetX, offsetY);
+	painter.scale(ratio, ratio);
+
+	if(showRGB_->isChecked() && !imageRGB_.isNull())
+	{
+		painter.setOpacity(0.5);
+		painter.drawImage(QPoint(0,0), imageRGB_);
+	}
+
+	painter.drawImage(QPoint(0,0), image_);
+}
+
+void EditDepthArea::resizeEvent(QResizeEvent *event)
+{
+    QWidget::resizeEvent(event);
+}
+
+void floodfill(QRgb * bits, const cv::Mat & depthImage, int x, int y, float lastDepthValue, float error, int &iterations)
+{
+	++iterations;
+	if(x>=0 && x<depthImage.cols &&
+	   y>=0 && y<depthImage.rows)
+	{
+		float currentValue;
+		if(depthImage.type() == CV_32FC1)
+		{
+			currentValue = depthImage.at<float>(y, x);
+		}
+		else
+		{
+			currentValue = float(depthImage.at<unsigned short>(y, x))/1000.0f;
+		}
+		if(currentValue == 0.0f)
+		{
+			return;
+		}
+
+		QRgb & rgb = bits[x+y*depthImage.cols];
+		if(qRed(rgb) == 0 && qGreen(rgb) == 0 && qBlue(rgb) == 0)
+		{
+			return;
+		}
+		if(lastDepthValue>=0.0f && fabs(lastDepthValue - currentValue) > error*lastDepthValue)
+		{
+			return;
+		}
+		rgb = 0;
+
+		if(y+1<depthImage.rows)
+		{
+			QRgb & rgb = bits[x+(y+1)*depthImage.cols];
+			if(qRed(rgb) != 0 || qGreen(rgb) != 0 || qBlue(rgb) != 0)
+			{
+				floodfill(bits, depthImage, x, y+1, currentValue, error, iterations);
+			}
+		}
+		if(y-1>=0)
+		{
+			QRgb & rgb = bits[x+(y-1)*depthImage.cols];
+			if(qRed(rgb) != 0 || qGreen(rgb) != 0 || qBlue(rgb) != 0)
+			{
+				floodfill(bits, depthImage, x, y-1, currentValue, error, iterations);
+			}
+		}
+		if(x+1<depthImage.cols)
+		{
+			QRgb & rgb = bits[x+1+y*depthImage.cols];
+			if(qRed(rgb) != 0 || qGreen(rgb) != 0 || qBlue(rgb) != 0)
+			{
+				floodfill(bits, depthImage, x+1, y, currentValue, error, iterations);
+			}
+		}
+		if(x-1>=0)
+		{
+			QRgb & rgb = bits[x-1+y*depthImage.cols];
+			if(qRed(rgb) != 0 || qGreen(rgb) != 0 || qBlue(rgb) != 0)
+			{
+				floodfill(bits, depthImage, x-1, y, currentValue, error, iterations);
+			}
+		}
+	}
+}
+
+void EditDepthArea::contextMenuEvent(QContextMenuEvent * e)
+{
+	QAction * action = menu_->exec(e->globalPos());
+	if(action == showRGB_)
+	{
+		this->update();
+	}
+	else if(action == removeCluster_)
+	{
+		float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+		QPoint pixel;
+		pixel.setX((e->pos().x()-offsetX)/scale);
+		pixel.setY((e->pos().y()-offsetY)/scale);
+		if(pixel.x()>=0 && pixel.x() < originalImage_.cols &&
+		   pixel.y()>=0 && pixel.y() < originalImage_.rows)
+		{
+			int iterations=0;
+			floodfill((QRgb*)image_.bits(), originalImage_, pixel.x(), pixel.y(), -1.0f, clusterError_, iterations);
+		}
+		modified_=true;
+		this->update();
+	}
+	else if(action == clusterErrorCluster_)
+	{
+		bool ok;
+		double error = QInputDialog::getDouble(this, tr("Set Cluster Error"), tr("Error:"), clusterError(), 0.001, 1, 3, &ok);
+		if(ok)
+		{
+			clusterError_= error;
+		}
+		modified_=true;
+	}
+	else if(action == setPenWidth_)
+	{
+		bool ok;
+		int width = QInputDialog::getInt(this, tr("Set Pen Width"), tr("Width (pixels):"), penWidth(), 1, 999, 1, &ok);
+		if(ok)
+		{
+			myPenWidth_ = width;
+		}
+	}
+	else if(action == colorMapBlackToWhite_ ||
+			action == colorMapWhiteToBlack_ ||
+			action == colorMapRedToBlue_ ||
+			action == colorMapBlueToRed_)
+	{
+		uCvQtDepthColorMap colorMap = uCvQtDepthWhiteToBlack;
+		if(colorMapBlackToWhite_->isChecked())
+		{
+			colorMap = uCvQtDepthBlackToWhite;
+		}
+		else if(colorMapRedToBlue_->isChecked())
+		{
+			colorMap = uCvQtDepthRedToBlue;
+		}
+		else if(colorMapBlueToRed_->isChecked())
+		{
+			colorMap = uCvQtDepthBlueToRed;
+		}
+		this->setColorMap(colorMap);
+	}
+	else if(action == resetChanges_)
+	{
+		this->resetChanges();
+	}
+}
+
+void EditDepthArea::drawLineTo(const QPoint &endPoint)
+{
+    QPainter painter(&image_);
+    painter.setPen(QPen(Qt::black, myPenWidth_, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
+    painter.drawLine(lastPoint_, endPoint);
+    modified_ = true;
+
+    update();
+    lastPoint_ = endPoint;
+}
+
+}

rtabmap/guilib/src/EditMapArea.cpp

@@ -0,0 +1,249 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <QWidget>
+#include <QPainter>
+#include <QMouseEvent>
+#include <QMenu>
+#include <QAction>
+#include <QActionGroup>
+#include <QInputDialog>
+
+#include "rtabmap/gui/EditMapArea.h"
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+EditMapArea::EditMapArea(QWidget *parent)
+    : QWidget(parent)
+{
+    setAttribute(Qt::WA_StaticContents);
+    modified_ = false;
+    scribbling_ = false;
+    myPenWidth_ = 3;
+
+    menu_ = new QMenu(tr(""), this);
+	setPenWidth_ = menu_->addAction(tr("Set Pen Width..."));
+	addObstacle_ = menu_->addAction(tr("Add Obstacle"));
+	addObstacle_->setCheckable(true);
+	addObstacle_->setChecked(true);
+	clearObstacle_ = menu_->addAction(tr("Clear Obstacle"));
+	clearObstacle_->setCheckable(true);
+	clearObstacle_->setChecked(false);
+	setUnknown_ = menu_->addAction(tr("Set Unknown"));
+	setUnknown_->setCheckable(true);
+	setUnknown_->setChecked(false);
+	QActionGroup * group = new QActionGroup(this);
+	group->addAction(addObstacle_);
+	group->addAction(clearObstacle_);
+	group->addAction(setUnknown_);
+	resetChanges_ = menu_->addAction(tr("Reset Changes"));
+}
+
+void EditMapArea::setMap(const cv::Mat &map)
+{
+	UASSERT(!map.empty());
+	UASSERT(map.type() == CV_8UC1);
+	originalMap_ = map;
+
+	map_ = uCvMat2QImage(map, true).convertToFormat(QImage::Format_RGB32);
+
+	modified_ = false;
+	update();
+}
+
+cv::Mat EditMapArea::getModifiedMap() const
+{
+	cv::Mat modifiedMap = originalMap_.clone();
+	if(modified_)
+	{
+		UASSERT(map_.width() == modifiedMap.cols &&
+				map_.height() == modifiedMap.rows);
+		UASSERT(modifiedMap.type() == CV_8UC1);
+		for(int j=0; j<map_.height(); ++j)
+		{
+			for(int i=0; i<map_.width(); ++i)
+			{
+				modifiedMap.at<unsigned char>(j,i) = qRed(map_.pixel(i, j));
+			}
+		}
+	}
+	return modifiedMap;
+}
+
+void EditMapArea::setPenWidth(int newWidth)
+{
+    myPenWidth_ = newWidth;
+}
+
+void EditMapArea::resetChanges()
+{
+    map_ = uCvMat2QImage(originalMap_).convertToFormat(QImage::Format_RGB32);
+    modified_ = false;
+    update();
+}
+
+void EditMapArea::mousePressEvent(QMouseEvent *event)
+{
+    if (event->button() == Qt::LeftButton) {
+    	float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+        lastPoint_.setX((event->pos().x()-offsetX)/scale);
+        lastPoint_.setY((event->pos().y()-offsetY)/scale);
+
+        scribbling_ = true;
+    }
+}
+
+void EditMapArea::mouseMoveEvent(QMouseEvent *event)
+{
+    if ((event->buttons() & Qt::LeftButton) && scribbling_)
+    {
+    	float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+		QPoint to;
+		to.setX((event->pos().x()-offsetX)/scale);
+		to.setY((event->pos().y()-offsetY)/scale);
+        drawLineTo(to);
+    }
+}
+
+void EditMapArea::mouseReleaseEvent(QMouseEvent *event)
+{
+    if (event->button() == Qt::LeftButton && scribbling_) {
+    	float scale, offsetX, offsetY;
+		computeScaleOffsets(rect(), scale, offsetX, offsetY);
+		QPoint to;
+		to.setX((event->pos().x()-offsetX)/scale);
+		to.setY((event->pos().y()-offsetY)/scale);
+		drawLineTo(to);
+        scribbling_ = false;
+    }
+}
+
+void EditMapArea::computeScaleOffsets(const QRect & targetRect, float & scale, float & offsetX, float & offsetY) const
+{
+	scale = 1.0f;
+	offsetX = 0.0f;
+	offsetY = 0.0f;
+
+	if(!map_.isNull())
+	{
+		float w = map_.width();
+		float h = map_.height();
+		float widthRatio = float(targetRect.width()) / w;
+		float heightRatio = float(targetRect.height()) / h;
+
+		//printf("w=%f, h=%f, wR=%f, hR=%f, sW=%d, sH=%d\n", w, h, widthRatio, heightRatio, this->rect().width(), this->rect().height());
+		if(widthRatio < heightRatio)
+		{
+			scale = widthRatio;
+		}
+		else
+		{
+			scale = heightRatio;
+		}
+
+		//printf("ratio=%f\n",ratio);
+
+		w *= scale;
+		h *= scale;
+
+		if(w < targetRect.width())
+		{
+			offsetX = (targetRect.width() - w)/2.0f;
+		}
+		if(h < targetRect.height())
+		{
+			offsetY = (targetRect.height() - h)/2.0f;
+		}
+		//printf("offsetX=%f, offsetY=%f\n",offsetX, offsetY);
+	}
+}
+
+void EditMapArea::paintEvent(QPaintEvent *event)
+{
+	//Scale
+	float ratio, offsetX, offsetY;
+	this->computeScaleOffsets(event->rect(), ratio, offsetX, offsetY);
+	QPainter painter(this);
+
+	painter.translate(offsetX, offsetY);
+	painter.scale(ratio, ratio);
+	painter.drawImage(QPoint(0,0), map_);
+}
+
+void EditMapArea::resizeEvent(QResizeEvent *event)
+{
+    QWidget::resizeEvent(event);
+}
+
+void EditMapArea::contextMenuEvent(QContextMenuEvent * e)
+{
+	QAction * action = menu_->exec(e->globalPos());
+	if(action == setPenWidth_)
+	{
+		bool ok;
+		int width = QInputDialog::getInt(this, tr("Set Pen Width"), tr("Width:"), penWidth(), 1, 99, 1, &ok);
+		if(ok)
+		{
+			myPenWidth_ = width;
+		}
+	}
+	else if(action == resetChanges_)
+	{
+		this->resetChanges();
+	}
+}
+
+void EditMapArea::drawLineTo(const QPoint &endPoint)
+{
+    QPainter painter(&map_);
+    QColor color;
+
+    //base on util3d::convertMap2Image8U();
+    if(addObstacle_->isChecked())
+    {
+    	color.setRgb(0,0,0);
+    }
+    else if(clearObstacle_->isChecked())
+    {
+    	color.setRgb(178,178,178);
+    }
+    else //unknown
+    {
+    	color.setRgb(89,89,89);
+    }
+    painter.setPen(QPen(color, myPenWidth_, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
+    painter.drawLine(lastPoint_, endPoint);
+    modified_ = true;
+
+    update();
+    lastPoint_ = endPoint;
+}
+
+}

rtabmap/guilib/src/ExportBundlerDialog.cpp

@@ -0,0 +1,578 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/ExportBundlerDialog.h"
+#include "ui_exportBundlerDialog.h"
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/core/Optimizer.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <QFileDialog>
+#include <QPushButton>
+#include <QMessageBox>
+#include <QTextStream>
+
+namespace rtabmap {
+
+ExportBundlerDialog::ExportBundlerDialog(QWidget * parent) :
+	QDialog(parent)
+{
+	_ui = new Ui_ExportBundlerDialog();
+	_ui->setupUi(this);
+
+	connect(_ui->toolButton_path, SIGNAL(clicked()), this, SLOT(getPath()));
+
+	restoreDefaults();
+	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+
+	connect(_ui->doubleSpinBox_laplacianVariance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_linearSpeed, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_angularSpeed, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->groupBox_export_points, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
+	connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateVisibility()));
+	connect(_ui->sba_rematchFeatures, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+
+	if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA) && !Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		_ui->groupBox_export_points->setEnabled(false);
+		_ui->groupBox_export_points->setChecked(false);
+	}
+	else if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+	{
+		_ui->comboBox_sbaType->setItemData(1, 0, Qt::UserRole - 1);
+		_ui->comboBox_sbaType->setCurrentIndex(0);
+	}
+	else if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		_ui->comboBox_sbaType->setItemData(0, 0, Qt::UserRole - 1);
+		_ui->comboBox_sbaType->setCurrentIndex(1);
+	}
+
+	_ui->lineEdit_path->setText(QDir::currentPath());
+
+	updateVisibility();
+}
+
+ExportBundlerDialog::~ExportBundlerDialog()
+{
+	delete _ui;
+}
+
+void ExportBundlerDialog::saveSettings(QSettings & settings, const QString & group) const
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	settings.setValue("maxLinearSpeed", _ui->doubleSpinBox_linearSpeed->value());
+	settings.setValue("maxAngularSpeed", _ui->doubleSpinBox_angularSpeed->value());
+	settings.setValue("laplacianThr", _ui->doubleSpinBox_laplacianVariance->value());
+	settings.setValue("exportPoints", _ui->groupBox_export_points->isChecked());
+	settings.setValue("sba_iterations", _ui->sba_iterations->value());
+	settings.setValue("sba_type", _ui->comboBox_sbaType->currentIndex());
+	settings.setValue("sba_variance", _ui->sba_variance->value());
+	settings.setValue("sba_rematch_features", _ui->sba_rematchFeatures->isChecked());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void ExportBundlerDialog::loadSettings(QSettings & settings, const QString & group)
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	_ui->doubleSpinBox_linearSpeed->setValue(settings.value("maxLinearSpeed", _ui->doubleSpinBox_linearSpeed->value()).toDouble());
+	_ui->doubleSpinBox_angularSpeed->setValue(settings.value("maxAngularSpeed", _ui->doubleSpinBox_angularSpeed->value()).toDouble());
+	_ui->doubleSpinBox_laplacianVariance->setValue(settings.value("laplacianThr", _ui->doubleSpinBox_laplacianVariance->value()).toDouble());
+	_ui->groupBox_export_points->setChecked(settings.value("exportPoints", _ui->groupBox_export_points->isChecked()).toBool());
+	_ui->sba_iterations->setValue(settings.value("sba_iterations", _ui->sba_iterations->value()).toInt());
+	_ui->comboBox_sbaType->setCurrentIndex((Optimizer::Type)settings.value("sba_type", _ui->comboBox_sbaType->currentIndex()).toInt());
+	_ui->sba_variance->setValue(settings.value("sba_variance", _ui->sba_variance->value()).toDouble());
+	_ui->sba_rematchFeatures->setChecked(settings.value("sba_rematch_features", _ui->sba_rematchFeatures->isChecked()).toBool());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void ExportBundlerDialog::setWorkingDirectory(const QString & path)
+{
+	_ui->lineEdit_path->setText((path.isEmpty()?QDir::currentPath():path) + "/bundler");
+}
+
+void ExportBundlerDialog::restoreDefaults()
+{
+	_ui->doubleSpinBox_linearSpeed->setValue(0);
+	_ui->doubleSpinBox_angularSpeed->setValue(0);
+	_ui->doubleSpinBox_laplacianVariance->setValue(0);
+	_ui->groupBox_export_points->setChecked(false);
+	_ui->sba_iterations->setValue(20);
+	if(Optimizer::isAvailable(Optimizer::kTypeG2O) || !Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(0);
+	}
+	else
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(1);
+	}
+
+	_ui->sba_variance->setValue(1.0);
+	_ui->sba_rematchFeatures->setChecked(true);
+}
+
+void ExportBundlerDialog::updateVisibility()
+{
+	_ui->sba_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
+	_ui->label_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
+}
+
+void ExportBundlerDialog::getPath()
+{
+	QString path = QFileDialog::getExistingDirectory(this, tr("Exporting cameras in Bundler format..."), _ui->lineEdit_path->text());
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_path->setText(path);
+	}
+}
+
+void ExportBundlerDialog::exportBundler(
+		std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const QMap<int, Signature> & signatures,
+		const ParametersMap & parameters)
+{
+	if(this->exec() != QDialog::Accepted)
+	{
+		return;
+	}
+	QString path = _ui->lineEdit_path->text();
+	if(!path.isEmpty())
+	{
+		if(!QDir(path).mkpath("."))
+		{
+			QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed creating directory %1.").arg(path));
+			return;
+		}
+
+		std::map<int, cv::Point3f> points3DMap;
+		std::map<int, std::map<int, FeatureBA> > wordReferences;
+		if(_ui->groupBox_export_points->isEnabled() && _ui->groupBox_export_points->isChecked())
+		{
+			std::map<int, Transform> posesOut;
+			std::multimap<int, Link> linksOut;
+			Optimizer::Type sbaType = _ui->comboBox_sbaType->currentIndex()==0?Optimizer::kTypeG2O:Optimizer::kTypeCVSBA;
+			UASSERT(Optimizer::isAvailable(sbaType));
+			ParametersMap parametersSBA = parameters;
+			uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(_ui->sba_iterations->value())));
+			uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(_ui->sba_variance->value())));
+			std::shared_ptr<Optimizer> sba(Optimizer::create(sbaType, parametersSBA));
+			sba->getConnectedGraph(poses.begin()->first, poses, links, posesOut, linksOut);
+			// set input poses as initial optimization guess
+			for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
+			{
+				iter->second = poses.at(iter->first);
+			}
+			if(_ui->sba_iterations->value() > 0)
+			{
+				UINFO("Do BA optimization with %d iterations.", _ui->sba_iterations->value());
+				poses = sba->optimizeBA(
+						posesOut.begin()->first,
+						posesOut,
+						linksOut,
+						signatures.toStdMap(),
+						points3DMap,
+						wordReferences,
+						_ui->sba_rematchFeatures->isChecked(),
+						parametersSBA);
+			}
+			else
+			{
+				UINFO("Do not optimize, just compute 3D features and word correspondences.");
+				poses = posesOut;
+				sba->computeBACorrespondences(poses,
+					linksOut, 
+					signatures.toStdMap(),
+					points3DMap,
+					wordReferences,
+					_ui->sba_rematchFeatures->isChecked(),
+					false,
+					parametersSBA);
+			}
+
+			if(poses.empty())
+			{
+				QMessageBox::warning(this, tr("Exporting cameras..."), tr("SBA optimization failed! Cannot export with 3D points.").arg(path));
+				return;
+			}
+		}
+
+		// export cameras and images
+		QFile fileOut(path+QDir::separator()+"cameras.out");
+		QFile fileList(path+QDir::separator()+"list.txt");
+		QFile fileListKeys(path+QDir::separator()+"list_keys.txt");
+		QDir(path).mkdir("images");
+		if(wordReferences.size())
+		{
+			QDir(path).mkdir("keys");
+		}
+		if(fileOut.open(QIODevice::WriteOnly | QIODevice::Text))
+		{
+			if(fileList.open(QIODevice::WriteOnly | QIODevice::Text))
+			{
+				std::map<int, Transform> cameras;
+				std::map<int, int> cameraIndexes;
+				int camIndex = 0;
+				std::map<int, QColor> colors;
+				for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					QMap<int, Signature>::const_iterator ster = signatures.find(iter->first);
+					if(ster!= signatures.end())
+					{
+						cv::Mat image = ster.value().sensorData().imageRaw();
+						if(image.empty())
+						{
+							ster.value().sensorData().uncompressDataConst(&image, 0, 0, 0);
+						}
+
+						double maxLinearVel = _ui->doubleSpinBox_linearSpeed->value();
+						double maxAngularVel = _ui->doubleSpinBox_angularSpeed->value();
+						double laplacianThr = _ui->doubleSpinBox_laplacianVariance->value();
+						bool blurryImage = false;
+						const std::vector<float> & velocity = ster.value().getVelocity();
+						if(maxLinearVel>0.0 || maxAngularVel>0.0)
+						{
+							if(velocity.size() == 6)
+							{
+								float transVel = uMax3(fabs(velocity[0]), fabs(velocity[1]), fabs(velocity[2]));
+								float rotVel = uMax3(fabs(velocity[3]), fabs(velocity[4]), fabs(velocity[5]));
+								if(maxLinearVel>0.0 && transVel > maxLinearVel)
+								{
+									UWARN("Fast motion detected for camera %d (speed=%f m/s > thr=%f m/s), camera is ignored for texturing.", iter->first, transVel, maxLinearVel);
+									blurryImage = true;
+								}
+								else if(maxAngularVel>0.0 && rotVel > maxAngularVel)
+								{
+									UWARN("Fast motion detected for camera %d (speed=%f rad/s > thr=%f rad/s), camera is ignored for texturing.", iter->first, rotVel, maxAngularVel);
+									blurryImage = true;
+								}
+							}
+							else
+							{
+								UWARN("Camera motion filtering is set, but velocity of camera %d is not available.", iter->first);
+							}
+						}
+
+						if(!blurryImage && !image.empty() && laplacianThr>0.0)
+						{
+							cv::Mat imgLaplacian;
+							cv::Laplacian(image, imgLaplacian, CV_16S);
+							cv::Mat m, s;
+							cv::meanStdDev(imgLaplacian, m, s);
+							double stddev_pxl = s.at<double>(0);
+							double var = stddev_pxl*stddev_pxl;
+							if(var < laplacianThr)
+							{
+								blurryImage = true;
+								UWARN("Camera's image %d is detected as blurry (var=%f < thr=%f), camera is ignored for texturing.", iter->first, var, laplacianThr);
+							}
+						}
+						if(!blurryImage)
+						{
+							cameras.insert(*iter);
+							cameraIndexes.insert(std::make_pair(iter->first, camIndex++));
+							QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
+							p = path+QDir::separator()+p;
+							if(cv::imwrite(p.toStdString(), image))
+							{
+								UINFO("saved image %s", p.toStdString().c_str());
+							}
+							else
+							{
+								UERROR("Failed to save image %s", p.toStdString().c_str());
+							}
+
+							//
+							// Descriptors
+							//
+							// The file format starts with 2 integers giving the total number of
+							// keypoints and the length of the descriptor vector for each keypoint
+							// (128). Then the location of each keypoint in the image is specified by
+							// 4 floating point numbers giving subpixel row and column location,
+							// scale, and orientation (in radians from -PI to PI).  Obviously, these
+							// numbers are not invariant to viewpoint, but can be used in later
+							// stages of processing to check for geometric consistency among matches.
+							// Finally, the invariant descriptor vector for the keypoint is given as
+							// a list of 128 integers in range [0,255].  Keypoints from a new image
+							// can be matched to those from previous images by simply looking for the
+							// descriptor vector with closest Euclidean distance among all vectors
+							// from previous images.
+							//
+							if(wordReferences.size())
+							{
+								std::list<FeatureBA> descriptors;
+								for(std::map<int, std::map<int, FeatureBA> >::iterator jter=wordReferences.begin(); jter!=wordReferences.end(); ++jter)
+								{
+									for(std::map<int, FeatureBA>::iterator kter=jter->second.begin(); kter!=jter->second.end(); ++kter)
+									{
+										if(kter->first == iter->first)
+										{
+											if(!kter->second.descriptor.empty())
+											{
+												descriptors.push_back(kter->second);
+											}
+
+											if(colors.find(jter->first) == colors.end())
+											{
+												if(!image.empty() &&
+												   kter->second.kpt.pt.x >= 0.0f && (int)kter->second.kpt.pt.x < image.cols &&
+												   kter->second.kpt.pt.y >= 0.0f && (int)kter->second.kpt.pt.y < image.rows)
+												{
+													UASSERT(image.type() == CV_8UC3 || image.type() == CV_8UC1);
+													QColor c;
+													if(image.channels() == 3)
+													{
+														cv::Vec3b & pixel = image.at<cv::Vec3b>((int)kter->second.kpt.pt.y, (int)kter->second.kpt.pt.x);
+														c.setRgb(pixel[2], pixel[1], pixel[0]);
+													}
+													else // grayscale
+													{
+														unsigned char & pixel = image.at<unsigned char>((int)kter->second.kpt.pt.y, (int)kter->second.kpt.pt.x);
+														c.setRgb(pixel, pixel, pixel);
+													}
+													colors.insert(std::make_pair(jter->first, c));
+												}
+											}
+										}
+									}
+								}
+
+								QString p = QString("keys")+QDir::separator()+tr("%1.key").arg(iter->first);
+								p = path+QDir::separator()+p;
+								QFile fileKey(p);
+								if(fileKey.open(QIODevice::WriteOnly | QIODevice::Text))
+								{
+									if(descriptors.size())
+									{
+										QTextStream key(&fileKey);
+										key << descriptors.size() << " " << descriptors.front().descriptor.cols << "\n";
+										for(std::list<FeatureBA>::iterator dter=descriptors.begin(); dter!=descriptors.end(); ++dter)
+										{
+											// unpack octave value to get the scale set by SIFT (https://github.com/opencv/opencv/issues/4554)
+											int octave = dter->kpt.octave & 255;
+											octave = octave < 128 ? octave : (-128 | octave);
+											float scale = octave >= 0 ? 1.f/(1 << octave) : (float)(1 << -octave);
+
+											key << dter->kpt.pt.x << " " << dter->kpt.pt.y << " " << scale << " " << dter->kpt.angle << "\n";
+											for(int i=0; i<dter->descriptor.cols; ++i)
+											{
+												if(dter->descriptor.type() == CV_8U)
+												{
+													key << " " << (int)dter->descriptor.at<unsigned char>(i);
+												}
+												else // assume CV_32F
+												{
+													key << " " << (int)dter->descriptor.at<float>(i);
+												}
+												if((i+1)%20 == 0 && i+1 < dter->descriptor.cols)
+												{
+													key << "\n";
+												}
+											}
+											key << "\n";
+										}
+									}
+									else
+									{
+										UWARN("No descriptors saved for frame %d in file %s. "
+												"Descriptors may not have been saved in the nodes. "
+												"Verify that parameter %s was true during mapping.",
+												iter->first, p.toStdString().c_str(),
+												Parameters::kMemRawDescriptorsKept().c_str());
+									}
+									fileKey.close();
+								}
+							}
+						}
+					}
+					else
+					{
+						UWARN("Could not find node data for pose %d", iter->first);
+					}
+				}
+
+				static const Transform opengl_world_T_rtabmap_world(
+						 0.0f, -1.0f, 0.0f, 0.0f,
+						 0.0f,  0.0f, 1.0f, 0.0f,
+						-1.0f,  0.0f, 0.0f, 0.0f);
+
+				static const Transform optical_rotation_inv(
+						 0.0f, -1.0f,  0.0f, 0.0f,
+						 0.0f,  0.0f, -1.0f, 0.0f,
+						 1.0f,  0.0f,  0.0f, 0.0f);
+
+				QTextStream out(&fileOut);
+				QTextStream list(&fileList);
+				out << "# Bundle file v0.3\n";
+				out << cameras.size() << " " << points3DMap.size() << "\n";
+
+				//
+				//  Each camera entry <cameraI> contains the estimated camera intrinsics and extrinsics, and has the form:
+				//
+				//	<f> <k1> <k2>   [the focal length, followed by two radial distortion coeffs]
+				//	<R>             [a 3x3 matrix representing the camera rotation]
+				//	<t>             [a 3-vector describing the camera translation]
+				//
+				//  The cameras are specified in the order they appear in the list of images.
+				//
+				for(std::map<int, Transform>::iterator iter=cameras.begin(); iter!=cameras.end(); ++iter)
+				{
+					QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
+					list << p << "\n";
+
+					Transform localTransform;
+					QMap<int, Signature>::const_iterator ster = signatures.find(iter->first);
+					UASSERT(ster!=signatures.end());
+					if(ster.value().sensorData().cameraModels().size())
+					{
+						out << ster.value().sensorData().cameraModels().at(0).fx() << " 0 0\n";
+						localTransform = ster.value().sensorData().cameraModels().at(0).localTransform();
+					}
+					else if(ster.value().sensorData().stereoCameraModels().size())
+					{
+						out << ster.value().sensorData().stereoCameraModels()[0].left().fx() << " 0 0\n";
+						localTransform = ster.value().sensorData().stereoCameraModels()[0].left().localTransform();
+					}
+
+					Transform pose = iter->second;
+					if(!localTransform.isNull())
+					{
+						pose*=localTransform*optical_rotation_inv;
+					}
+					Transform poseGL = opengl_world_T_rtabmap_world*pose.inverse();
+
+					out << poseGL.r11() << " " << poseGL.r12() << " " << poseGL.r13() << "\n";
+					out << poseGL.r21() << " " << poseGL.r22() << " " << poseGL.r23() << "\n";
+					out << poseGL.r31() << " " << poseGL.r32() << " " << poseGL.r33() << "\n";
+					out << poseGL.x()   << " " << poseGL.y()   << " " << poseGL.z()   << "\n";
+				}
+				if(wordReferences.size())
+				{
+					if(fileListKeys.open(QIODevice::WriteOnly | QIODevice::Text))
+					{
+						QTextStream listKeys(&fileListKeys);
+						for(std::map<int, Transform>::iterator iter=cameras.begin(); iter!=cameras.end(); ++iter)
+						{
+							QString p = QString("keys")+QDir::separator()+tr("%1.key").arg(iter->first);
+							listKeys << p << "\n";
+						}
+						fileListKeys.close();
+					}
+				}
+
+				//
+				//  Each point entry has the form:
+				//
+				//		<position>      [a 3-vector describing the 3D position of the point]
+				//		<color>         [a 3-vector describing the RGB color of the point]
+				//		<view list>     [a list of views the point is visible in]
+				//
+				//	The view list begins with the length of the list (i.e., the number of cameras
+				//	the point is visible in). The list is then given as a list of quadruplets
+				//	<camera> <key> <x> <y>, where <camera> is a camera index, <key> the index
+				//	of the SIFT keypoint where the point was detected in that camera, and <x>
+				//	and <y> are the detected positions of that keypoint. Both indices are
+				//	0-based (e.g., if camera 0 appears in the list, this corresponds to the
+				//	first camera in the scene file and the first image in "list.txt"). The
+				//	pixel positions are floating point numbers in a coordinate system where
+				//	the origin is the center of the image, the x-axis increases to the right,
+				//	and the y-axis increases towards the top of the image. Thus, (-w/2, -h/2)
+				//	is the lower-left corner of the image, and (w/2, h/2) is the top-right
+				//	corner (where w and h are the width and height of the image).
+				//
+				std::map<int, int> descriptorIndexes;
+				for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
+				{
+					std::map<int, std::map<int, FeatureBA> >::iterator jter = wordReferences.find(iter->first);
+					out << iter->second.x << " " << iter->second.y << " " << iter->second.z << "\n";
+					UASSERT(colors.find(iter->first) != colors.end());
+					QColor & c = colors.at(iter->first);
+					out << c.red() << " " << c.green() << " " << c.blue() << "\n";
+					out << jter->second.size();
+					for(std::map<int, FeatureBA>::iterator kter = jter->second.begin(); kter!=jter->second.end(); ++kter)
+					{
+						// <camera> <key> <x> <y>
+						int camId = kter->first;
+						UASSERT(signatures.contains(camId));
+						UASSERT(cameraIndexes.find(camId) != cameraIndexes.end());
+						const Signature & s = signatures[camId];
+						cv::Point2f pt;
+						if(signatures[camId].sensorData().cameraModels().size())
+						{
+							pt.x = kter->second.kpt.pt.x - s.sensorData().cameraModels().at(0).cx();
+							pt.y = kter->second.kpt.pt.y - s.sensorData().cameraModels().at(0).cy();
+						}
+						else if(signatures[camId].sensorData().stereoCameraModels().size())
+						{
+							pt.x = kter->second.kpt.pt.x - s.sensorData().stereoCameraModels()[0].left().cx();
+							pt.y = kter->second.kpt.pt.y - s.sensorData().stereoCameraModels()[0].left().cy();
+						}
+						descriptorIndexes.insert(std::make_pair(camId, 0));
+						out << " " << cameraIndexes.at(camId) << " " << descriptorIndexes.at(camId)++ << " " << pt.x << " " << -pt.y;
+					}
+					out << "\n";
+				}
+
+				fileList.close();
+				fileOut.close();
+
+				QMessageBox::information(this,
+						tr("Exporting cameras in Bundler format..."),
+						tr("%1 cameras/images and %2 points exported to directory \"%3\".%4")
+						.arg(poses.size())
+						.arg(points3DMap.size())
+						.arg(path)
+						.arg(poses.size()>cameras.size()?tr(" %1/%2 cameras ignored for too fast motion and/or blur level.").arg(poses.size()-cameras.size()).arg(poses.size()):""));
+			}
+			else
+			{
+				fileOut.close();
+				QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"list.txt"));
+			}
+		}
+		else
+		{
+			QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"cameras.out"));
+		}
+	}
+}
+
+}

rtabmap/guilib/src/ExportDialog.cpp

@@ -0,0 +1,170 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/ExportDialog.h"
+#include "ui_exportDialog.h"
+
+#include <QFileDialog>
+#include <QPushButton>
+
+namespace rtabmap {
+
+ExportDialog::ExportDialog(QWidget * parent) :
+	QDialog(parent)
+{
+	_ui = new Ui_ExportDialog();
+	_ui->setupUi(this);
+
+	connect(_ui->toolButton_path, SIGNAL(clicked()), this, SLOT(getPath()));
+
+	restoreDefaults();
+	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+
+	connect(_ui->spinBox_ignored, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_framerate, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_session, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_rgb, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_depth, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_depth2d, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_odom, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_userData, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+
+	_ui->lineEdit_path->setText(QDir::currentPath()+QDir::separator()+"output.db");
+}
+
+ExportDialog::~ExportDialog()
+{
+	delete _ui;
+}
+
+void ExportDialog::saveSettings(QSettings & settings, const QString & group) const
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	settings.setValue("framesIgnored", this->framesIgnored());
+	settings.setValue("targetFramerate", this->targetFramerate());
+	settings.setValue("sessionExported", this->sessionExported());
+	settings.setValue("rgbExported", this->isRgbExported());
+	settings.setValue("depthExported", this->isDepthExported());
+	settings.setValue("depth2dExported", this->isDepth2dExported());
+	settings.setValue("odomExported", this->isOdomExported());
+	settings.setValue("userDataExported", this->isUserDataExported());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void ExportDialog::loadSettings(QSettings & settings, const QString & group)
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	_ui->spinBox_ignored->setValue(settings.value("framesIgnored", this->framesIgnored()).toInt());
+	_ui->doubleSpinBox_framerate->setValue(settings.value("targetFramerate", this->targetFramerate()).toDouble());
+	_ui->spinBox_session->setValue(settings.value("sessionExported", this->sessionExported()).toInt());
+	_ui->checkBox_rgb->setChecked(settings.value("rgbExported", this->isRgbExported()).toBool());
+	_ui->checkBox_depth->setChecked(settings.value("depthExported", this->isDepthExported()).toBool());
+	_ui->checkBox_depth2d->setChecked(settings.value("depth2dExported", this->isDepth2dExported()).toBool());
+	_ui->checkBox_odom->setChecked(settings.value("odomExported", this->isOdomExported()).toBool());
+	_ui->checkBox_userData->setChecked(settings.value("userDataExported", this->isUserDataExported()).toBool());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void ExportDialog::restoreDefaults()
+{
+	_ui->spinBox_ignored->setValue(0);
+	_ui->doubleSpinBox_framerate->setValue(0);
+	_ui->spinBox_session->setValue(-1);
+	_ui->checkBox_rgb->setChecked(true);
+	_ui->checkBox_depth->setChecked(true);
+	_ui->checkBox_depth2d->setChecked(true);
+	_ui->checkBox_odom->setChecked(true);
+	_ui->checkBox_userData->setChecked(false);
+}
+
+void ExportDialog::getPath()
+{
+	QString path = QFileDialog::getSaveFileName(this, tr("Output database path..."), _ui->lineEdit_path->text(), tr("RTAB-Map database (*.db)"));
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_path->setText(path);
+	}
+}
+
+QString ExportDialog::outputPath() const
+{
+	return _ui->lineEdit_path->text();
+}
+
+int ExportDialog::framesIgnored() const
+{
+	return _ui->spinBox_ignored->value();
+}
+
+double ExportDialog::targetFramerate() const
+{
+	return _ui->doubleSpinBox_framerate->value();
+}
+
+int ExportDialog::sessionExported() const
+{
+	return _ui->spinBox_session->value();
+}
+
+bool ExportDialog::isRgbExported() const
+{
+	return _ui->checkBox_rgb->isChecked();
+}
+
+bool ExportDialog::isDepthExported() const
+{
+	return _ui->checkBox_depth->isChecked();
+}
+
+bool ExportDialog::isDepth2dExported() const
+{
+	return _ui->checkBox_depth2d->isChecked();
+}
+
+bool ExportDialog::isOdomExported() const
+{
+	return _ui->checkBox_odom->isChecked();
+}
+
+bool ExportDialog::isUserDataExported() const
+{
+	return _ui->checkBox_userData->isChecked();
+}
+
+}

rtabmap/guilib/src/GuiLib.qrc

@@ -0,0 +1,50 @@
+<RCC>
+    <qresource prefix="/">
+        <file>images/Stop1NormalYellow.png</file>
+        <file>images/PauseNormalRed.png</file>
+        <file>images/PauseNormal.png</file>
+        <file>images/Play1Normal.png</file>
+        <file>images/Pause.ico</file>
+        <file>images/PauseOnLoop.ico</file>
+        <file>images/PauseOnLocalLoop.ico</file>
+        <file>images/PauseLoopRejected.ico</file>
+        <file>qss/default.qss</file>
+        <file>images/Plot16.png</file>
+        <file>images/Plot48.png</file>
+        <file>images/RTAB-Map.ico</file>
+        <file>images/RTAB-Map.png</file>
+        <file>images/IntRoLab.png</file>
+        <file>images/IntRoLabSmall.png</file>
+        <file>images/metal_7280826_512.jpg</file>
+        <file>images/crosshatch_metal_grille_9280154_150.JPG</file>
+        <file>images/mag_glass.png</file>
+        <file>images/document-open.png</file>
+        <file>images/document-new.png</file>
+        <file>images/document-save.png</file>
+        <file>images/document-properties.png</file>
+        <file>images/view-refresh.png</file>
+        <file>images/system-log-out.png</file>
+        <file>images/kinect_xbox_360.png</file>
+        <file>images/kinect_xbox_one.png</file>
+        <file>images/k4a.png</file>
+        <file>images/sense.png</file>
+        <file>images/xtion_pro_live.png</file>
+        <file>images/bumblebee2.png</file>
+        <file>images/webcam.png</file>
+        <file>images/zed.png</file>
+        <file>images/r200.png</file>
+        <file>images/zr300.png</file>
+        <file>images/d435.png</file>
+        <file>images/d415.png</file>
+        <file>images/tara.png</file>
+        <file>images/t265.png</file>
+        <file>images/sr300.png</file>
+        <file>images/mynteyes.png</file>
+        <file>images/l515.png</file>
+        <file>images/oakd.png</file>
+        <file>images/oakd_lite.png</file>
+        <file>images/astra.png</file>
+        <file>images/oakdpro.png</file>
+        <file>images/seer_sense_DS80.png</file>
+    </qresource>
+</RCC>

rtabmap/guilib/src/KeypointItem.cpp

@@ -0,0 +1,163 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/KeypointItem.h"
+
+#include <QtGui/QPen>
+#include <QtGui/QBrush>
+#include <QGraphicsScene>
+#include "rtabmap/utilite/ULogger.h"
+
+namespace rtabmap {
+
+KeypointItem::KeypointItem(int id, const cv::KeyPoint & kpt, float depth, const QColor & color, QGraphicsItem * parent) :
+	QGraphicsEllipseItem(kpt.pt.x-(kpt.size==0?3.0f:kpt.size)/2.0f, kpt.pt.y-(kpt.size==0?3.0f:kpt.size)/2.0f, kpt.size==0?3.0f:kpt.size, kpt.size==0?3.0f:kpt.size, parent),
+	_id(id),
+	_kpt(kpt),
+	_placeHolder(0),
+	_depth(depth)
+{
+	this->setColor(color);
+	this->setAcceptHoverEvents(true);
+	this->setFlag(QGraphicsItem::ItemIsFocusable, true);
+	_width = pen().width();
+}
+
+KeypointItem::~KeypointItem()
+{
+	delete _placeHolder;
+}
+
+void KeypointItem::setColor(const QColor & color)
+{
+	this->setPen(QPen(color));
+	this->setBrush(QBrush(color));
+}
+
+void KeypointItem::showDescription()
+{
+	if(!_placeHolder)
+	{
+		_placeHolder = new QGraphicsRectItem (this);
+		_placeHolder->setVisible(false);
+		if(qGray(pen().color().rgb()) > 255/2)
+		{
+			_placeHolder->setBrush(QBrush(QColor ( 0,0,0, 170 )));
+		}
+		else
+		{
+			_placeHolder->setBrush(QBrush(QColor ( 255, 255, 255, 170 )));
+		}
+		QGraphicsTextItem * text = new QGraphicsTextItem(_placeHolder);
+		text->setDefaultTextColor(this->pen().color().rgb());
+		// Make octave compatible with SIFT packed octave (https://github.com/opencv/opencv/issues/4554)
+		int octave = _kpt.octave & 255;
+		octave = octave < 128 ? octave : (-128 | octave);
+		float scale = octave >= 0 ? 1.f/(1 << octave) : (float)(1 << -octave);
+		if(_depth <= 0)
+		{
+			text->setPlainText(QString( "Id = %1\n"
+					"Dir = %3\n"
+					"Hessian = %4\n"
+					"X = %5\n"
+					"Y = %6\n"
+					"Size = %7\n"
+					"Octave = %8\n"
+					"Scale = %9").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(octave).arg(scale));
+		}
+		else
+		{
+			text->setPlainText(QString( "Id = %1\n"
+					"Dir = %3\n"
+					"Hessian = %4\n"
+					"X = %5\n"
+					"Y = %6\n"
+					"Size = %7\n"
+					"Octave = %8\n"
+					"Scale = %9\n"
+					"Depth = %10 m").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(octave).arg(scale).arg(_depth));
+		}
+		_placeHolder->setRect(text->boundingRect());
+	}
+
+
+	if(_placeHolder->parentItem())
+	{
+		_placeHolder->setParentItem(0); // Make it a to level item
+	}
+	QPen pen = this->pen();
+	this->setPen(QPen(pen.color(), _width+2));
+	_placeHolder->setZValue(this->zValue()+1);
+	_placeHolder->setPos(this->mapFromScene(0,0));
+	_placeHolder->setVisible(true);
+}
+
+void KeypointItem::hideDescription()
+{
+	if(_placeHolder)
+	{
+		_placeHolder->setVisible(false);
+	}
+	this->setPen(QPen(pen().color(), _width));
+}
+
+void KeypointItem::hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
+{
+	QGraphicsScene * scene = this->scene();
+	if(scene && scene->focusItem() == 0)
+	{
+		this->showDescription();
+	}
+	else
+	{
+		this->setPen(QPen(pen().color(), _width+2));
+	}
+	QGraphicsEllipseItem::hoverEnterEvent(event);
+}
+
+void KeypointItem::hoverLeaveEvent ( QGraphicsSceneHoverEvent * event )
+{
+	if(!this->hasFocus())
+	{
+		this->hideDescription();
+	}
+	QGraphicsEllipseItem::hoverEnterEvent(event);
+}
+
+void KeypointItem::focusInEvent ( QFocusEvent * event )
+{
+	this->showDescription();
+	QGraphicsEllipseItem::focusInEvent(event);
+}
+
+void KeypointItem::focusOutEvent ( QFocusEvent * event )
+{
+	this->hideDescription();
+	QGraphicsEllipseItem::focusOutEvent(event);
+}
+
+}

rtabmap/guilib/src/LinkRefiningDialog.cpp

@@ -0,0 +1,156 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/LinkRefiningDialog.h"
+#include "ui_linkRefiningDialog.h"
+
+#include <QPushButton>
+
+namespace rtabmap {
+
+LinkRefiningDialog::LinkRefiningDialog(QWidget * parent) :
+	QDialog(parent),
+	defaultNodeIdMin_(0),
+	defaultNodeIdMax_(0),
+	defaultMapIdMin_(0),
+	defaultMapIdMax_(0)
+{
+	ui_ = new Ui_linkRefiningDialog();
+	ui_->setupUi(this);
+
+	ui_->comboBox_link_type->addItem("All");
+	for(int i =0; i<Link::kPosePrior; ++i)
+	{
+		ui_->comboBox_link_type->addItem(Link::typeName((Link::Type)i).c_str());
+		if(((Link::Type)i) == Link::kVirtualClosure)
+		{
+			ui_->comboBox_link_type->setItemData(i+1, 0, Qt::UserRole - 1);
+		}
+	}
+
+	restoreDefaults();
+
+	connect(ui_->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+	connect(ui_->comboBox_link_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateIntraInterState()));
+	connect(ui_->spinBox_node_from, SIGNAL(valueChanged(int)), this, SLOT(setRangeToNodeId()));
+	connect(ui_->spinBox_node_to, SIGNAL(valueChanged(int)), this, SLOT(setRangeToNodeId()));
+	connect(ui_->spinBox_map_from, SIGNAL(valueChanged(int)), this, SLOT(setRangeToMapId()));
+	connect(ui_->spinBox_map_to, SIGNAL(valueChanged(int)), this, SLOT(setRangeToMapId()));
+}
+
+LinkRefiningDialog::~LinkRefiningDialog()
+{
+	delete ui_;
+}
+
+void LinkRefiningDialog::setMinMax(
+		int nodeIdMin,
+		int nodeIdMax,
+		int mapIdMin,
+		int mapIdMax)
+{
+	bool reset = defaultNodeIdMin_ == 0;
+	defaultNodeIdMin_ = nodeIdMin;
+	defaultNodeIdMax_ = nodeIdMax;
+	defaultMapIdMin_ = mapIdMin;
+	defaultMapIdMax_ = mapIdMax;
+	ui_->spinBox_node_from->setMinimum(defaultNodeIdMin_);
+	ui_->spinBox_node_to->setMaximum(defaultNodeIdMax_);
+	ui_->spinBox_map_from->setMinimum(defaultMapIdMin_);
+	ui_->spinBox_map_to->setMaximum(defaultMapIdMax_);
+	if(reset)
+	{
+		restoreDefaults();
+	}
+}
+
+Link::Type LinkRefiningDialog::getLinkType() const
+{
+	if(ui_->comboBox_link_type->currentIndex() == 0)
+	{
+		return Link::kEnd;
+	}
+	return (Link::Type)(ui_->comboBox_link_type->currentIndex()-1);
+}
+
+void LinkRefiningDialog::getIntraInterSessions(bool & intra, bool & inter) const
+{
+	intra = ui_->comboBox_link_inter_intra->currentIndex() == 0 || ui_->comboBox_link_inter_intra->currentIndex() == 1 || getLinkType() == Link::kNeighbor;
+	inter = ui_->comboBox_link_inter_intra->currentIndex() == 0 || ui_->comboBox_link_inter_intra->currentIndex() == 2;
+}
+
+bool LinkRefiningDialog::isRangeByNodeId() const
+{
+	return ui_->radioButton_nodes->isChecked();
+}
+
+bool LinkRefiningDialog::isRangeByMapId() const
+{
+	return ui_->radioButton_maps->isChecked();
+}
+
+void LinkRefiningDialog::getRangeNodeId(int & from, int & to) const
+{
+	from = ui_->spinBox_node_from->value();
+	to = ui_->spinBox_node_to->value();
+}
+
+void LinkRefiningDialog::getRangeMapId(int & from, int & to) const
+{
+	from = ui_->spinBox_map_from->value();
+	to = ui_->spinBox_map_to->value();
+}
+
+void LinkRefiningDialog::restoreDefaults()
+{
+	ui_->comboBox_link_type->setCurrentIndex(0);
+	ui_->comboBox_link_inter_intra->setCurrentIndex(0);
+	ui_->spinBox_node_from->setValue(defaultNodeIdMin_);
+	ui_->spinBox_node_to->setValue(defaultNodeIdMax_);
+	ui_->spinBox_map_from->setValue(defaultMapIdMin_);
+	ui_->spinBox_map_to->setValue(defaultMapIdMax_);
+
+	ui_->radioButton_nodes->setChecked(true);
+}
+
+void LinkRefiningDialog::updateIntraInterState()
+{
+	ui_->comboBox_link_inter_intra->setEnabled(getLinkType() != Link::kNeighbor);
+}
+
+void LinkRefiningDialog::setRangeToNodeId()
+{
+	ui_->radioButton_nodes->setChecked(true);
+}
+
+void LinkRefiningDialog::setRangeToMapId()
+{
+	ui_->radioButton_maps->setChecked(true);
+}
+
+
+}

rtabmap/guilib/src/LoopClosureViewer.cpp

@@ -0,0 +1,154 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/LoopClosureViewer.h"
+#include "ui_loopClosureViewer.h"
+
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Signature.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UStl.h"
+
+#include <QtCore/QTimer>
+
+namespace rtabmap {
+
+LoopClosureViewer::LoopClosureViewer(QWidget * parent) :
+	QWidget(parent),
+	decimation_(1),
+	maxDepth_(0),
+	minDepth_(0)
+{
+	ui_ = new Ui_loopClosureViewer();
+	ui_->setupUi(this);
+	ui_->cloudViewerTransform->setCameraLockZ(false);
+
+	connect(ui_->checkBox_rawCloud, SIGNAL(clicked()), this, SLOT(updateView()));
+}
+
+LoopClosureViewer::~LoopClosureViewer() {
+	delete ui_;
+}
+
+void LoopClosureViewer::setData(const Signature & sA, const Signature & sB)
+{
+	sA_ = sA;
+	sB_ = sB;
+	if(sA_.id()>0 && sB_.id()>0)
+	{
+		ui_->label_idA->setText(QString("[%1-%2]").arg(sA.id()).arg(sB.id()));
+	}
+}
+
+void LoopClosureViewer::updateView(const Transform & transform, const ParametersMap & parameters)
+{
+	if(sA_.id()>0 && sB_.id()>0)
+	{
+		int decimation = 1;
+		float maxDepth = 0;
+		float minDepth = 0;
+
+		if(!ui_->checkBox_rawCloud->isChecked())
+		{
+			decimation = decimation_;
+			maxDepth = maxDepth_;
+			minDepth = minDepth_;
+		}
+
+		UDEBUG("decimation = %d", decimation);
+		UDEBUG("maxDepth = %f", maxDepth);
+		UDEBUG("minDepth = %d", minDepth);
+
+		Transform t;
+		if(!transform.isNull())
+		{
+			transform_ = transform;
+			t = transform;
+		}
+		else if(!transform_.isNull())
+		{
+			t = transform_;
+		}
+		else
+		{
+			t = sB_.getPose();
+		}
+
+		UDEBUG("t= %s", t.prettyPrint().c_str());
+		ui_->label_transform->setText(QString("(%1)").arg(t.prettyPrint().c_str()));
+		if(!t.isNull())
+		{
+			//cloud 3d
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA, cloudB;
+			cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, minDepth, 0, parameters);
+			cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, minDepth, 0, parameters);
+
+			//cloud 2d
+			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
+			scanA = util3d::laserScanToPointCloud(sA_.sensorData().laserScanRaw(), sA_.sensorData().laserScanRaw().localTransform());
+			scanB = util3d::laserScanToPointCloud(sB_.sensorData().laserScanRaw(), sB_.sensorData().laserScanRaw().localTransform());
+
+			ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_.id()).arg(cloudB->size()).arg(sA_.id()).arg(cloudA->size()));
+
+			if(cloudA->size())
+			{
+				ui_->cloudViewerTransform->addCloud("cloud0", cloudA);
+			}
+			if(cloudB->size())
+			{
+				cloudB = util3d::transformPointCloud(cloudB, t);
+				ui_->cloudViewerTransform->addCloud("cloud1", cloudB);
+			}
+			if(scanA->size())
+			{
+				ui_->cloudViewerTransform->addCloud("scan0", scanA);
+			}
+			if(scanB->size())
+			{
+				scanB = util3d::transformPointCloud(scanB, t);
+				ui_->cloudViewerTransform->addCloud("scan1", scanB);
+			}
+		}
+		else
+		{
+			UERROR("loop transform is null !?!?");
+			ui_->cloudViewerTransform->removeAllClouds();
+		}
+		ui_->cloudViewerTransform->refreshView();
+	}
+}
+
+
+void LoopClosureViewer::showEvent(QShowEvent * event)
+{
+	QWidget::showEvent( event );
+	QTimer::singleShot(500, this, SLOT(updateView())); // make sure the QVTKWidget is shown!
+}
+

rtabmap/guilib/src/MapVisibilityWidget.cpp

@@ -0,0 +1,143 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/MapVisibilityWidget.h"
+
+#include <QCheckBox>
+#include <QVBoxLayout>
+#include <QScrollArea>
+
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+MapVisibilityWidget::MapVisibilityWidget(QWidget * parent) : QWidget(parent) {
+
+	QVBoxLayout * verticalLayout1 = new QVBoxLayout(this);
+	QScrollArea * scrollArea = new QScrollArea(this);
+	scrollArea->setWidgetResizable(true);
+	QWidget * scrollAreaWidgetContent = new QWidget();
+	scrollAreaWidgetContent->setObjectName("area");
+	QVBoxLayout * layout2 = new QVBoxLayout(scrollAreaWidgetContent);
+	scrollAreaWidgetContent->setLayout(layout2);
+	scrollArea->setWidget(scrollAreaWidgetContent);
+
+	QCheckBox * selectAll = new QCheckBox("Select all", this);
+	connect(selectAll, SIGNAL(toggled(bool)), this, SLOT(selectAll(bool)));
+	verticalLayout1->addWidget(selectAll);
+	verticalLayout1->addWidget(scrollArea);
+}
+
+MapVisibilityWidget::~MapVisibilityWidget() {
+
+}
+
+void MapVisibilityWidget::showEvent(QShowEvent * event)
+{
+	updateCheckBoxes();
+}
+
+void MapVisibilityWidget::clear()
+{
+	_poses.clear();
+	_mask.clear();
+	updateCheckBoxes();
+}
+
+void MapVisibilityWidget::updateCheckBoxes()
+{
+	QWidget * area = this->findChild<QWidget*>("area");
+	QVBoxLayout * layout = (QVBoxLayout *)area->layout();
+	QList<QCheckBox*> checkboxes = area->findChildren<QCheckBox*>();
+	while(checkboxes.size() && checkboxes.size() > (int)_poses.size())
+	{
+		delete *checkboxes.begin();
+		checkboxes.erase(checkboxes.begin());
+	}
+	int i=0;
+	for(std::map<int, Transform>::iterator iter=_poses.begin(); iter!=_poses.end(); ++iter)
+	{
+		bool added = false;
+		if(i >= checkboxes.size())
+		{
+			checkboxes.push_back(new QCheckBox(area));
+			added = true;
+		}
+		checkboxes[i]->setText(QString("%1 (%2)").arg(iter->first).arg(iter->second.prettyPrint().c_str()));
+		checkboxes[i]->setChecked(_mask.at(iter->first));
+		if(added)
+		{
+			connect(checkboxes[i], SIGNAL(stateChanged(int)), this, SLOT(signalVisibility()));
+			layout->addWidget(checkboxes[i]);
+		}
+
+		++i;
+	}
+}
+
+void MapVisibilityWidget::setMap(const std::map<int, Transform> & poses, const std::map<int, bool> & mask)
+{
+	UASSERT(poses.size() == mask.size());
+	_poses = poses;
+	_mask = mask;
+	if(this->isVisible())
+	{
+		updateCheckBoxes();
+	}
+}
+
+std::map<int, Transform> MapVisibilityWidget::getVisiblePoses() const
+{
+	std::map<int, Transform> poses;
+	for(std::map<int, Transform>::const_iterator iter=_poses.begin(); iter!=_poses.end(); ++iter)
+	{
+		if(_mask.at(iter->first) && iter->first > 0)
+		{
+			poses.insert(*iter);
+		}
+	}
+	return poses;
+}
+
+void MapVisibilityWidget::signalVisibility()
+{
+	QCheckBox * check = qobject_cast<QCheckBox*>(sender());
+	_mask.at(check->text().split('(').first().toInt()) = check->isChecked();
+	Q_EMIT visibilityChanged(check->text().split('(').first().toInt(), check->isChecked());
+}
+
+void MapVisibilityWidget::selectAll(bool checked)
+{
+	QWidget * area = this->findChild<QWidget*>("area");
+	QList<QCheckBox*> checkboxes = area->findChildren<QCheckBox*>();
+	for(int i = 0; i<checkboxes.size(); ++i)
+	{
+		checkboxes[i]->setChecked(checked);
+	}
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/MultiSessionLocSubView.cpp

@@ -0,0 +1,75 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#include "rtabmap/gui/MultiSessionLocSubView.h"
+#include "ui_multiSessionLocSubView.h"
+
+namespace rtabmap {
+
+MultiSessionLocSubView::MultiSessionLocSubView(ImageView * mainView, int mapId, QWidget * parent) :
+		QWidget(parent),
+		mapId_(mapId)
+{
+	ui_ = new Ui_multiSessionLocSubView();
+	ui_->setupUi(this);
+	ui_->imageView->setFeaturesShown(mainView->isFeaturesShown());
+	ui_->imageView->setFeaturesSize(mainView->getFeaturesSize());
+	ui_->imageView->setAlpha(mainView->getAlpha());
+	ui_->imageView->setDefaultMatchingFeatureColor(mainView->getDefaultMatchingFeatureColor());
+}
+MultiSessionLocSubView::~MultiSessionLocSubView() {}
+
+void MultiSessionLocSubView::updateView(
+		int nodeId,
+		const QImage & image,
+		const std::multimap<int, cv::KeyPoint> & features,
+		float locRatio,
+		const QColor & bgColor)
+{
+	if(image.isNull())
+	{
+		ui_->imageView->clear();
+	}
+	else
+	{
+		ui_->imageView->setImage(image);
+		ui_->imageView->setFeatures(features, cv::Mat(), ui_->imageView->getDefaultMatchingFeatureColor());
+		ui_->imageView->setBackgroundColor(bgColor);
+	}
+	ui_->label->setText(QString("%1 [%2]").arg(nodeId).arg(mapId_));
+	ui_->locProgressBar->setValue(locRatio * 100);
+}
+
+void MultiSessionLocSubView::clear()
+{
+	ui_->imageView->clear();
+	ui_->label->setText(QString("[%1]").arg(mapId_));
+}
+
+} /* namespace rtabmap */
+

rtabmap/guilib/src/MultiSessionLocWidget.cpp

@@ -0,0 +1,207 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/MultiSessionLocWidget.h"
+#include "rtabmap/gui/MultiSessionLocSubView.h"
+#include "rtabmap/gui/ImageView.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/core/Graph.h"
+
+#include <QHBoxLayout>
+#include <QPushButton>
+#include <QProgressBar>
+
+namespace rtabmap {
+
+MultiSessionLocWidget::MultiSessionLocWidget(
+		const QMap<int, Signature> * cache,
+		const std::map<int, int> * mapIds,
+		QWidget * parent) :
+				QWidget(parent),
+				cache_(cache),
+				mapIds_(mapIds),
+				totalFrames_(0),
+				totalLoops_(0)
+{
+	UASSERT(cache != 0);
+	UASSERT(mapIds != 0);
+	imageView_ = new ImageView(this);
+	imageView_->setObjectName("multisession_imageview");
+	totalLocProgressBar_ = new QProgressBar(this);
+	resetbutton_ = new QPushButton(this);
+	resetbutton_->setText("Reset");
+
+	// setup layout
+	this->setLayout(new QHBoxLayout());
+	QVBoxLayout * vLayout = new QVBoxLayout();
+	vLayout->addWidget(imageView_, 1);
+	QHBoxLayout * hLayout = new QHBoxLayout();
+	hLayout->addWidget(resetbutton_, 0);
+	hLayout->addWidget(totalLocProgressBar_, 1);
+	vLayout->addLayout(hLayout, 0);
+	((QHBoxLayout*)this->layout())->addLayout(vLayout, 1);
+
+	connect(resetbutton_, SIGNAL(clicked()), this, SLOT(clear()));
+}
+MultiSessionLocWidget::~MultiSessionLocWidget() {}
+
+void MultiSessionLocWidget::updateView(
+		const Signature & lastSignature,
+		const Statistics & stats)
+{
+	++totalFrames_;
+	std::multimap<int, Link> loopLinks = graph::filterLinks(lastSignature.getLinks(), Link::kGlobalClosure, true);
+	std::multimap<int, Link> localLinks = graph::filterLinks(lastSignature.getLinks(), Link::kLocalSpaceClosure, true);
+	loopLinks.insert(localLinks.begin(), localLinks.end());
+
+	if(!loopLinks.empty())
+	{
+		++totalLoops_;
+		totalLocProgressBar_->setValue(float(totalLoops_)/float(totalFrames_) * 100);
+	}
+
+	if(!lastSignature.sensorData().imageRaw().empty() ||
+	   !lastSignature.sensorData().imageCompressed().empty())
+	{
+		cv::Mat image;
+		lastSignature.sensorData().uncompressDataConst(&image, 0);
+		if(!image.empty())
+		{
+			imageView_->setImage(uCvMat2QImage(image));
+			imageView_->setFeatures(lastSignature.getWordsKpts(), cv::Mat(), imageView_->getDefaultMatchingFeatureColor());
+			imageView_->setBackgroundColor(Qt::black);
+		}
+		else
+		{
+			imageView_->clear();
+		}
+	}
+	else
+	{
+		imageView_->clear();
+	}
+
+	std::map<int, std::pair<int, float> > top;
+	const std::map<int, float> & likelihood = stats.posterior();
+	for(std::map<int, float>::const_iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
+	{
+		if(mapIds_->find(iter->first) != mapIds_->end())
+		{
+			int mapId = mapIds_->at(iter->first);
+			if(subViews_.find(mapId)==subViews_.end())
+			{
+				MultiSessionLocSubView * subView = new MultiSessionLocSubView(imageView_, mapId, this);
+				((QHBoxLayout*)this->layout())->addWidget(subView, 1);
+				subViews_.insert(std::make_pair(mapId, std::make_pair(subView, 0)));
+			}
+
+			if(top.find(mapId) == top.end() || top.at(mapId).second < iter->second)
+			{
+				uInsert(top, std::make_pair(mapId, std::make_pair(iter->first, iter->second)));
+			}
+		}
+	}
+
+	// update highest loop closure hypotheses per session
+	for(std::map<int, std::pair<MultiSessionLocSubView*, int> >::iterator iter=subViews_.begin(); iter!=subViews_.end(); ++iter)
+	{
+
+		for(std::multimap<int, Link>::iterator jter=loopLinks.begin(); jter!=loopLinks.end(); ++jter)
+		{
+			if(mapIds_->find(jter->first)!= mapIds_->end())
+			{
+				int mapId = mapIds_->at(jter->first);
+				iter->second.second += mapId==iter->first?1:0;
+			}
+		}
+
+		if(uContains(top, iter->first))
+		{
+			int nodeId = top.at(iter->first).first;
+			if(cache_->contains(nodeId))
+			{
+				cv::Mat image;
+				const Signature & s = (*cache_)[nodeId];
+
+				Link link = loopLinks.find(nodeId) != loopLinks.end()?loopLinks.find(nodeId)->second:Link();
+				std::multimap<int, cv::KeyPoint> keypoints;
+				for(std::multimap<int, int>::const_iterator jter=s.getWords().begin(); jter!=s.getWords().end(); ++jter)
+				{
+					if(jter->first>0 && lastSignature.getWords().find(jter->first) != lastSignature.getWords().end())
+					{
+						keypoints.insert(std::make_pair(jter->first, s.getWordsKpts()[jter->second]));
+					}
+				}
+
+				if(!keypoints.empty())
+				{
+					s.sensorData().uncompressDataConst(&image, 0);
+					if(!image.empty())
+					{
+						iter->second.first->updateView(
+								nodeId,
+								uCvMat2QImage(image),
+								keypoints,
+								float(iter->second.second)/float(totalFrames_),
+								link.type() == Link::kLocalSpaceClosure?Qt::yellow:
+								link.type() == Link::kGlobalClosure?Qt::green:Qt::gray);
+					}
+					else
+					{
+						iter->second.first->clear();
+					}
+				}
+				else
+				{
+					iter->second.first->clear();
+				}
+			}
+			else
+			{
+				iter->second.first->clear();
+			}
+		}
+		else
+		{
+			iter->second.first->clear();
+		}
+	}
+}
+void MultiSessionLocWidget::clear()
+{
+	for(std::map<int, std::pair<MultiSessionLocSubView*, int> >::iterator iter=subViews_.begin(); iter!=subViews_.end(); ++iter)
+	{
+		delete iter->second.first;
+	}
+	subViews_.clear();
+	imageView_->clear();
+	totalFrames_ = 0;
+	totalLoops_ = 0;
+	totalLocProgressBar_->setValue(0);
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/OdometryViewer.cpp

@@ -0,0 +1,515 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/OdometryViewer.h"
+
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Odometry.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UCv2Qt.h"
+
+#include "rtabmap/gui/ImageView.h"
+#include "rtabmap/gui/CloudViewer.h"
+
+#include <QPushButton>
+#include <QSpinBox>
+#include <QDoubleSpinBox>
+#include <QCheckBox>
+#include <QLabel>
+#include <QHBoxLayout>
+#include <QVBoxLayout>
+#include <QApplication>
+
+namespace rtabmap {
+
+OdometryViewer::OdometryViewer(
+	int maxClouds, 
+	int decimation, 
+	float voxelSize, 
+	float maxDepth, 
+	int qualityWarningThr, 
+	QWidget * parent,
+	const ParametersMap & parameters) :
+		QDialog(parent),
+		imageView_(new ImageView(this)),
+		cloudView_(new CloudViewer(this)),
+		processingData_(false),
+		odomImageShow_(true),
+		odomImageDepthShow_(true),
+		lastOdomPose_(Transform::getIdentity()),
+		qualityWarningThr_(qualityWarningThr),
+		id_(0),
+		validDecimationValue_(1),
+		parameters_(parameters)
+{
+
+	qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
+
+	imageView_->setImageDepthShown(false);
+	imageView_->setMinimumSize(320, 240);
+	imageView_->setAlpha(255);
+
+	cloudView_->setCameraTargetLocked();
+	cloudView_->setGridShown(true);
+	cloudView_->setFrustumShown(true);
+
+	QLabel * maxCloudsLabel = new QLabel("Max clouds", this);
+	QLabel * voxelLabel = new QLabel("Voxel", this);
+	QLabel * maxDepthLabel = new QLabel("Max depth", this);
+	QLabel * decimationLabel = new QLabel("Decimation", this);
+	maxCloudsSpin_ = new QSpinBox(this);
+	maxCloudsSpin_->setMinimum(0);
+	maxCloudsSpin_->setMaximum(100);
+	maxCloudsSpin_->setValue(maxClouds);
+	voxelSpin_ = new QDoubleSpinBox(this);
+	voxelSpin_->setMinimum(0);
+	voxelSpin_->setMaximum(1);
+	voxelSpin_->setDecimals(3);
+	voxelSpin_->setSingleStep(0.01);
+	voxelSpin_->setSuffix(" m");
+	voxelSpin_->setValue(voxelSize);
+	maxDepthSpin_ = new QDoubleSpinBox(this);
+	maxDepthSpin_->setMinimum(0);
+	maxDepthSpin_->setMaximum(100);
+	maxDepthSpin_->setDecimals(0);
+	maxDepthSpin_->setSingleStep(1);
+	maxDepthSpin_->setSuffix(" m");
+	maxDepthSpin_->setValue(maxDepth);
+	decimationSpin_ = new QSpinBox(this);
+	decimationSpin_->setMinimum(1);
+	decimationSpin_->setMaximum(16);
+	decimationSpin_->setValue(decimation);
+	cloudShown_ = new QCheckBox(this);
+	cloudShown_->setText("Cloud");
+	cloudShown_->setChecked(true);
+	scanShown_ = new QCheckBox(this);
+	scanShown_->setText("Scan");
+	scanShown_->setChecked(true);
+	featuresShown_ = new QCheckBox(this);
+	featuresShown_->setText("Features");
+	featuresShown_->setChecked(true);
+	timeLabel_ = new QLabel(this);
+	QPushButton * resetButton = new QPushButton("reset", this);
+	QPushButton * clearButton = new QPushButton("clear", this);
+	QPushButton * closeButton = new QPushButton("close", this);
+	connect(resetButton, SIGNAL(clicked()), this, SLOT(reset()));
+	connect(clearButton, SIGNAL(clicked()), this, SLOT(clear()));
+	connect(closeButton, SIGNAL(clicked()), this, SLOT(reject()));
+
+	//layout
+	QHBoxLayout * layout = new QHBoxLayout();
+	layout->setContentsMargins(0,0,0,0);
+	layout->setSpacing(0);
+	layout->addWidget(imageView_,1);
+	layout->addWidget(cloudView_,1);
+
+	QHBoxLayout * hlayout2 = new QHBoxLayout();
+	hlayout2->setContentsMargins(0,0,0,0);
+	hlayout2->addWidget(maxCloudsLabel);
+	hlayout2->addWidget(maxCloudsSpin_);
+	hlayout2->addWidget(voxelLabel);
+	hlayout2->addWidget(voxelSpin_);
+	hlayout2->addWidget(maxDepthLabel);
+	hlayout2->addWidget(maxDepthSpin_);
+	hlayout2->addWidget(decimationLabel);
+	hlayout2->addWidget(decimationSpin_);
+	hlayout2->addWidget(cloudShown_);
+	hlayout2->addWidget(scanShown_);
+	hlayout2->addWidget(featuresShown_);
+	hlayout2->addWidget(timeLabel_);
+	hlayout2->addStretch(1);
+	hlayout2->addWidget(resetButton);
+	hlayout2->addWidget(clearButton);
+	hlayout2->addWidget(closeButton);
+
+	QVBoxLayout * vlayout = new QVBoxLayout(this);
+	vlayout->setContentsMargins(0,0,0,0);
+	vlayout->setSpacing(0);
+	vlayout->addLayout(layout, 1);
+	vlayout->addLayout(hlayout2);
+
+	this->setLayout(vlayout);
+}
+
+OdometryViewer::~OdometryViewer()
+{
+	this->unregisterFromEventsManager();
+	this->clear();
+	UDEBUG("");
+}
+
+void OdometryViewer::reset()
+{
+	this->post(new OdometryResetEvent());
+}
+
+void OdometryViewer::clear()
+{
+	addedClouds_.clear();
+	cloudView_->clear();
+}
+
+void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
+{
+	processingData_ = true;
+	int quality = odom.info().reg.inliers;
+
+	bool lost = false;
+	bool lostStateChanged = false;
+
+	if(odom.pose().isNull())
+	{
+		UDEBUG("odom lost"); // use last pose
+		lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
+		imageView_->setBackgroundColor(Qt::darkRed);
+		cloudView_->setBackgroundColor(Qt::darkRed);
+
+		lost = true;
+	}
+	else if(odom.info().reg.inliers>0 &&
+			qualityWarningThr_ &&
+			odom.info().reg.inliers < qualityWarningThr_)
+	{
+		UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, qualityWarningThr_);
+		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
+		imageView_->setBackgroundColor(Qt::darkYellow);
+		cloudView_->setBackgroundColor(Qt::darkYellow);
+	}
+	else
+	{
+		UDEBUG("odom ok");
+		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
+		imageView_->setBackgroundColor(cloudView_->getDefaultBackgroundColor());
+		cloudView_->setBackgroundColor(Qt::black);
+	}
+
+	timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation));
+
+	if(cloudShown_->isChecked() &&
+		!odom.data().imageRaw().empty() &&
+		!odom.data().depthOrRightRaw().empty() &&
+		(odom.data().stereoCameraModels().size() || odom.data().cameraModels().size()))
+	{
+		UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);
+
+		if(!odom.data().depthRaw().empty())
+		{
+			if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 &&
+			   odom.data().imageRaw().rows % decimationSpin_->value() == 0)
+			{
+				validDecimationValue_ = decimationSpin_->value();
+			}
+			else
+			{
+				UWARN("Decimation (%d) must be a denominator of the width and height of "
+						"the image (%d/%d). Using last valid decimation value (%d).",
+						decimationSpin_->value(),
+						odom.data().imageRaw().cols,
+						odom.data().imageRaw().rows,
+						validDecimationValue_);
+			}
+		}
+		else
+		{
+			validDecimationValue_ = decimationSpin_->value();
+		}
+
+		// visualization: buffering the clouds
+		// Create the new cloud
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+		pcl::IndicesPtr validIndices(new std::vector<int>);
+		cloud = util3d::cloudRGBFromSensorData(
+				odom.data(),
+				validDecimationValue_,
+				0,
+				0,
+				validIndices.get(),
+				parameters_);
+
+		if(voxelSpin_->value())
+		{
+			cloud = util3d::voxelize(cloud, validIndices, voxelSpin_->value());
+		}
+
+		if(cloud->size())
+		{
+			if(!odom.pose().isNull())
+			{
+				if(cloudView_->getAddedClouds().contains("cloudtmp"))
+				{
+					cloudView_->removeCloud("cloudtmp");
+				}
+
+				while(maxCloudsSpin_->value()>0 && (int)addedClouds_.size() > maxCloudsSpin_->value())
+				{
+					UASSERT(cloudView_->removeCloud(addedClouds_.first()));
+					addedClouds_.pop_front();
+				}
+
+				odom.data().id()?id_=odom.data().id():++id_;
+				std::string cloudName = uFormat("cloud%d", id_);
+				addedClouds_.push_back(cloudName);
+				UASSERT(cloudView_->addCloud(cloudName, cloud, odom.pose()));
+			}
+			else
+			{
+				cloudView_->addCloud("cloudtmp", cloud, lastOdomPose_);
+			}
+		}
+	}
+	else if(!cloudShown_->isChecked())
+	{
+		while(!addedClouds_.empty())
+		{
+			UASSERT(cloudView_->removeCloud(addedClouds_.first()));
+			addedClouds_.pop_front();
+		}
+	}
+
+	if(!odom.pose().isNull())
+	{
+		lastOdomPose_ = odom.pose();
+		cloudView_->updateCameraTargetPosition(odom.pose());
+
+		if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
+		{
+			cloudView_->updateCameraFrustums(odom.pose(), odom.data().cameraModels());
+		}
+		else if(odom.data().stereoCameraModels().size() && !odom.data().stereoCameraModels()[0].localTransform().isNull())
+		{
+			cloudView_->updateCameraFrustums(odom.pose(), odom.data().stereoCameraModels());
+		}
+	}
+
+	if(scanShown_->isChecked())
+	{
+		// scan local map
+		if(!odom.info().localScanMap.isEmpty())
+		{
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
+			cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap, odom.info().localScanMap.localTransform());
+			if(!cloudView_->addCloud("scanMapOdom", cloud, Transform::getIdentity(), Qt::blue))
+			{
+				UERROR("Adding scanMapOdom to viewer failed!");
+			}
+			else
+			{
+				cloudView_->setCloudVisibility("scanMapOdom", true);
+				cloudView_->setCloudOpacity("scanMapOdom", 0.5);
+			}
+		}
+		// scan cloud
+		if(!odom.data().laserScanRaw().isEmpty())
+		{
+			LaserScan scan = odom.data().laserScanRaw();
+
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
+			cloud = util3d::laserScanToPointCloudNormal(scan, odom.pose() * scan.localTransform());
+
+			if(!cloudView_->addCloud("scanOdom", cloud, Transform::getIdentity(), Qt::magenta))
+			{
+				UERROR("Adding scanOdom to viewer failed!");
+			}
+			else
+			{
+				cloudView_->setCloudVisibility("scanOdom", true);
+				cloudView_->setCloudOpacity("scanOdom", 0.5);
+			}
+		}
+	}
+	else
+	{
+		cloudView_->removeCloud("scanMapOdom");
+		cloudView_->removeCloud("scanOdom");
+	}
+
+	// 3d features
+	if(featuresShown_->isChecked())
+	{
+		if(!odom.info().localMap.empty() && !odom.pose().isNull())
+		{
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+			cloud->resize(odom.info().localMap.size());
+			int i=0;
+			for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
+			{
+				// filter very far features from current location
+				if(uNormSquared(iter->second.x-odom.pose().x(), iter->second.y-odom.pose().y(), iter->second.z-odom.pose().z()) < 50*50)
+				{
+					(*cloud)[i].x = iter->second.x;
+					(*cloud)[i].y = iter->second.y;
+					(*cloud)[i].z = iter->second.z;
+
+					// green = inlier, yellow = outliers
+					bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
+					(*cloud)[i].r = inlier?0:255;
+					(*cloud)[i].g = 255;
+					(*cloud)[i++].b = 0;
+				}
+			}
+			cloud->resize(i);
+
+			if(!cloudView_->addCloud("featuresOdom", cloud))
+			{
+				UERROR("Adding featuresOdom to viewer failed!");
+			}
+			else
+			{
+				cloudView_->setCloudVisibility("featuresOdom", true);
+				cloudView_->setCloudPointSize("featuresOdom", 3);
+			}
+		}
+	}
+	else
+	{
+		cloudView_->removeCloud("featuresOdom");
+	}
+
+	if(!odom.data().imageRaw().empty())
+	{
+		if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM)
+		{
+			imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow);
+		}
+		else if(odom.info().type == (int)Odometry::kTypeF2F ||
+				odom.info().type == (int)Odometry::kTypeViso2 ||
+				odom.info().type == (int)Odometry::kTypeFovis ||
+				odom.info().type == (int)Odometry::kTypeMSCKF)
+		{
+			std::vector<cv::KeyPoint> kpts;
+			cv::KeyPoint::convert(odom.info().newCorners, kpts, 7);
+			imageView_->setFeatures(kpts, odom.data().depthRaw(), Qt::red);
+		}
+
+		imageView_->clearLines();
+		if(lost)
+		{
+			if(lostStateChanged)
+			{
+				// save state
+				odomImageShow_ = imageView_->isImageShown();
+				odomImageDepthShow_ = imageView_->isImageDepthShown();
+			}
+			imageView_->setImageDepth(odom.data().imageRaw());
+			imageView_->setImageShown(true);
+			imageView_->setImageDepthShown(true);
+		}
+		else
+		{
+			if(lostStateChanged)
+			{
+				// restore state
+				imageView_->setImageShown(odomImageShow_);
+				imageView_->setImageDepthShown(odomImageDepthShow_);
+			}
+
+			imageView_->setImage(uCvMat2QImage(odom.data().imageRaw()));
+			if(imageView_->isImageDepthShown())
+			{
+				imageView_->setImageDepth(odom.data().depthOrRightRaw());
+			}
+
+			if( odom.info().type == Odometry::kTypeF2M ||
+				odom.info().type == (int)Odometry::kTypeORBSLAM ||
+				odom.info().type == (int)Odometry::kTypeMSCKF)
+			{
+				if(imageView_->isFeaturesShown())
+				{
+					for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
+					{
+						imageView_->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
+					}
+					for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
+					{
+						imageView_->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
+					}
+				}
+			}
+		}
+		if((odom.info().type == (int)Odometry::kTypeF2F ||
+			odom.info().type == (int)Odometry::kTypeViso2 ||
+			odom.info().type == (int)Odometry::kTypeFovis) && odom.info().cornerInliers.size())
+		{
+			if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
+			{
+				//draw lines
+				UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
+				for(unsigned int i=0; i<odom.info().cornerInliers.size(); ++i)
+				{
+					if(imageView_->isFeaturesShown())
+					{
+						imageView_->setFeatureColor(odom.info().cornerInliers[i], Qt::green); // inliers
+					}
+					if(imageView_->isLinesShown())
+					{
+						imageView_->addLine(
+								odom.info().newCorners[odom.info().cornerInliers[i]].x,
+								odom.info().newCorners[odom.info().cornerInliers[i]].y,
+								odom.info().refCorners[odom.info().cornerInliers[i]].x,
+								odom.info().refCorners[odom.info().cornerInliers[i]].y,
+								Qt::blue);
+					}
+				}
+			}
+		}
+
+		if(!odom.data().imageRaw().empty())
+		{
+			imageView_->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
+		}
+	}
+
+	imageView_->update();
+	cloudView_->update();
+	cloudView_->refreshView();
+	QApplication::processEvents();
+	processingData_ = false;
+}
+
+bool OdometryViewer::handleEvent(UEvent * event)
+{
+	if(!processingData_ && this->isVisible())
+	{
+		if(event->getClassName().compare("OdometryEvent") == 0)
+		{
+			rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
+			if(odomEvent->data().isValid())
+			{
+				processingData_ = true;
+				QMetaObject::invokeMethod(this, "processData",
+						Q_ARG(rtabmap::OdometryEvent, *odomEvent));
+			}
+		}
+	}
+	return false;
+}
+
+} /* namespace rtabmap */

rtabmap/guilib/src/ParametersToolBox.cpp

@@ -0,0 +1,596 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+//
+// Original version from Find-Object: https://github.com/introlab/find-object
+//
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Optimizer.h>
+
+#include "rtabmap/gui/ParametersToolBox.h"
+#include <QComboBox>
+#include <QDoubleSpinBox>
+#include <QLineEdit>
+#include <QStackedWidget>
+#include <QScrollArea>
+#include <QLabel>
+#include <QGroupBox>
+#include <QCheckBox>
+#include <QVBoxLayout>
+#include <QMessageBox>
+#include <QPushButton>
+#include <stdio.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <opencv2/opencv_modules.hpp>
+
+namespace rtabmap {
+
+ParametersToolBox::ParametersToolBox(QWidget *parent) :
+	QWidget(parent),
+	comboBox_(new QComboBox(this)),
+	stackedWidget_(new QStackedWidget(this))
+{
+	QVBoxLayout * layout = new QVBoxLayout(this);
+	this->setLayout(layout);
+
+	layout->addWidget(comboBox_);
+	layout->addWidget(stackedWidget_, 1);
+	QPushButton * resetButton = new QPushButton(this);
+	resetButton->setText(tr("Restore Defaults"));
+	layout->addWidget(resetButton);
+	connect(resetButton, SIGNAL(clicked()), this, SLOT(resetCurrentPage()));
+}
+
+ParametersToolBox::~ParametersToolBox()
+{
+}
+
+QWidget * ParametersToolBox::getParameterWidget(const QString & key)
+{
+	return this->findChild<QWidget*>(key);
+}
+
+QStringList ParametersToolBox::resetPage(int index)
+{
+	QStringList paramChanged;
+	const QObjectList & children = stackedWidget_->widget(index)->children().first()->children().first()->children();
+	for(int j=0; j<children.size();++j)
+	{
+		QString key = children.at(j)->objectName();
+		// ignore working memory
+		QString group = key.split("/").first();
+		if(parameters_.find(key.toStdString())!=parameters_.end())
+		{
+			UASSERT_MSG(parameters_.find(key.toStdString()) != parameters_.end(), uFormat("key=%s", key.toStdString().c_str()).c_str());
+			std::string value = Parameters::getDefaultParameters().at(key.toStdString());
+			parameters_.at(key.toStdString()) = value;
+
+			if(qobject_cast<QComboBox*>(children.at(j)))
+			{
+				if(((QComboBox*)children.at(j))->currentIndex() != QString::fromStdString(value).split(':').first().toInt())
+				{
+					((QComboBox*)children.at(j))->setCurrentIndex(QString::fromStdString(value).split(':').first().toInt());
+					paramChanged.append(key);
+				}
+			}
+			else if(qobject_cast<QSpinBox*>(children.at(j)))
+			{
+				if(((QSpinBox*)children.at(j))->value() != uStr2Int(value))
+				{
+					((QSpinBox*)children.at(j))->setValue(uStr2Int(value));
+					paramChanged.append(key);
+				}
+			}
+			else if(qobject_cast<QDoubleSpinBox*>(children.at(j)))
+			{
+				if(((QDoubleSpinBox*)children.at(j))->value() != uStr2Double(value))
+				{
+					((QDoubleSpinBox*)children.at(j))->setValue(uStr2Double(value));
+					paramChanged.append(key);
+				}
+			}
+			else if(qobject_cast<QCheckBox*>(children.at(j)))
+			{
+				if(((QCheckBox*)children.at(j))->isChecked() != uStr2Bool(value))
+				{
+					((QCheckBox*)children.at(j))->setChecked(uStr2Bool(value));
+					paramChanged.append(key);
+				}
+			}
+			else if(qobject_cast<QLineEdit*>(children.at(j)))
+			{
+				if(((QLineEdit*)children.at(j))->text().compare(QString::fromStdString(value)) != 0)
+				{
+					((QLineEdit*)children.at(j))->setText(QString::fromStdString(value));
+					paramChanged.append(key);
+				}
+			}
+		}
+	}
+	return paramChanged;
+}
+
+void ParametersToolBox::resetCurrentPage()
+{
+	this->blockSignals(true);
+	QStringList paramChanged = this->resetPage(stackedWidget_->currentIndex());
+	this->blockSignals(false);
+	Q_EMIT parametersChanged(paramChanged);
+}
+
+void ParametersToolBox::resetAllPages()
+{
+	QStringList paramChanged;
+	this->blockSignals(true);
+	for(int i=0; i< stackedWidget_->count(); ++i)
+	{
+		paramChanged.append(this->resetPage(i));
+	}
+	this->blockSignals(false);
+	Q_EMIT parametersChanged(paramChanged);
+}
+
+void ParametersToolBox::updateParametersVisibility()
+{
+	//show/hide not used parameters
+	/*QComboBox * descriptorBox = this->findChild<QComboBox*>(Parameters::kFeature2D_2Descriptor());
+	QComboBox * detectorBox = this->findChild<QComboBox*>(Parameters::kFeature2D_1Detector());
+	if(descriptorBox && detectorBox)
+	{
+		QString group = Parameters::kFeature2D_2Descriptor().split('/').first();
+		QWidget * panel = 0;
+		for(int i=0; i<this->count(); ++i)
+		{
+			if(this->widget(i)->objectName().compare(group) == 0)
+			{
+				panel = this->widget(i);
+				break;
+			}
+		}
+		if(panel)
+		{
+			const QObjectList & objects = panel->children();
+			QString descriptorName = descriptorBox->currentText();
+			QString detectorName = detectorBox->currentText();
+
+			for(int i=0; i<objects.size(); ++i)
+			{
+				if(!objects[i]->objectName().isEmpty())
+				{
+					if(objects[i]->objectName().contains(descriptorName) || objects[i]->objectName().contains(detectorName))
+					{
+						((QWidget*)objects[i])->setVisible(true);
+					}
+					else if(objects[i]->objectName().contains("Fast") && detectorName == QString("ORB"))
+					{
+						((QWidget*)objects[i])->setVisible(true);	// ORB uses some FAST parameters
+					}
+					else if(!objects[i]->objectName().split('/').at(1).at(0).isDigit())
+					{
+						((QWidget*)objects[i])->setVisible(false);
+					}
+				}
+			}
+		}
+	}*/
+}
+
+void ParametersToolBox::setupUi(const ParametersMap & parameters)
+{
+	parameters_ = parameters;
+	QWidget * currentItem = 0;
+	QStringList groups;
+	for(ParametersMap::const_iterator iter=parameters.begin();
+			iter!=parameters.end();
+			++iter)
+	{
+		QStringList splitted = QString::fromStdString(iter->first).split('/');
+		QString group = splitted.first();
+
+		QString name = splitted.last();
+		if(currentItem == 0 || currentItem->objectName().compare(group) != 0)
+		{
+			groups.push_back(group);
+			QScrollArea * area = new QScrollArea(this);
+			stackedWidget_->addWidget(area);
+			currentItem = new QWidget();
+			currentItem->setObjectName(group);
+			QVBoxLayout * layout = new QVBoxLayout(currentItem);
+			layout->setSizeConstraint(QLayout::SetMinimumSize);
+			layout->setContentsMargins(0,0,0,0);
+			layout->setSpacing(0);
+			area->setWidget(currentItem);
+
+			addParameter(layout, iter->first, iter->second);
+		}
+		else
+		{
+			addParameter((QVBoxLayout*)currentItem->layout(), iter->first, iter->second);
+		}
+	}
+	comboBox_->addItems(groups);
+	connect(comboBox_, SIGNAL(currentIndexChanged(int)), stackedWidget_, SLOT(setCurrentIndex(int)));
+
+	updateParametersVisibility();
+}
+
+void ParametersToolBox::updateParameter(const std::string & key, const std::string & value)
+{
+	QString group = QString::fromStdString(key).split("/").first();
+	if(parameters_.find(key) != parameters_.end())
+	{
+		parameters_.at(key) = value;
+		QWidget * widget = this->findChild<QWidget*>(key.c_str());
+		QString type = QString::fromStdString(Parameters::getType(key));
+		if(type.compare("string") == 0)
+		{
+			QString valueQt = QString::fromStdString(value);
+			if(valueQt.contains(';'))
+			{
+				// It's a list, just change the index
+				QStringList splitted = valueQt.split(':');
+				((QComboBox*)widget)->setCurrentIndex(splitted.first().toInt());
+			}
+			else
+			{
+				((QLineEdit*)widget)->setText(valueQt);
+			}
+		}
+		else if(type.compare("int") == 0)
+		{
+			((QSpinBox*)widget)->setValue(uStr2Int(value));
+		}
+		else if(type.compare("uint") == 0)
+		{
+			((QSpinBox*)widget)->setValue(uStr2Int(value));
+		}
+		else if(type.compare("double") == 0)
+		{
+			((QDoubleSpinBox*)widget)->setValue(uStr2Double(value));
+		}
+		else if(type.compare("float") == 0)
+		{
+			((QDoubleSpinBox*)widget)->setValue(uStr2Float(value));
+		}
+		else if(type.compare("bool") == 0)
+		{
+			((QCheckBox*)widget)->setChecked(uStr2Bool(value));
+		}
+	}
+}
+
+void ParametersToolBox::addParameter(
+		QVBoxLayout * layout,
+		const std::string & key,
+		const std::string & value)
+{
+	std::string type = Parameters::getType(key);
+	if(type.compare("string") == 0)
+	{
+		addParameter(layout, key.c_str(), QString::fromStdString(value));
+	}
+	else if(type.compare("int") == 0 ||
+	        type.compare("uint") == 0 ||
+	        type.compare("unsigned int") == 0)
+	{
+		addParameter(layout, key.c_str(), uStr2Int(value));
+	}
+	else if(type.compare("double") == 0 ||
+	        type.compare("float") == 0)
+	{
+		addParameter(layout, key.c_str(), uStr2Double(value));
+	}
+	else if(type.compare("bool") == 0)
+	{
+		addParameter(layout, key.c_str(), uStr2Bool(value));
+	}
+	else
+	{
+	    UWARN("Not implemented type \"%s\" for parameter \"%s\". Parameter is not added to toolbox.", type.c_str(), key.c_str());
+	}
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout,
+		const QString & key,
+		const QString & value)
+{
+	if(value.contains(';'))
+	{
+		QComboBox * widget = new QComboBox(this);
+		widget->setObjectName(key);
+		QStringList splitted = value.split(':');
+		widget->addItems(splitted.last().split(';'));
+
+		widget->setCurrentIndex(splitted.first().toInt());
+		connect(widget, SIGNAL(currentIndexChanged(int)), this, SLOT(changeParameter(int)));
+		addParameter(layout, key, widget);
+	}
+	else
+	{
+		QLineEdit * widget = new QLineEdit(value, this);
+		widget->setObjectName(key);
+		connect(widget, SIGNAL(editingFinished()), this, SLOT(changeParameter()));
+		addParameter(layout, key, widget);
+	}
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout,
+		const QString & key,
+		const double & value)
+{
+	QDoubleSpinBox * widget = new QDoubleSpinBox(this);
+	int decimals = 0;
+	int decimalValue = 0;
+
+	QString str = Parameters::getDefaultParameters().at(key.toStdString()).c_str();
+	if(!str.isEmpty())
+	{
+		str.replace(',', '.');
+		QStringList items = str.split('.');
+		if(items.size() == 2)
+		{
+			decimals = items.back().length();
+			decimalValue = items.back().toInt();
+		}
+	}
+
+	double def = uStr2Double(Parameters::getDefaultParameters().at(key.toStdString()));
+	if(def<0.001 || (decimals >= 4 && decimalValue>0))
+	{
+		widget->setDecimals(5);
+		widget->setSingleStep(0.0001);
+	}
+	else if(def<0.01 || (decimals >= 3 && decimalValue>0))
+	{
+		widget->setDecimals(4);
+		widget->setSingleStep(0.001);
+	}
+	else if(def<0.1 || (decimals >= 2 && decimalValue>0))
+	{
+		widget->setDecimals(3);
+		widget->setSingleStep(0.01);
+	}
+	else if(def<1.0 || (decimals >= 1 && decimalValue>0))
+	{
+		widget->setDecimals(2);
+		widget->setSingleStep(0.1);
+	}
+	else
+	{
+		widget->setDecimals(1);
+	}
+
+
+	if(def>0.0)
+	{
+		widget->setMaximum(def*1000000.0);
+	}
+	else if(def==0.0)
+	{
+		widget->setMaximum(1000000.0);
+	}
+	else if(def<0.0)
+	{
+		widget->setMinimum(def*1000000.0);
+		widget->setMaximum(0.0);
+	}
+
+	// set minimum for selected parameters
+	if(key.compare(Parameters::kGridMinGroundHeight().c_str()) == 0 ||
+		key.compare(Parameters::kGridMaxGroundHeight().c_str()) == 0 ||
+		key.compare(Parameters::kGridMaxObstacleHeight().c_str()) == 0 ||
+		key.compare(Parameters::kVisDepthMaskFloorThr().c_str()) == 0)
+	{
+		widget->setMinimum(-1000000.0);
+	}
+
+	widget->setValue(value);
+	widget->setObjectName(key);
+	connect(widget, SIGNAL(editingFinished()), this, SLOT(changeParameter()));
+	addParameter(layout, key, widget);
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout,
+		const QString & key,
+		const int & value)
+{
+	QSpinBox * widget = new QSpinBox(this);
+	int def = uStr2Int(Parameters::getDefaultParameters().at(key.toStdString()));
+
+	if(def>0)
+	{
+		widget->setMaximum(def*1000000);
+	}
+	else if(def == 0)
+	{
+		widget->setMaximum(1000000);
+	}
+	else if(def<0)
+	{
+		widget->setMinimum(def*1000000);
+		widget->setMaximum(0);
+	}
+	widget->setValue(value);
+	widget->setObjectName(key);
+
+	if(key.compare(Parameters::kVisFeatureType().c_str()) == 0)
+	{
+#ifndef RTABMAP_NONFREE
+		if(value <= 1)
+		{
+			UWARN("SURF/SIFT not available, setting feature default to FAST/BRIEF.");
+			widget->setValue(4);
+		}
+#endif
+	}
+	if(key.compare(Parameters::kOptimizerStrategy().c_str()) == 0)
+	{
+		if(value == 0 && !Optimizer::isAvailable(Optimizer::kTypeTORO))
+		{
+			if(Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+			{
+				UWARN("TORO is not available, setting optimization default to GTSAM.");
+				widget->setValue(2);
+			}
+			else if(Optimizer::isAvailable(Optimizer::kTypeG2O))
+			{
+				UWARN("TORO is not available, setting optimization default to g2o.");
+				widget->setValue(1);
+			}
+		}
+		if(value == 1 && !Optimizer::isAvailable(Optimizer::kTypeG2O))
+		{
+			if(Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+			{
+				UWARN("g2o is not available, setting optimization default to GTSAM.");
+				widget->setValue(2);
+			}
+			else if(Optimizer::isAvailable(Optimizer::kTypeTORO))
+			{
+				UWARN("g2o is not available, setting optimization default to TORO.");
+				widget->setValue(0);
+			}
+		}
+		if(value == 2 && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+		{
+			if(Optimizer::isAvailable(Optimizer::kTypeG2O))
+			{
+				UWARN("GTSAM is not available, setting optimization default to g2o.");
+				widget->setValue(2);
+			}
+			else if(Optimizer::isAvailable(Optimizer::kTypeTORO))
+			{
+				UWARN("GTSAM is not available, setting optimization default to TORO.");
+				widget->setValue(1);
+			}
+		}
+		if(!Optimizer::isAvailable(Optimizer::kTypeG2O) &&
+			!Optimizer::isAvailable(Optimizer::kTypeGTSAM) &&
+			!Optimizer::isAvailable(Optimizer::kTypeTORO))
+		{
+			widget->setEnabled(false);
+		}
+	}
+
+	connect(widget, SIGNAL(editingFinished()), this, SLOT(changeParameter()));
+	addParameter(layout, key, widget);
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout,
+		const QString & key,
+		const bool & value)
+{
+	QCheckBox * widget = new QCheckBox(this);
+	widget->setChecked(value);
+	widget->setObjectName(key);
+	connect(widget, SIGNAL(stateChanged(int)), this, SLOT(changeParameter(int)));
+	addParameter(layout, key, widget);
+}
+
+void ParametersToolBox::addParameter(QVBoxLayout * layout, const QString & key, QWidget * widget)
+{
+	QHBoxLayout * hLayout = new QHBoxLayout();
+	layout->insertLayout(layout->count()-1, hLayout);
+	QString tmp = key.split('/').last();
+	QLabel * label = new QLabel(tmp, this);
+	label->setObjectName(key+"/label");
+	label->setToolTip(QString("<FONT>%1 [default=%2]</FONT>")
+			.arg(Parameters::getDescription(key.toStdString()).c_str())
+			.arg(uValue(Parameters::getDefaultParameters(), key.toStdString(), std::string("?")).c_str()));
+	label->setTextInteractionFlags(Qt::TextSelectableByMouse);
+	hLayout->addWidget(label);
+	hLayout->addWidget(widget);
+}
+
+void ParametersToolBox::changeParameter(const QString & value)
+{
+	if(sender())
+	{
+		parameters_.at(sender()->objectName().toStdString()) = value.toStdString();
+		QStringList paramChanged;
+		paramChanged.append(sender()->objectName());
+		Q_EMIT parametersChanged(paramChanged);
+	}
+}
+void ParametersToolBox::changeParameter()
+{
+	if(sender())
+	{
+		QDoubleSpinBox * doubleSpinBox = qobject_cast<QDoubleSpinBox*>(sender());
+		QSpinBox * spinBox = qobject_cast<QSpinBox*>(sender());
+		QLineEdit * lineEdit = qobject_cast<QLineEdit*>(sender());
+		if(doubleSpinBox)
+		{
+			parameters_.at(sender()->objectName().toStdString()) = uNumber2Str(doubleSpinBox->value());
+		}
+		else if(spinBox)
+		{
+			parameters_.at(sender()->objectName().toStdString()) = uNumber2Str(spinBox->value());
+		}
+		else if(lineEdit)
+		{
+			parameters_.at(sender()->objectName().toStdString()) =  lineEdit->text().toStdString();
+		}
+		QStringList paramChanged;
+		paramChanged.append(sender()->objectName());
+		Q_EMIT parametersChanged(paramChanged);
+	}
+}
+
+void ParametersToolBox::changeParameter(const int & value)
+{
+	if(sender())
+	{
+		QStringList paramChanged;
+		QComboBox * comboBox = qobject_cast<QComboBox*>(sender());
+		QCheckBox * checkBox = qobject_cast<QCheckBox*>(sender());
+		if(comboBox)
+		{
+			QStringList items;
+			for(int i=0; i<comboBox->count(); ++i)
+			{
+				items.append(comboBox->itemText(i));
+			}
+			QString merged = QString::number(value) + QString(":") + items.join(";");
+			parameters_.at(sender()->objectName().toStdString()) = merged.toStdString();
+
+			this->updateParametersVisibility();
+		}
+		else if(checkBox)
+		{
+			parameters_.at(sender()->objectName().toStdString()) = uBool2Str(value==Qt::Checked?true:false);
+		}
+
+		paramChanged.append(sender()->objectName());
+		Q_EMIT parametersChanged(paramChanged);
+	}
+}
+
+} // namespace find_object

rtabmap/guilib/src/PdfPlot.cpp

@@ -0,0 +1,176 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/PdfPlot.h"
+#include <rtabmap/utilite/ULogger.h>
+#include "rtabmap/utilite/UCv2Qt.h"
+#include "rtabmap/core/util3d.h"
+
+namespace rtabmap {
+
+PdfPlotItem::PdfPlotItem(float dataX, float dataY, float width, int childCount) :
+	UPlotItem(dataX, dataY, width),
+	_img(0),
+	_signaturesRef(0),
+	_text(0)
+{
+	setLikelihood(dataX, dataY, childCount);
+}
+
+PdfPlotItem::~PdfPlotItem()
+{
+
+}
+
+void PdfPlotItem::setLikelihood(int id, float value, int childCount)
+{
+	if(_img && id != this->data().x())
+	{
+		delete _img;
+		_img = 0;
+	}
+	this->setData(QPointF(id, value));
+	_childCount = childCount;
+}
+
+void PdfPlotItem::showDescription(bool shown)
+{
+	if(!_text)
+	{
+		_text = new QGraphicsTextItem(this);
+		_text->setVisible(false);
+	}
+	if(shown)
+	{
+		if(!_img && _signaturesRef)
+		{
+			QImage img;
+			QMap<int, Signature>::const_iterator iter = _signaturesRef->find(int(this->data().x()));
+			if(iter != _signaturesRef->constEnd() && !iter.value().sensorData().imageCompressed().empty())
+			{
+				cv::Mat image;
+				iter.value().sensorData().uncompressDataConst(&image, 0, 0);
+				if(!image.empty())
+				{
+					img = uCvMat2QImage(image);
+					QPixmap scaled = QPixmap::fromImage(img).scaledToWidth(128);
+					_img = new QGraphicsPixmapItem(scaled, this);
+					_img->setVisible(false);
+				}
+			}
+		}
+
+		if(_img)
+			_text->setPos(this->mapFromScene(4+150,0));
+		else
+			_text->setPos(this->mapFromScene(4,0));
+		if(_childCount >= 0)
+		{
+			_text->setPlainText(QString("ID = %1\nValue = %2\nWeight = %3").arg(this->data().x()).arg(this->data().y()).arg(_childCount));
+		}
+		else
+		{
+			_text->setPlainText(QString("ID = %1\nValue = %2").arg(this->data().x()).arg(this->data().y()));
+		}
+		_text->setVisible(true);
+		if(_img)
+		{
+			_img->setPos(this->mapFromScene(4,0));
+			_img->setVisible(true);
+		}
+	}
+	else
+	{
+		_text->setVisible(false);
+		if(_img)
+			_img->setVisible(false);
+	}
+	UPlotItem::showDescription(shown);
+}
+
+
+
+
+
+PdfPlotCurve::PdfPlotCurve(const QString & name, const QMap<int, Signature> * signaturesMapRef = 0, QObject * parent) :
+	UPlotCurve(name, parent),
+	_signaturesMapRef(signaturesMapRef)
+{
+
+}
+
+PdfPlotCurve::~PdfPlotCurve()
+{
+
+}
+
+void PdfPlotCurve::clear()
+{
+	UPlotCurve::clear();
+}
+
+void PdfPlotCurve::setData(const QMap<int, float> & dataMap, const QMap<int, int> & weightsMap)
+{
+	ULOGGER_DEBUG("dataMap=%d, weightsMap=%d", dataMap.size(), weightsMap.size());
+	if(dataMap.size() > 0)
+	{
+		//match the size of the current data
+		int margin = int((_items.size()+1)/2) - dataMap.size();
+
+		while(margin < 0)
+		{
+			PdfPlotItem * newItem = new PdfPlotItem(0, 0, 2, 0);
+			newItem->setSignaturesRef(_signaturesMapRef);
+			this->_addValue(newItem);
+			++margin;
+		}
+
+		while(margin > 0)
+		{
+			this->removeItem(0);
+			--margin;
+		}
+
+		ULOGGER_DEBUG("itemsize=%d", _items.size());
+
+		// update values
+		QList<QGraphicsItem*>::iterator iter = _items.begin();
+		for(QMap<int, float>::const_iterator i=dataMap.begin(); i!=dataMap.end(); ++i)
+		{
+			((PdfPlotItem*)*iter)->setLikelihood(i.key(),  i.value(), weightsMap.value(i.key(), -1));
+			//2 times...
+			++iter;
+			++iter;
+		}
+
+		//reset minMax, this will force the plot to update the axes
+		this->updateMinMax();
+		Q_EMIT dataChanged(this);
+	}
+}
+
+}

rtabmap/guilib/src/PostProcessingDialog.cpp

@@ -0,0 +1,358 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/PostProcessingDialog.h"
+#include "ui_postProcessingDialog.h"
+
+#include <QPushButton>
+#include <QMessageBox>
+#include <rtabmap/core/Optimizer.h>
+
+namespace rtabmap {
+
+PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
+	QDialog(parent)
+{
+	_ui = new Ui_PostProcessingDialog();
+	_ui->setupUi(this);
+
+	if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA) &&
+		!Optimizer::isAvailable(Optimizer::kTypeG2O) &&
+		!Optimizer::isAvailable(Optimizer::kTypeCeres))
+	{
+		_ui->sba->setEnabled(false);
+		_ui->sba->setChecked(false);
+	}
+	else
+	{
+		if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+		{
+			_ui->comboBox_sbaType->setItemData(1, 0, Qt::UserRole - 1);
+			_ui->comboBox_sbaType->setCurrentIndex(0);
+		}
+		if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+		{
+			_ui->comboBox_sbaType->setItemData(0, 0, Qt::UserRole - 1);
+			_ui->comboBox_sbaType->setCurrentIndex(1);
+		}
+		if(!Optimizer::isAvailable(Optimizer::kTypeCeres))
+		{
+			_ui->comboBox_sbaType->setItemData(2, 0, Qt::UserRole - 1);
+			_ui->comboBox_sbaType->setCurrentIndex(1);
+		}
+	}
+
+	restoreDefaults();
+
+	connect(_ui->buttonBox, SIGNAL(clicked(QAbstractButton *)), this, SLOT(closeDialog(QAbstractButton *)));
+
+	connect(_ui->detectMoreLoopClosures, SIGNAL(clicked(bool)), this, SLOT(updateButtonBox()));
+	connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
+	connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
+	connect(_ui->sba, SIGNAL(clicked(bool)), this, SLOT(updateButtonBox()));
+	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+
+	connect(_ui->detectMoreLoopClosures, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
+	connect(_ui->clusterRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->clusterAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->intraSession, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->interSession, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+
+	connect(_ui->sba, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
+	connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateVisibility()));
+	connect(_ui->sba_rematchFeatures, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+
+	updateVisibility();
+}
+
+PostProcessingDialog::~PostProcessingDialog()
+{
+	delete _ui;
+}
+
+void PostProcessingDialog::closeDialog ( QAbstractButton * button )
+{
+	UDEBUG("");
+
+	QDialogButtonBox::ButtonRole role = _ui->buttonBox->buttonRole(button);
+	switch(role)
+	{
+	case QDialogButtonBox::RejectRole:
+		this->reject();
+		break;
+
+	case QDialogButtonBox::AcceptRole:
+		if(validateForm())
+		{
+			this->accept();
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+bool PostProcessingDialog::validateForm()
+{
+	if(_ui->detectMoreLoopClosures->isChecked() && !this->intraSession() && !this->interSession())
+	{
+		QMessageBox::warning(this, tr("Configuration error"), tr("Intra-session and inter-session parameters cannot be both disabled at the same time. Please select one (or both)."));
+		return false;
+	}
+	return true;
+}
+
+void PostProcessingDialog::updateVisibility()
+{
+	_ui->sba_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
+	_ui->label_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
+}
+
+void PostProcessingDialog::saveSettings(QSettings & settings, const QString & group) const
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	settings.setValue("detect_more_lc", this->isDetectMoreLoopClosures());
+	settings.setValue("cluster_radius", this->clusterRadius());
+	settings.setValue("cluster_angle", this->clusterAngle());
+	settings.setValue("iterations", this->iterations());
+	settings.setValue("intra_session", this->intraSession());
+	settings.setValue("inter_session", this->interSession());
+	settings.setValue("refine_neigbors", this->isRefineNeighborLinks());
+	settings.setValue("refine_lc", this->isRefineLoopClosureLinks());
+	settings.setValue("sba", this->isSBA());
+	settings.setValue("sba_iterations", this->sbaIterations());
+	settings.setValue("sba_type", this->sbaType());
+	settings.setValue("sba_variance", this->sbaVariance());
+	settings.setValue("sba_rematch_features", this->sbaRematchFeatures());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void PostProcessingDialog::loadSettings(QSettings & settings, const QString & group)
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	this->setDetectMoreLoopClosures(settings.value("detect_more_lc", this->isDetectMoreLoopClosures()).toBool());
+	this->setClusterRadius(settings.value("cluster_radius", this->clusterRadius()).toDouble());
+	this->setClusterAngle(settings.value("cluster_angle", this->clusterAngle()).toDouble());
+	this->setIterations(settings.value("iterations", this->iterations()).toInt());
+	this->setIntraSession(settings.value("intra_session", this->intraSession()).toBool());
+	this->setInterSession(settings.value("inter_session", this->interSession()).toBool());
+	this->setRefineNeighborLinks(settings.value("refine_neigbors", this->isRefineNeighborLinks()).toBool());
+	this->setRefineLoopClosureLinks(settings.value("refine_lc", this->isRefineLoopClosureLinks()).toBool());
+	this->setSBA(settings.value("sba", this->isSBA()).toBool());
+	this->setSBAIterations(settings.value("sba_iterations", this->sbaIterations()).toInt());
+	this->setSBAType((Optimizer::Type)settings.value("sba_type", this->sbaType()).toInt());
+	this->setSBAVariance(settings.value("sba_variance", this->sbaVariance()).toDouble());
+	this->setSBARematchFeatures(settings.value("sba_rematch_features", this->sbaRematchFeatures()).toBool());
+
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void PostProcessingDialog::restoreDefaults()
+{
+	setDetectMoreLoopClosures(true);
+	setClusterRadius(1);
+	setClusterAngle(30);
+	setIterations(5);
+	setIntraSession(true);
+	setInterSession(true);
+	setRefineNeighborLinks(false);
+	setRefineLoopClosureLinks(false);
+	setSBA(false);
+	setSBAIterations(20);
+	Optimizer::Type sbaType = Optimizer::kTypeG2O; // g2o
+	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		if(Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+		{
+			sbaType = Optimizer::kTypeCVSBA;
+		}
+		else if(Optimizer::isAvailable(Optimizer::kTypeCeres))
+		{
+			sbaType = Optimizer::kTypeCeres;
+		}
+	}
+	setSBAType(sbaType);
+	setSBAVariance(1.0);
+	setSBARematchFeatures(true);
+}
+
+void PostProcessingDialog::updateButtonBox()
+{
+	_ui->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(
+			isDetectMoreLoopClosures() || isRefineNeighborLinks() || isRefineLoopClosureLinks() || isSBA());
+}
+
+bool PostProcessingDialog::isDetectMoreLoopClosures() const
+{
+	return _ui->detectMoreLoopClosures->isChecked();
+}
+
+double PostProcessingDialog::clusterRadius() const
+{
+	return _ui->clusterRadius->value();
+}
+
+double PostProcessingDialog::clusterAngle() const
+{
+	return _ui->clusterAngle->value();
+}
+
+int PostProcessingDialog::iterations() const
+{
+	return _ui->iterations->value();
+}
+
+bool PostProcessingDialog::intraSession() const
+{
+	return _ui->intraSession->isChecked();
+}
+
+bool PostProcessingDialog::interSession() const
+{
+	return _ui->interSession->isChecked();
+}
+
+bool PostProcessingDialog::isRefineNeighborLinks() const
+{
+	return _ui->refineNeighborLinks->isChecked();
+}
+
+bool PostProcessingDialog::isRefineLoopClosureLinks() const
+{
+	return _ui->refineLoopClosureLinks->isChecked();
+}
+
+bool PostProcessingDialog::isSBA() const
+{
+	return _ui->sba->isEnabled() && _ui->sba->isChecked();
+}
+
+int PostProcessingDialog::sbaIterations() const
+{
+	return _ui->sba_iterations->value();
+}
+double PostProcessingDialog::sbaVariance() const
+{
+	return _ui->sba_variance->value();
+}
+Optimizer::Type PostProcessingDialog::sbaType() const
+{
+	return _ui->comboBox_sbaType->currentIndex()==2?Optimizer::kTypeCeres:_ui->comboBox_sbaType->currentIndex()==1?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O;
+}
+bool PostProcessingDialog::sbaRematchFeatures() const
+{
+	return _ui->sba_rematchFeatures->isChecked();
+}
+
+//setters
+void PostProcessingDialog::setDetectMoreLoopClosures(bool on)
+{
+	_ui->detectMoreLoopClosures->setChecked(on);
+}
+void PostProcessingDialog::setClusterRadius(double radius)
+{
+	_ui->clusterRadius->setValue(radius);
+}
+void PostProcessingDialog::setClusterAngle(double angle)
+{
+	_ui->clusterAngle->setValue(angle);
+}
+void PostProcessingDialog::setIterations(int iterations)
+{
+	_ui->iterations->setValue(iterations);
+}
+void PostProcessingDialog::setIntraSession(bool enabled)
+{
+	_ui->intraSession->setChecked(enabled);
+}
+void PostProcessingDialog::setInterSession(bool enabled)
+{
+	_ui->interSession->setChecked(enabled);
+}
+void PostProcessingDialog::setRefineNeighborLinks(bool on)
+{
+	_ui->refineNeighborLinks->setChecked(on);
+}
+void PostProcessingDialog::setRefineLoopClosureLinks(bool on)
+{
+	_ui->refineLoopClosureLinks->setChecked(on);
+}
+void PostProcessingDialog::setSBA(bool on)
+{
+	_ui->sba->setChecked((
+			Optimizer::isAvailable(Optimizer::kTypeCVSBA) ||
+			Optimizer::isAvailable(Optimizer::kTypeG2O) ||
+			Optimizer::isAvailable(Optimizer::kTypeCeres)) && on);
+}
+void PostProcessingDialog::setSBAIterations(int iterations)
+{
+	_ui->sba_iterations->setValue(iterations);
+}
+void PostProcessingDialog::setSBAVariance(double variance)
+{
+	_ui->sba_variance->setValue(variance);
+}
+void PostProcessingDialog::setSBAType(Optimizer::Type type)
+{
+	if(type == Optimizer::kTypeCeres)
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(2);
+	}
+	else if(type == Optimizer::kTypeCVSBA)
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(1);
+	}
+	else
+	{
+		_ui->comboBox_sbaType->setCurrentIndex(0);
+	}
+}
+void PostProcessingDialog::setSBARematchFeatures(bool value)
+{
+	_ui->sba_rematchFeatures->setChecked(value);
+}
+
+
+}

rtabmap/guilib/src/ProgressDialog.cpp

@@ -0,0 +1,189 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/ProgressDialog.h"
+#include <QLayout>
+#include <QProgressBar>
+#include <QTextEdit>
+#include <QLabel>
+#include <QPushButton>
+#include <QtGui/QCloseEvent>
+#include <QCheckBox>
+#include <QtCore/QTimer>
+#include <QtCore/QTime>
+#include <QScrollBar>
+#include "rtabmap/utilite/ULogger.h"
+
+namespace rtabmap {
+
+ProgressDialog::ProgressDialog(QWidget *parent, Qt::WindowFlags flags) :
+		QDialog(parent, flags),
+		_delayedClosingTime(1),
+		_canceled(false)
+{
+	_text = new QLabel(this);
+	_text->setWordWrap(true);
+	_progressBar = new QProgressBar(this);
+	_progressBar->setMaximum(1);
+	_detailedText  = new QTextEdit(this);
+	_detailedText->setReadOnly(true);
+	_detailedText->setLineWrapMode(QTextEdit::NoWrap);
+	_closeButton = new QPushButton(this);
+	_closeButton->setText("Close");
+	_cancelButton = new QPushButton(this);
+	_cancelButton->setText("Cancel");
+	_cancelButton-> setVisible(false);
+	_closeWhenDoneCheckBox = new QCheckBox(this);
+	_closeWhenDoneCheckBox->setChecked(true);
+	_closeWhenDoneCheckBox->setText("Close when done.");
+	_endMessage = "Finished!";
+	this->clear();
+	connect(_closeButton, SIGNAL(clicked()), this, SLOT(close()));
+	connect(_cancelButton, SIGNAL(clicked()), this, SLOT(cancel()));
+
+	QVBoxLayout * layout = new QVBoxLayout(this);
+	layout->addWidget(_text);
+	layout->addWidget(_progressBar);
+	layout->addWidget(_detailedText);
+	QHBoxLayout * hLayout = new QHBoxLayout();
+	layout->addLayout(hLayout);
+	hLayout->addWidget(_closeWhenDoneCheckBox);
+	hLayout->addStretch();
+	hLayout->addWidget(_cancelButton);
+	hLayout->addWidget(_closeButton);
+	this->setLayout(layout);
+
+	this->setModal(true);
+}
+
+ProgressDialog::~ProgressDialog()
+{
+
+}
+
+void ProgressDialog::setAutoClose(bool on, int delayedClosingTimeSec)
+{
+	if(delayedClosingTimeSec >= 0)
+	{
+		_delayedClosingTime = delayedClosingTimeSec;
+	}
+	_closeWhenDoneCheckBox->setChecked(on);
+}
+
+void ProgressDialog::setCancelButtonVisible(bool visible)
+{
+	_cancelButton->setVisible(visible);
+}
+
+void ProgressDialog::appendText(const QString & text, const QColor & color)
+{
+	UDEBUG(text.toStdString().c_str());
+	_text->setText(text);
+	QString html = tr("<html><font color=\"#999999\">%1 </font><font color=\"%2\">%3</font></html>").arg(QTime::currentTime().toString("HH:mm:ss")).arg(color.name()).arg(text);
+	_detailedText->append(html);
+	_detailedText->ensureCursorVisible();
+	_detailedText->horizontalScrollBar()->setSliderPosition(0);
+	_detailedText->verticalScrollBar()->setSliderPosition(_detailedText->verticalScrollBar()->maximum());
+}
+void ProgressDialog::setValue(int value)
+{
+	_progressBar->setValue(value);
+	if(value == _progressBar->maximum())
+	{
+		_text->setText(_endMessage);
+		_closeButton->setEnabled(true);
+		if(_closeWhenDoneCheckBox->isChecked() && _delayedClosingTime == 0)
+		{
+			this->close();
+		}
+		else if(_closeWhenDoneCheckBox->isChecked())
+		{
+			QTimer::singleShot(_delayedClosingTime*1000, this, SLOT(closeDialog()));
+		}
+	}
+}
+int ProgressDialog::maximumSteps() const
+{
+	return _progressBar->maximum();
+}
+void ProgressDialog::setMaximumSteps(int steps)
+{
+	_progressBar->setMaximum(steps);
+}
+
+void ProgressDialog::incrementStep(int steps)
+{
+	//incremental progress bar (if we don't know how many items will be added)
+	if(_progressBar->value() >= _progressBar->maximum()-steps)
+	{
+		_progressBar->setMaximum(_progressBar->maximum()+steps+1);
+	}
+	_progressBar->setValue(_progressBar->value()+steps);
+}
+
+void ProgressDialog::clear()
+{
+	_text->clear();
+	_detailedText->clear();
+	resetProgress();
+}
+
+void ProgressDialog::resetProgress()
+{
+	_progressBar->reset();
+	_closeButton->setEnabled(false);
+	_canceled = false;
+}
+
+void ProgressDialog::closeDialog()
+{
+	if(_closeWhenDoneCheckBox->isChecked())
+	{
+		close();
+	}
+}
+
+void ProgressDialog::closeEvent(QCloseEvent *event)
+{
+	if(_progressBar->value() == _progressBar->maximum())
+	{
+		_canceled = false;
+		event->accept();
+	}
+	else
+	{
+		event->ignore();
+	}
+}
+
+void ProgressDialog::cancel()
+{
+	_canceled = true;
+	Q_EMIT canceled();
+}
+
+}

rtabmap/guilib/src/StatsToolBox.cpp

@@ -0,0 +1,679 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/StatsToolBox.h"
+
+#include <QHBoxLayout>
+#include <QVBoxLayout>
+#include <QGridLayout>
+#include <QMenu>
+#include <QLabel>
+#include <QToolButton>
+#include <QtCore/QChildEvent>
+#include <QtCore/QDir>
+#include <QtGui/QContextMenuEvent>
+#include <QToolBox>
+#include <QDialog>
+
+#include "rtabmap/utilite/UPlot.h"
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+StatItem::StatItem(const QString & name, bool cacheOn, const std::vector<qreal> & x, const std::vector<qreal> & y, const QString & unit, const QMenu * menu, QGridLayout * grid, QWidget * parent) :
+	QWidget(parent),
+	_button(0),
+	_name(0),
+	_value(0),
+	_unit(0),
+	_menu(0),
+	_cacheOn(cacheOn)
+{
+	this->setupUi(grid);
+	_name->setText(name);
+	if(y.size() == 1 || (y.size() > 1 && _cacheOn))
+	{
+		_value->setNum(y[y.size()-1]);
+	}
+	else if(y.size() > 1)
+	{
+		_value->setText("*");
+	}
+	if(cacheOn)
+	{
+		_x = x;
+		_y = y;
+	}
+	_unit->setText(unit);
+	this->updateMenu(menu);
+}
+
+StatItem::~StatItem()
+{
+
+}
+
+void StatItem::clearCache()
+{
+	_x.clear();
+	_y.clear();
+	_value->clear();
+}
+
+void StatItem::addValue(qreal y)
+{
+	if(_cacheOn)
+	{
+		_y.push_back(y);
+	}
+	_value->setText(QString::number(y, 'g', 3));
+	Q_EMIT valueAdded(y);
+}
+
+void StatItem::addValue(qreal x, qreal y)
+{
+	if(_cacheOn)
+	{
+		if (_x.size() && x <_x.back())
+		{
+			clearCache();
+		}
+
+		_y.push_back(y);
+		_x.push_back(x);
+	}
+
+	_value->setText(QString::number(y, 'g', 3));
+	Q_EMIT valueAdded(x,y);
+}
+
+void StatItem::setValues(const std::vector<qreal> & x, const std::vector<qreal> & y)
+{
+	if(_cacheOn)
+	{
+		_x = x;
+		_y = y;
+		if(y.size())
+		{
+			_value->setNum(y[y.size()-1]);
+		}
+	}
+	else
+	{
+		_value->setText("*");
+	}
+	Q_EMIT valuesChanged(x,y);
+}
+
+QString StatItem::value() const
+{
+	return _value->text();
+}
+
+void StatItem::setupUi(QGridLayout * grid)
+{
+	_menu = new QMenu(this);
+	_menu->addMenu("Add to figure...");
+	_button = new QToolButton(this);
+	_button->setIcon(QIcon(":/images/Plot16.png"));
+	_button->setPopupMode(QToolButton::InstantPopup);
+	_button->setMenu(_menu);
+	_name = new QLabel(this);
+	_name->setTextInteractionFlags(Qt::TextSelectableByMouse);
+	_name->setWordWrap(true);
+	_value = new QLabel(this);
+	_value->setTextInteractionFlags(Qt::TextSelectableByMouse);
+	_unit = new QLabel(this);
+
+	if(grid)
+	{
+		int row = grid->rowCount();
+
+		//This fixes an issue where the
+		//button (used on col 0) of the first line in the
+		//toolbox couldn't be clicked
+		grid->addWidget(_button, row, 3);
+		grid->addWidget(_name, row, 0);
+		grid->addWidget(_value, row, 1);
+		grid->addWidget(_unit, row, 2);
+	}
+	else
+	{
+		QHBoxLayout * layout = new QHBoxLayout(this);
+		this->setLayout(layout);
+		layout->addWidget(_button);
+		layout->addWidget(_name);
+		layout->addWidget(_value);
+		layout->addWidget(_unit);
+		layout->addStretch();
+		layout->setContentsMargins(0,0,0,0);
+	}
+}
+
+void StatItem::setCacheOn(bool on)
+{
+	_cacheOn = on;
+	if(!on)
+	{
+		_x.clear();
+		_y.clear();
+	}
+}
+
+void StatItem::updateMenu(const QMenu * menu)
+{
+	_menu->clear();
+	QAction * action;
+	QList<QAction *> actions = menu->actions();
+	QMenu * plotMenu = _menu->addMenu("Add to figure...");
+	for(int i=0; i<actions.size(); ++i)
+	{
+		action = plotMenu->addAction(actions.at(i)->text());
+		connect(action, SIGNAL(triggered()), this, SLOT(preparePlotRequest()));
+	}
+}
+
+void StatItem::preparePlotRequest()
+{
+	QAction * action = qobject_cast<QAction*>(sender());
+	if(action)
+	{
+		Q_EMIT plotRequested(this, action->text());
+	}
+}
+
+
+
+
+
+
+
+
+StatsToolBox::StatsToolBox(QWidget * parent) :
+	QWidget(parent)
+{
+	ULOGGER_DEBUG("");
+	//Statistics in the GUI (for plotting)
+	_statBox = new QToolBox(this);
+	this->setLayout(new QVBoxLayout());
+	this->layout()->setContentsMargins(0,0,0,0);
+	this->layout()->addWidget(_statBox);
+	_statBox->layout()->setSpacing(0);
+	_plotMenu = new QMenu(this);
+	_plotMenu->addAction(tr("<New figure>"));
+	_workingDirectory = QDir::homePath();
+	_newFigureMaxItems = 0;
+}
+
+StatsToolBox::~StatsToolBox()
+{
+	closeFigures();
+}
+
+void StatsToolBox::closeFigures()
+{
+	QMap<QString, QWidget*> figuresTmp = _figures;
+	for(QMap<QString, QWidget*>::iterator iter = figuresTmp.begin(); iter!=figuresTmp.end(); ++iter)
+	{
+		iter.value()->close();
+	}
+}
+
+void StatsToolBox::setCacheOn(bool on)
+{
+	QList<StatItem *> items = _statBox->findChildren<StatItem *>();
+	for(int i=0; i<items.size(); ++i)
+	{
+		items[i]->setCacheOn(on);
+	}
+}
+
+void StatsToolBox::updateStat(const QString & statFullName, bool cacheOn)
+{
+	std::vector<qreal> vx,vy;
+	updateStat(statFullName, vx, vy, cacheOn);
+}
+
+void StatsToolBox::updateStat(const QString & statFullName, qreal y, bool cacheOn)
+{
+	std::vector<qreal> vx,vy(1);
+	vy[0] = y;
+	updateStat(statFullName, vx, vy, cacheOn);
+}
+
+void StatsToolBox::updateStat(const QString & statFullName, qreal x, qreal y, bool cacheOn)
+{
+	std::vector<qreal> vx(1),vy(1);
+	vx[0] = x;
+	vy[0] = y;
+	updateStat(statFullName, vx, vy, cacheOn);
+}
+
+void StatsToolBox::updateStat(const QString & statFullName, const std::vector<qreal> & x, const std::vector<qreal> & y, bool cacheOn)
+{
+	// round qreal to max 2 numbers after the dot
+	//x = (qreal(int(100*x)))/100;
+	//y = (qreal(int(100*y)))/100;
+
+	StatItem * item = _statBox->findChild<StatItem *>(statFullName);
+	if(item)
+	{
+		item->setCacheOn(cacheOn);
+		if(y.size() == 1 && x.size() == 1)
+		{
+			item->addValue(x[0], y[0]);
+		}
+		else if(y.size() == 1 && x.size() == 0)
+		{
+			item->addValue(y[0]);
+		}
+		else
+		{
+			item->setValues(x, y);
+		}
+	}
+	else
+	{
+		// statFullName format : "Grp/Name/unit"
+		QStringList list = statFullName.split('/');
+		QString grp;
+		QString name;
+		QString unit;
+		if(list.size() >= 3)
+		{
+			grp = list.at(0);
+			name = list.at(1);
+			unit = list.at(2);
+			for(int i=3; i<list.size(); ++i)
+			{
+				unit += "/" + list.at(i);
+			}
+		}
+		else if(list.size() == 2)
+		{
+			grp = list.at(0);
+			name = list.at(1);
+		}
+		else if(list.size() == 1)
+		{
+			name = list.at(0);
+		}
+		else
+		{
+			ULOGGER_WARN("A statistic has no name");
+			return;
+		}
+
+		if(grp.isEmpty())
+		{
+			grp = tr("Global");
+		}
+
+		int index = -1;
+		for(int i=0; i<_statBox->count(); ++i)
+		{
+			if(_statBox->itemText(i).compare(grp) == 0)
+			{
+				index = i;
+				break;
+			}
+		}
+
+		if(index<0)
+		{
+			QWidget * newWidget = new QWidget(_statBox);
+			index = _statBox->addItem(newWidget, grp);
+			QVBoxLayout * layout = new QVBoxLayout(newWidget);
+			newWidget->setLayout(layout);
+			QGridLayout * grid = new QGridLayout();
+			grid->setVerticalSpacing(2);
+			grid->setColumnStretch(0, 1);
+			layout->addLayout(grid);
+			layout->addStretch();
+		}
+
+		QVBoxLayout * layout = qobject_cast<QVBoxLayout *>(_statBox->widget(index)->layout());
+		if(!layout)
+		{
+			ULOGGER_ERROR("Layout is null ?!?");
+			return;
+		}
+		QGridLayout * grid = qobject_cast<QGridLayout *>(layout->itemAt(0)->layout());
+		if(!grid)
+		{
+			ULOGGER_ERROR("Layout is null ?!?");
+			return;
+		}
+
+		item = new StatItem(name, cacheOn, x, y, unit, _plotMenu, grid, _statBox->widget(index));
+		item->setObjectName(statFullName);
+
+		//layout->insertWidget(layout->count()-1, item);
+		connect(item, SIGNAL(plotRequested(const StatItem *, const QString &)), this, SLOT(plot(const StatItem *, const QString &)));
+		connect(this, SIGNAL(menuChanged(const QMenu *)), item, SLOT(updateMenu(const QMenu *)));
+	}
+}
+
+void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
+{
+	QWidget * fig = _figures.value(plotName, (QWidget*)0);
+	UPlot * plot = 0;
+	if(fig)
+	{
+		plot = fig->findChild<UPlot *>(plotName);
+	}
+	if(plot)
+	{
+		// if not already in the plot
+		if(!plot->contains(stat->objectName()))
+		{
+			UPlotCurve * curve = new UPlotCurve(stat->objectName(), plot);
+			curve->setPen(plot->getRandomPenColored());
+			connect(stat, SIGNAL(valueAdded(qreal)), curve, SLOT(addValue(qreal)));
+			connect(stat, SIGNAL(valueAdded(qreal, qreal)), curve, SLOT(addValue(qreal, qreal)));
+			connect(stat, SIGNAL(valuesChanged(const std::vector<qreal> &, const std::vector<qreal> &)), curve, SLOT(setData(const std::vector<qreal> &, const std::vector<qreal> &)));
+			if(stat->value().compare("*") == 0)
+			{
+				plot->setMaxVisibleItems(0);
+			}
+			if(!stat->yValues().empty())
+			{
+				if(stat->xValues().size() == stat->yValues().size())
+				{
+					curve->setData(stat->xValues(),stat->yValues());
+				}
+				else
+				{
+					curve->setData(stat->yValues());
+				}
+			}
+			if(!plot->addCurve(curve))
+			{
+				ULOGGER_WARN("Already added to the figure");
+			}
+			Q_EMIT figuresSetupChanged();
+		}
+		else
+		{
+			ULOGGER_WARN("Already added to the figure");
+		}
+		plot->activateWindow();
+	}
+	else
+	{
+		//Create a new plot
+		QString id = tr("Figure 0");
+		if(_plotMenu->actions().size())
+		{
+			id = _plotMenu->actions().last()->text();
+		}
+		id.replace(tr("Figure "), "");
+		QString newPlotName = QString(tr("Figure %1")).arg(id.toInt()+1);
+		//Dock
+		QDialog * figure = new QDialog(0, Qt::Window);
+		_figures.insert(newPlotName, figure);
+		QHBoxLayout * hLayout = new QHBoxLayout(figure);
+		hLayout->setContentsMargins(0,0,0,0);
+		figure->setWindowTitle(newPlotName);
+		figure->setAttribute(Qt::WA_DeleteOnClose, true);
+		connect(figure, SIGNAL(destroyed(QObject*)), this, SLOT(figureDeleted(QObject*)));
+		//Plot
+		UPlot * newPlot = new UPlot(figure);
+		newPlot->setWorkingDirectory(_workingDirectory);
+		newPlot->setMaxVisibleItems(_newFigureMaxItems);
+		newPlot->setObjectName(newPlotName);
+		hLayout->addWidget(newPlot);
+		_plotMenu->addAction(newPlotName);
+		figure->setSizeGripEnabled(true);
+
+		//Add a new curve linked to the statBox
+		UPlotCurve * curve = new UPlotCurve(stat->objectName(), newPlot);
+		curve->setPen(newPlot->getRandomPenColored());
+		connect(stat, SIGNAL(valueAdded(qreal)), curve, SLOT(addValue(qreal)));
+		connect(stat, SIGNAL(valueAdded(qreal, qreal)), curve, SLOT(addValue(qreal, qreal)));
+		connect(stat, SIGNAL(valuesChanged(const std::vector<qreal> &, const std::vector<qreal> &)), curve, SLOT(setData(const std::vector<qreal> &, const std::vector<qreal> &)));
+		if(stat->value().compare("*") == 0)
+		{
+			newPlot->setMaxVisibleItems(0);
+		}
+
+		if(!stat->yValues().empty())
+		{
+			if(stat->xValues().size() == stat->yValues().size())
+			{
+				curve->setData(stat->xValues(),stat->yValues());
+			}
+			else
+			{
+				curve->setData(stat->yValues());
+			}
+		}
+
+		if(!newPlot->addCurve(curve))
+		{
+			ULOGGER_ERROR("Not supposed to be here !?!");
+			delete curve;
+		}
+		figure->show();
+		Q_EMIT figuresSetupChanged();
+
+		Q_EMIT menuChanged(_plotMenu);
+	}
+}
+
+void StatsToolBox::figureDeleted(QObject * obj)
+{
+	if(obj)
+	{
+		QWidget * plot = qobject_cast<QWidget*>(obj);
+		if(plot)
+		{
+			_figures.remove(plot->windowTitle());
+			QList<QAction*> actions = _plotMenu->actions();
+			for(int i=0; i<actions.size(); ++i)
+			{
+				if(actions.at(i)->text().compare(plot->windowTitle()) == 0)
+				{
+					_plotMenu->removeAction(actions.at(i));
+					delete actions[i];
+					Q_EMIT menuChanged(_plotMenu);
+					break;
+				}
+			}
+			Q_EMIT figuresSetupChanged();
+		}
+		else
+		{
+			UERROR("");
+		}
+	}
+	else
+	{
+		UERROR("");
+	}
+}
+
+void StatsToolBox::clear()
+{
+	for (QMap<QString, QWidget*>::iterator i = _figures.begin(); i != _figures.end(); ++i)
+	{
+		QList<UPlot *> plots = i.value()->findChildren<UPlot *>();
+		if (plots.size() == 1)
+		{
+			QStringList names = plots[0]->curveNames();
+			plots[0]->clearData();
+		}
+		else
+        {
+            UERROR("");
+        }
+    }
+    if(_statBox->currentWidget())
+    {
+        QList<StatItem*> items = _statBox->currentWidget()->findChildren<StatItem*>();
+        for (int i = 0; i<items.size(); ++i)
+        {
+            items[i]->clearCache();
+        }
+    }
+}
+
+void StatsToolBox::contextMenuEvent(QContextMenuEvent * event)
+{
+	QMenu topMenu(this);
+	QMenu * menu = topMenu.addMenu(tr("Add all statistics from tab \"%1\" to...").arg(_statBox->itemText(_statBox->currentIndex())));
+	QList<QAction* > actions = _plotMenu->actions();
+	menu->addActions(actions);
+	QAction * aClearFigures = topMenu.addAction(tr("Clear all figures"));
+	QAction * action = topMenu.exec(event->globalPos());
+	QString plotName;
+	if(action)
+	{
+		if(action == aClearFigures)
+		{
+			this->clear();
+		}
+		else
+		{
+			for(int i=0; i<actions.size(); ++i)
+			{
+				if(actions.at(i) == action)
+				{
+					plotName = actions.at(i)->text();
+					break;
+				}
+			}
+		}
+	}
+
+	if(!plotName.isEmpty() && _statBox->currentWidget())
+	{
+		QList<StatItem*> items = _statBox->currentWidget()->findChildren<StatItem*>();
+		for(int i=0; i<items.size(); ++i)
+		{
+			this->plot(items.at(i), plotName);
+			if(plotName.compare(tr("<New figure>")) == 0)
+			{
+				plotName = _plotMenu->actions().last()->text();
+			}
+		}
+	}
+}
+
+void StatsToolBox::getFiguresSetup(QList<int> & curvesPerFigure, QStringList & curveNames, QStringList & curveThresholds)
+{
+	curvesPerFigure.clear();
+	curveNames.clear();
+	curveThresholds.clear();
+	for(QMap<QString, QWidget*>::iterator i=_figures.begin(); i!=_figures.end(); ++i)
+	{
+		QList<UPlot *> plots = i.value()->findChildren<UPlot *>();
+		if(plots.size() == 1)
+		{
+			QStringList names = plots[0]->curveNames();
+			curvesPerFigure.append(names.size());
+			curveNames.append(names);
+			for(int j=0; j<names.size(); ++j)
+			{
+				curveThresholds.append(plots[0]->isThreshold(names[j])?QString::number(plots[0]->getThresholdValue(names[j])):"NA");
+			}
+		}
+		else
+		{
+			UERROR("");
+		}
+	}
+}
+void StatsToolBox::addCurve(const QString & name, bool newFigure, bool cacheOn)
+{
+	StatItem * item = _statBox->findChild<StatItem *>(name);
+	if(!item)
+	{
+		this->updateStat(name, cacheOn);
+		item = _statBox->findChild<StatItem *>(name);
+	}
+
+	if(item)
+	{
+		if(newFigure)
+		{
+			this->plot(item, "");
+		}
+		else
+		{
+			this->plot(item, _plotMenu->actions().last()->text());
+		}
+	}
+	else
+	{
+		ULOGGER_ERROR("Not supposed to be here...");
+	}
+}
+void StatsToolBox::addThreshold(const QString & name, qreal value)
+{
+	QString plotName = _plotMenu->actions().last()->text();
+	QWidget * fig = _figures.value(plotName, (QWidget*)0);
+	if(fig)
+	{
+		UPlot * plot = fig->findChild<UPlot *>(plotName);
+		if(plot)
+			plot->addThreshold(name, value);
+	}
+	else
+	{
+		UERROR("There are no figures, cannot add threshold \"%s\"", name.toStdString().c_str());
+	}
+}
+
+
+void StatsToolBox::setWorkingDirectory(const QString & workingDirectory)
+{
+	if(QDir(workingDirectory).exists())
+	{
+		_workingDirectory = workingDirectory;
+		for(QMap<QString, QWidget*>::iterator i=_figures.begin(); i!=_figures.end(); ++i)
+		{
+			QList<UPlot *> plots = i.value()->findChildren<UPlot *>();
+			if(plots.size() == 1)
+			{
+				plots[0]->setWorkingDirectory(_workingDirectory);
+			}
+			else
+			{
+				UERROR("");
+			}
+		}
+	}
+	else
+	{
+		UWARN("The directory \"%s\" doesn't exist, using \"%s\" instead...",
+				workingDirectory.toStdString().c_str(),
+				_workingDirectory.toStdString().c_str());
+	}
+}
+
+}

rtabmap/guilib/src/chisel_conversions.h

@@ -0,0 +1,159 @@
+/*
+ * chisel_conversions.h
+ *
+ *  Created on: 2018-03-25
+ *      Author: mathieu
+ */
+
+#ifndef CHISEL_CONVERSIONS_H_
+#define CHISEL_CONVERSIONS_H_
+
+#include <rtabmap/core/CameraModel.h>
+#include <open_chisel/Chisel.h>
+#include <pcl/PolygonMesh.h>
+#include <pcl/common/transforms.h>
+
+namespace rtabmap
+{
+
+std::shared_ptr<chisel::ColorImage<unsigned char> > colorImageToChisel(const cv::Mat & image)
+{
+	UASSERT(image.type() == CV_8UC3 || image.type() == CV_8UC4);
+	std::shared_ptr<chisel::ColorImage<unsigned char> > imageChisel(new chisel::ColorImage<unsigned char>(image.cols, image.rows, image.channels()));
+	memcpy(imageChisel->GetMutableData(), image.data, image.total()*sizeof(unsigned char)*image.channels());
+	return imageChisel;
+}
+
+std::shared_ptr<chisel::DepthImage<float> > depthImageToChisel(const cv::Mat & image)
+{
+	UASSERT(image.type() == CV_32FC1);
+	std::shared_ptr<chisel::DepthImage<float> > imageChisel(new chisel::DepthImage<float>(image.cols, image.rows));
+	memcpy(imageChisel->GetMutableData(), (float*)image.data, image.total()*sizeof(float));
+	return imageChisel;
+}
+
+chisel::PinholeCamera cameraModelToChiselCamera(const CameraModel& camera)
+{
+   chisel::PinholeCamera cameraToReturn;
+   chisel::Intrinsics intrinsics;
+   intrinsics.SetFx(camera.fx());
+   intrinsics.SetFy(camera.fy());
+   intrinsics.SetCx(camera.cx());
+   intrinsics.SetCy(camera.cy());
+   cameraToReturn.SetIntrinsics(intrinsics);
+   cameraToReturn.SetWidth(camera.imageWidth());
+   cameraToReturn.SetHeight(camera.imageHeight());
+   return cameraToReturn;
+}
+
+template<typename PointRGBT>
+chisel::PointCloudPtr pointCloudRGBToChisel(const typename pcl::PointCloud<PointRGBT>& cloud, const Transform & transform = Transform::getIdentity())
+{
+	chisel::PointCloudPtr chiselCloud(new chisel::PointCloud());
+	chiselCloud->GetMutablePoints().resize(cloud.size());
+	chiselCloud->GetMutableColors().resize(cloud.size());
+	float byteToFloat = 1.0f / 255.0f;
+	int oi=0;
+	Eigen::Affine3f transformf = transform.toEigen3f();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		const PointRGBT & pt = cloud.at(i);
+		if(pcl::isFinite(pt))
+		{
+			PointRGBT ptt = pcl::transformPoint(pt, transformf);
+
+			chisel::Vec3& xyz =  chiselCloud->GetMutablePoints().at(oi);
+			xyz(0) = ptt.x;
+			xyz(1) = ptt.y;
+			xyz(2) = ptt.z;
+
+			chisel::Vec3& rgb = chiselCloud->GetMutableColors().at(oi);
+			rgb(0) = ptt.r * byteToFloat;
+			rgb(1) = ptt.g * byteToFloat;
+			rgb(2) = ptt.b * byteToFloat;
+
+			++oi;
+		}
+	}
+	chiselCloud->GetMutablePoints().resize(oi);
+	chiselCloud->GetMutableColors().resize(oi);
+	return chiselCloud;
+}
+
+template<typename PointT>
+chisel::PointCloudPtr pointCloudToChisel(const typename pcl::PointCloud<PointT>& cloud, const Transform & transform = Transform::getIdentity())
+{
+	chisel::PointCloudPtr chiselCloud(new chisel::PointCloud());
+	chiselCloud->GetMutablePoints().resize(cloud.size());
+	int oi=0;
+	Eigen::Affine3f transformf = transform.toEigen3f();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		const PointT & pt = cloud.at(i);
+		if(pcl::isFinite(pt))
+		{
+			PointT ptt = pcl::transformPoint(pt, transformf);
+
+			chisel::Vec3& xyz =  chiselCloud->GetMutablePoints().at(oi);
+			xyz(0) = ptt.x;
+			xyz(1) = ptt.y;
+			xyz(2) = ptt.z;
+
+			++oi;
+		}
+	}
+	chiselCloud->GetMutablePoints().resize(oi);
+	return chiselCloud;
+}
+
+pcl::PolygonMesh::Ptr chiselToPolygonMesh(const chisel::MeshMap& meshMap, unsigned char r=100, unsigned char g=100, unsigned char b=100)
+{
+	pcl::PolygonMesh::Ptr mesh (new pcl::PolygonMesh);
+
+	if(meshMap.size())
+	{
+		bool hasColor = meshMap.begin()->second->colors.size();
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		size_t v = 0;
+		for (const std::pair<chisel::ChunkID, chisel::MeshPtr>& it : meshMap)
+		{
+			UASSERT((!hasColor || (it.second->vertices.size() == it.second->colors.size())) &&
+					it.second->vertices.size() == it.second->normals.size());
+			cloud->resize(cloud->size() + it.second->vertices.size());
+
+			mesh->polygons.resize(mesh->polygons.size()+it.second->vertices.size()/3);
+
+			for (unsigned int i=0;i<it.second->vertices.size(); ++i)
+			{
+				pcl::PointXYZRGBNormal & pt = cloud->at(v);
+				pt.x = it.second->vertices[i][0];
+				pt.y = it.second->vertices[i][1];
+				pt.z = it.second->vertices[i][2];
+				if(hasColor)
+				{
+					pt.r = it.second->colors[i][0] * 255.0f;
+					pt.g = it.second->colors[i][1] * 255.0f;
+					pt.b = it.second->colors[i][2] * 255.0f;
+				}
+				else
+				{
+					pt.r = r;
+					pt.g = g;
+					pt.b = b;
+				}
+				pt.normal_x = it.second->normals[i][0];
+				pt.normal_y = it.second->normals[i][1];
+				pt.normal_z = it.second->normals[i][2];
+				pcl::Vertices & polygon = mesh->polygons.at(v/3);
+				polygon.vertices.push_back(v++);
+			}
+		}
+		pcl::toPCLPointCloud2(*cloud, mesh->cloud);
+	}
+	return mesh;
+}
+
+}
+
+
+#endif /* CHISEL_CONVERSIONS_H_ */

rtabmap/guilib/src/opencv/vtkImageMatSource.cpp

@@ -0,0 +1,148 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+// Authors:
+//  * Anatoly Baksheev, Itseez Inc.  myname.mysurname <> mycompany.com
+//
+//M*/
+
+#include "vtkImageMatSource.h"
+#include <vtkImageData.h>
+#include <vtkInformation.h>
+#include <vtkInformationVector.h>
+#include <vtkStreamingDemandDrivenPipeline.h>
+#include <vtkObjectFactory.h>
+#include <vtkVersionMacros.h>
+
+namespace rtabmap {
+	vtkStandardNewMacro(vtkImageMatSource);
+}
+
+rtabmap::vtkImageMatSource::vtkImageMatSource()
+{
+    this->SetNumberOfInputPorts(0);
+    this->ImageData = vtkSmartPointer<vtkImageData>::New();
+}
+
+int rtabmap::vtkImageMatSource::RequestInformation(vtkInformation *, vtkInformationVector**, vtkInformationVector *outputVector)
+{
+    vtkInformation* outInfo = outputVector->GetInformationObject(0);
+
+    outInfo->Set(vtkStreamingDemandDrivenPipeline::WHOLE_EXTENT(), this->ImageData->GetExtent(), 6);
+    outInfo->Set(vtkDataObject::SPACING(), 1.0, 1.0, 1.0);
+    outInfo->Set(vtkDataObject::ORIGIN(),  0.0, 0.0, 0.0);
+
+    vtkDataObject::SetPointDataActiveScalarInfo(outInfo, this->ImageData->GetScalarType(), this->ImageData->GetNumberOfScalarComponents());
+    return 1;
+}
+
+int rtabmap::vtkImageMatSource::RequestData(vtkInformation*, vtkInformationVector**, vtkInformationVector *outputVector)
+{
+     vtkInformation *outInfo = outputVector->GetInformationObject(0);
+
+     vtkImageData *output = vtkImageData::SafeDownCast(outInfo->Get(vtkDataObject::DATA_OBJECT()) );
+     output->ShallowCopy(this->ImageData);
+     return 1;
+}
+
+void rtabmap::vtkImageMatSource::SetImage(cv::InputArray _image)
+{
+    CV_Assert(_image.depth() == CV_8U && (_image.channels() == 1 || _image.channels() == 3 || _image.channels() == 4));
+
+	cv::Mat image = _image.getMat();
+
+    this->ImageData->SetDimensions(image.cols, image.rows, 1);
+#if VTK_MAJOR_VERSION <= 5
+    this->ImageData->SetNumberOfScalarComponents(image.channels());
+    this->ImageData->SetScalarTypeToUnsignedChar();
+    this->ImageData->AllocateScalars();
+#else
+    this->ImageData->AllocateScalars(VTK_UNSIGNED_CHAR, image.channels());
+#endif
+
+    switch(image.channels())
+    {
+    case 1: copyGrayImage(image, this->ImageData); break;
+    case 3: copyRGBImage (image, this->ImageData); break;
+    case 4: copyRGBAImage(image, this->ImageData); break;
+    }
+    this->ImageData->Modified();
+}
+
+void rtabmap::vtkImageMatSource::copyGrayImage(const cv::Mat &source, vtkSmartPointer<vtkImageData> output)
+{
+    unsigned char* dptr = reinterpret_cast<unsigned char*>(output->GetScalarPointer());
+    size_t elem_step = output->GetIncrements()[1]/sizeof(unsigned char);
+
+    for (int y = 0; y < source.rows; ++y)
+    {
+        unsigned char* drow = dptr + elem_step * y;
+        const unsigned char *srow = source.ptr<unsigned char>(source.rows-(y+1)); // vertical flip for texturing
+        for (int x = 0; x < source.cols; ++x)
+            drow[x] = *srow++;
+    }
+}
+
+void rtabmap::vtkImageMatSource::copyRGBImage(const cv::Mat &source, vtkSmartPointer<vtkImageData> output)
+{
+	cv::Vec3b* dptr = reinterpret_cast<cv::Vec3b*>(output->GetScalarPointer());
+    size_t elem_step = output->GetIncrements()[1]/sizeof(cv::Vec3b);
+
+    for (int y = 0; y < source.rows; ++y)
+    {
+		cv::Vec3b* drow = dptr + elem_step * y;
+        const unsigned char *srow = source.ptr<unsigned char>(source.rows - (y + 1)); // vertical flip for texturing
+        for (int x = 0; x < source.cols; ++x, srow += source.channels())
+            drow[x] = cv::Vec3b(srow[2], srow[1], srow[0]);
+    }
+}
+
+void rtabmap::vtkImageMatSource::copyRGBAImage(const cv::Mat &source, vtkSmartPointer<vtkImageData> output)
+{
+    cv::Vec4b* dptr = reinterpret_cast<cv::Vec4b*>(output->GetScalarPointer());
+    size_t elem_step = output->GetIncrements()[1]/sizeof(cv::Vec4b);
+
+    for (int y = 0; y < source.rows; ++y)
+    {
+		cv::Vec4b* drow = dptr + elem_step * y;
+        const unsigned char *srow = source.ptr<unsigned char>(source.rows - (y + 1)); // vertical flip for texturing
+        for (int x = 0; x < source.cols; ++x, srow += source.channels())
+            drow[x] = cv::Vec4b(srow[2], srow[1], srow[0], srow[3]);
+    }
+}

rtabmap/guilib/src/opencv/vtkImageMatSource.h

@@ -0,0 +1,81 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+// Authors:
+//  * Anatoly Baksheev, Itseez Inc.  myname.mysurname <> mycompany.com
+//
+//M*/
+
+#ifndef RTABMAP__vtkImageMatSource_h
+#define RTABMAP__vtkImageMatSource_h
+
+#include <opencv2/core/core.hpp>
+#include <vtkImageAlgorithm.h>
+#include <vtkSmartPointer.h>
+
+namespace rtabmap
+{
+	class vtkImageMatSource : public vtkImageAlgorithm
+	{
+	public:
+		static vtkImageMatSource *New();
+		vtkTypeMacro(vtkImageMatSource, vtkImageAlgorithm);
+
+		void SetImage(cv::InputArray image);
+
+	protected:
+		vtkImageMatSource();
+		~vtkImageMatSource() {}
+
+		vtkSmartPointer<vtkImageData> ImageData;
+
+		int RequestInformation(vtkInformation*, vtkInformationVector**, vtkInformationVector*);
+		int RequestData(vtkInformation*, vtkInformationVector**, vtkInformationVector*);
+	private:
+		vtkImageMatSource(const vtkImageMatSource&);  // Not implemented.
+		void operator=(const vtkImageMatSource&);  // Not implemented.
+
+		static void copyGrayImage(const cv::Mat &source, vtkSmartPointer<vtkImageData> output);
+		static void copyRGBImage(const cv::Mat &source, vtkSmartPointer<vtkImageData> output);
+		static void copyRGBAImage(const cv::Mat &source, vtkSmartPointer<vtkImageData> output);
+	};
+}
+
+
+#endif

rtabmap/guilib/src/qss/default.qss

@@ -0,0 +1,17 @@
+
+/*QGroupBox {
+     background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1,
+                                       stop: 0 #E0E0E0, stop: 1 #FFFFFF);
+     border: 2px solid gray;
+     border-radius: 5px;
+     margin-top: 1ex; /* leave space at the top for the title */
+ }
+
+ QGroupBox::title {
+     subcontrol-origin: margin;
+     subcontrol-position: top center;
+     padding: 0 3px;
+     background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1,
+                                       stop: 0 #FFOECE, stop: 1 #FFFFFF);
+ }
+ */

rtabmap/guilib/src/ui/consoleWidget.ui

@@ -0,0 +1,71 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>consoleWidget</class>
+ <widget class="QWidget" name="consoleWidget">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>400</width>
+    <height>300</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>consoleWidget</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout">
+   <item>
+    <widget class="QTextEdit" name="textEdit">
+     <property name="lineWrapMode">
+      <enum>QTextEdit::NoWrap</enum>
+     </property>
+     <property name="readOnly">
+      <bool>true</bool>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout" stretch="0,0,1">
+     <item>
+      <widget class="QSpinBox" name="spinBox_lines">
+       <property name="suffix">
+        <string> lines</string>
+       </property>
+       <property name="maximum">
+        <number>99999</number>
+       </property>
+       <property name="value">
+        <number>100</number>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QSpinBox" name="spinBox_time">
+       <property name="suffix">
+        <string> ms</string>
+       </property>
+       <property name="maximum">
+        <number>1000</number>
+       </property>
+       <property name="singleStep">
+        <number>100</number>
+       </property>
+       <property name="value">
+        <number>1000</number>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QPushButton" name="pushButton_clear">
+       <property name="text">
+        <string>Clear</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

rtabmap/guilib/src/ui/createSimpleCalibrationDialog.ui

@@ -0,0 +1,397 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>createSimpleCalibrationDialog</class>
+ <widget class="QDialog" name="createSimpleCalibrationDialog">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>563</width>
+    <height>597</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Create simple calibration</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout_3">
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout">
+     <item>
+      <widget class="QComboBox" name="comboBox_advanced">
+       <item>
+        <property name="text">
+         <string>Basic (images are already rectified)</string>
+        </property>
+       </item>
+       <item>
+        <property name="text">
+         <string>Advanced (images should be rectified)</string>
+        </property>
+       </item>
+      </widget>
+     </item>
+     <item>
+      <widget class="QCheckBox" name="checkBox_stereo">
+       <property name="text">
+        <string>Stereo</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <widget class="QStackedWidget" name="stackedWidget">
+     <property name="currentIndex">
+      <number>0</number>
+     </property>
+     <widget class="QWidget" name="page_2">
+      <layout class="QVBoxLayout" name="verticalLayout">
+       <item>
+        <widget class="QGroupBox" name="groupBox">
+         <property name="title">
+          <string>Camera Intrinsics</string>
+         </property>
+         <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
+          <item row="1" column="1">
+           <widget class="QLabel" name="label_2">
+            <property name="text">
+             <string>fy</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_fx">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="1">
+           <widget class="QLabel" name="label_baseline">
+            <property name="text">
+             <string>baseline (m)</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QLabel" name="label_3">
+            <property name="text">
+             <string>cx</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_fy">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="1">
+           <widget class="QLabel" name="label_4">
+            <property name="text">
+             <string>cy</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="1">
+           <widget class="QLabel" name="label">
+            <property name="text">
+             <string>fx</string>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_baseline">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="minimum">
+             <double>0.000000000000000</double>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_cy">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_cx">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+      </layout>
+     </widget>
+     <widget class="QWidget" name="page">
+      <layout class="QVBoxLayout" name="verticalLayout_2">
+       <item>
+        <widget class="QGroupBox" name="groupBox_3">
+         <property name="title">
+          <string>Camera Intrinsics</string>
+         </property>
+         <layout class="QGridLayout" name="gridLayout_3" columnstretch="0,0,1">
+          <item row="1" column="0">
+           <widget class="QLabel" name="label_left">
+            <property name="text">
+             <string>Left</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_fx_r">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_fx_l">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="1">
+           <widget class="QLabel" name="label_right">
+            <property name="text">
+             <string>Right</string>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_cx_l">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="2">
+           <widget class="QLabel" name="label_10">
+            <property name="text">
+             <string>fx</string>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="1">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_fy_r">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_fy_l">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="1">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_cx_r">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="1">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_cy_r">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="0">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_cy_l">
+            <property name="decimals">
+             <number>6</number>
+            </property>
+            <property name="maximum">
+             <double>99999.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="2">
+           <widget class="QLabel" name="label_9">
+            <property name="text">
+             <string>fy</string>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="1">
+           <widget class="QLineEdit" name="lineEdit_D_r"/>
+          </item>
+          <item row="5" column="2">
+           <widget class="QLabel" name="label_8">
+            <property name="text">
+             <string>cy</string>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="0">
+           <widget class="QLineEdit" name="lineEdit_D_l"/>
+          </item>
+          <item row="6" column="2">
+           <widget class="QLabel" name="label_12">
+            <property name="toolTip">
+             <string>k1, k2, p1, p2[, k3[, k4, k5, k6 ]]</string>
+            </property>
+            <property name="text">
+             <string>Distorsion coefficients (4, 5 or 8 values)</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="2">
+           <widget class="QLabel" name="label_11">
+            <property name="text">
+             <string>cx</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+      </layout>
+     </widget>
+    </widget>
+   </item>
+   <item>
+    <widget class="QGroupBox" name="groupBox_2">
+     <property name="title">
+      <string>Image Size</string>
+     </property>
+     <layout class="QGridLayout" name="gridLayout_4" columnstretch="0,1">
+      <item row="0" column="0">
+       <widget class="QSpinBox" name="spinBox_width">
+        <property name="maximum">
+         <number>99999</number>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QLabel" name="label_5">
+        <property name="text">
+         <string>Image width</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QSpinBox" name="spinBox_height">
+        <property name="maximum">
+         <number>99999</number>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="1">
+       <widget class="QLabel" name="label_13">
+        <property name="text">
+         <string>Image height</string>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <widget class="QGroupBox" name="groupBox_stereo_extrinsics">
+     <property name="title">
+      <string>Stereo Camera Extrinsics</string>
+     </property>
+     <layout class="QGridLayout" name="gridLayout_2">
+      <item row="0" column="0" rowspan="2" colspan="2">
+       <widget class="QLineEdit" name="lineEdit_RT">
+        <property name="toolTip">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="2">
+       <widget class="QLabel" name="label_6">
+        <property name="text">
+         <string>Transform between the cameras used for stereo rectification.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <spacer name="verticalSpacer">
+     <property name="orientation">
+      <enum>Qt::Vertical</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>20</width>
+       <height>40</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
+   <item>
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Save</set>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

rtabmap/guilib/src/ui/depthCalibrationDialog.ui

@@ -0,0 +1,590 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DepthCalibrationDialog</class>
+ <widget class="QDialog" name="DepthCalibrationDialog">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>523</width>
+    <height>781</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Depth Calibration</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout">
+   <item>
+    <widget class="QLabel" name="label_111">
+     <property name="text">
+      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;CLAMS approach is used for depth calibration. Please visit &lt;a href=&quot;http://www.alexteichman.com/octo/clams/&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;CLAMS website&lt;/span&gt;&lt;/a&gt; for tips about how to get a good map for depth calibration. If you want to process multiple mapping sessions, uncheck &amp;quot;Reset previous model&amp;quot;. If logger's level is debug, 3D map and generated depth images will be shown during the calibration.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+     </property>
+     <property name="textFormat">
+      <enum>Qt::RichText</enum>
+     </property>
+     <property name="wordWrap">
+      <bool>true</bool>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QGroupBox" name="groupBox_2">
+     <property name="title">
+      <string>Map generation</string>
+     </property>
+     <layout class="QGridLayout" name="gridLayout_3" columnstretch="0,1">
+      <item row="0" column="0">
+       <widget class="QCheckBox" name="checkBox_laserScan">
+        <property name="text">
+         <string/>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QLabel" name="label_110">
+        <property name="text">
+         <string>Use 3D laser scans for 3D map.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QSpinBox" name="spinBox_decimation">
+        <property name="minimum">
+         <number>1</number>
+        </property>
+        <property name="maximum">
+         <number>32</number>
+        </property>
+        <property name="value">
+         <number>1</number>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="1">
+       <widget class="QLabel" name="label_108">
+        <property name="text">
+         <string>Decimation (1-2-4-8-...).</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="0">
+       <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>1</number>
+        </property>
+        <property name="maximum">
+         <double>100.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.100000000000000</double>
+        </property>
+        <property name="value">
+         <double>4.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="1">
+       <widget class="QLabel" name="label_132">
+        <property name="text">
+         <string>Maximum depth (0 means no limit).</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="0">
+       <widget class="QDoubleSpinBox" name="doubleSpinBox_minDepth">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>1</number>
+        </property>
+        <property name="maximum">
+         <double>100.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.100000000000000</double>
+        </property>
+        <property name="value">
+         <double>0.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="1">
+       <widget class="QLabel" name="label_133">
+        <property name="text">
+         <string>Minimum depth.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="4" column="0">
+       <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>3</number>
+        </property>
+        <property name="maximum">
+         <double>1.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.010000000000000</double>
+        </property>
+        <property name="value">
+         <double>0.010000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="4" column="1">
+       <widget class="QLabel" name="label_voxel">
+        <property name="text">
+         <string>Voxel size. Set 0 to disable.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <widget class="QGroupBox" name="groupBox">
+     <property name="title">
+      <string>Distortion Model</string>
+     </property>
+     <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
+      <item row="2" column="0">
+       <widget class="QSpinBox" name="spinBox_bin_width">
+        <property name="suffix">
+         <string> pix</string>
+        </property>
+        <property name="minimum">
+         <number>1</number>
+        </property>
+        <property name="maximum">
+         <number>999</number>
+        </property>
+        <property name="value">
+         <number>8</number>
+        </property>
+       </widget>
+      </item>
+      <item row="6" column="0">
+       <widget class="QSpinBox" name="spinBox_smoothing">
+        <property name="minimum">
+         <number>1</number>
+        </property>
+        <property name="maximum">
+         <number>32</number>
+        </property>
+        <property name="value">
+         <number>1</number>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="1">
+       <widget class="QLabel" name="label_113">
+        <property name="text">
+         <string>Bin width. Should be a multiple of image width.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="4" column="1">
+       <widget class="QLabel" name="label_117">
+        <property name="text">
+         <string>Bin depth.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="1">
+       <widget class="QLabel" name="label_120">
+        <property name="text">
+         <string>Image height.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="1">
+       <widget class="QLabel" name="label_116">
+        <property name="text">
+         <string>Bin height. Should be a multiple of image height.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="0">
+       <widget class="QSpinBox" name="spinBox_bin_height">
+        <property name="suffix">
+         <string> pix</string>
+        </property>
+        <property name="minimum">
+         <number>1</number>
+        </property>
+        <property name="maximum">
+         <number>999</number>
+        </property>
+        <property name="value">
+         <number>6</number>
+        </property>
+       </widget>
+      </item>
+      <item row="5" column="1">
+       <widget class="QLabel" name="label_118">
+        <property name="text">
+         <string>Maximum depth.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="6" column="1">
+       <widget class="QLabel" name="label_115">
+        <property name="text">
+         <string>Smoothing.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QLabel" name="label_119">
+        <property name="text">
+         <string>Image width.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="0">
+       <widget class="QLabel" name="label_width">
+        <property name="text">
+         <string>NA</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_height">
+        <property name="text">
+         <string>NA</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item row="4" column="0">
+       <widget class="QDoubleSpinBox" name="doubleSpinBox_bin_depth">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>1</number>
+        </property>
+        <property name="minimum">
+         <double>0.100000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>10.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>1.000000000000000</double>
+        </property>
+        <property name="value">
+         <double>2.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="5" column="0">
+       <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepthModel">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>0</number>
+        </property>
+        <property name="minimum">
+         <double>1.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>100.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>1.000000000000000</double>
+        </property>
+        <property name="value">
+         <double>10.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
+     <item row="2" column="0">
+      <widget class="QDoubleSpinBox" name="doubleSpinBox_coneRadius">
+       <property name="suffix">
+        <string> m</string>
+       </property>
+       <property name="decimals">
+        <number>2</number>
+       </property>
+       <property name="minimum">
+        <double>0.010000000000000</double>
+       </property>
+       <property name="maximum">
+        <double>1.000000000000000</double>
+       </property>
+       <property name="singleStep">
+        <double>0.010000000000000</double>
+       </property>
+       <property name="value">
+        <double>0.020000000000000</double>
+       </property>
+      </widget>
+     </item>
+     <item row="3" column="0">
+      <widget class="QDoubleSpinBox" name="doubleSpinBox_coneStdDevThresh">
+       <property name="suffix">
+        <string> m</string>
+       </property>
+       <property name="decimals">
+        <number>2</number>
+       </property>
+       <property name="minimum">
+        <double>0.010000000000000</double>
+       </property>
+       <property name="maximum">
+        <double>1.000000000000000</double>
+       </property>
+       <property name="singleStep">
+        <double>0.010000000000000</double>
+       </property>
+       <property name="value">
+        <double>0.030000000000000</double>
+       </property>
+      </widget>
+     </item>
+     <item row="2" column="1">
+      <widget class="QLabel" name="label_voxel_2">
+       <property name="text">
+        <string>Cone radius.</string>
+       </property>
+       <property name="wordWrap">
+        <bool>true</bool>
+       </property>
+       <property name="textInteractionFlags">
+        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+       </property>
+      </widget>
+     </item>
+     <item row="3" column="1">
+      <widget class="QLabel" name="label_voxel_3">
+       <property name="text">
+        <string>Cone standard deviation threshold.</string>
+       </property>
+       <property name="wordWrap">
+        <bool>true</bool>
+       </property>
+       <property name="textInteractionFlags">
+        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+       </property>
+      </widget>
+     </item>
+     <item row="4" column="1">
+      <widget class="QLabel" name="label_109">
+       <property name="text">
+        <string>Reset previous model.</string>
+       </property>
+       <property name="wordWrap">
+        <bool>true</bool>
+       </property>
+       <property name="textInteractionFlags">
+        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+       </property>
+      </widget>
+     </item>
+     <item row="4" column="0">
+      <widget class="QCheckBox" name="checkBox_resetModel">
+       <property name="text">
+        <string/>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout">
+     <item>
+      <widget class="QLabel" name="label_112">
+       <property name="text">
+        <string>Current model:</string>
+       </property>
+       <property name="wordWrap">
+        <bool>true</bool>
+       </property>
+       <property name="textInteractionFlags">
+        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLabel" name="label_trainingSamples">
+       <property name="text">
+        <string>0</string>
+       </property>
+       <property name="wordWrap">
+        <bool>true</bool>
+       </property>
+       <property name="textInteractionFlags">
+        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLabel" name="label_114">
+       <property name="text">
+        <string>training samples</string>
+       </property>
+       <property name="wordWrap">
+        <bool>true</bool>
+       </property>
+       <property name="textInteractionFlags">
+        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <spacer name="verticalSpacer">
+     <property name="orientation">
+      <enum>Qt::Vertical</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>20</width>
+       <height>40</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
+   <item>
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok|QDialogButtonBox::RestoreDefaults|QDialogButtonBox::Save</set>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections>
+  <connection>
+   <sender>buttonBox</sender>
+   <signal>rejected()</signal>
+   <receiver>DepthCalibrationDialog</receiver>
+   <slot>reject()</slot>
+   <hints>
+    <hint type="sourcelabel">
+     <x>316</x>
+     <y>260</y>
+    </hint>
+    <hint type="destinationlabel">
+     <x>286</x>
+     <y>274</y>
+    </hint>
+   </hints>
+  </connection>
+ </connections>
+</ui>

rtabmap/guilib/src/ui/editConstraintDialog.ui

@@ -0,0 +1,459 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>EditConstraintDialog</class>
+ <widget class="QDialog" name="EditConstraintDialog">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>393</width>
+    <height>360</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Edit Constraint</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout">
+   <item>
+    <widget class="QGroupBox" name="groupBox_pose">
+     <property name="title">
+      <string>Pose</string>
+     </property>
+     <layout class="QGridLayout" name="gridLayout" columnstretch="0,1,0,1">
+      <item row="0" column="0">
+       <widget class="QLabel" name="label">
+        <property name="text">
+         <string>x</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QDoubleSpinBox" name="x">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>-9999.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>9999.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="2">
+       <widget class="QLabel" name="label_4">
+        <property name="text">
+         <string>roll</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="3">
+       <widget class="QDoubleSpinBox" name="roll">
+        <property name="suffix">
+         <string> rad</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>-3.150000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>3.150000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_2">
+        <property name="text">
+         <string>y</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="1">
+       <widget class="QDoubleSpinBox" name="y">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>-9999.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>9999.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="2">
+       <widget class="QLabel" name="label_5">
+        <property name="text">
+         <string>pitch</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="3">
+       <widget class="QDoubleSpinBox" name="pitch">
+        <property name="suffix">
+         <string> rad</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>-3.150000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>3.150000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="0">
+       <widget class="QLabel" name="label_3">
+        <property name="text">
+         <string>z</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="1">
+       <widget class="QDoubleSpinBox" name="z">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>-9999.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>9999.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="2">
+       <widget class="QLabel" name="label_6">
+        <property name="text">
+         <string>yaw</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="3">
+       <widget class="QDoubleSpinBox" name="yaw">
+        <property name="suffix">
+         <string> rad</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>-3.150000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>3.150000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <widget class="QGroupBox" name="groupBox_covariance">
+     <property name="title">
+      <string>Covariance</string>
+     </property>
+     <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1,0,1">
+      <item row="0" column="3">
+       <widget class="QDoubleSpinBox" name="angular_sigma_roll">
+        <property name="suffix">
+         <string> rad</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>0.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>3.150000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+        <property name="value">
+         <double>0.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="2">
+       <widget class="QLabel" name="label_10">
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;σ pitch&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="0">
+       <widget class="QLabel" name="label_7">
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;σ  x&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="2">
+       <widget class="QLabel" name="label_8">
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;σ roll&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="3">
+       <widget class="QDoubleSpinBox" name="angular_sigma_pitch">
+        <property name="suffix">
+         <string> rad</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>0.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>3.150000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+        <property name="value">
+         <double>0.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QDoubleSpinBox" name="linear_sigma_x">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>0.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>9999.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+        <property name="value">
+         <double>0.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="3">
+       <widget class="QDoubleSpinBox" name="angular_sigma_yaw">
+        <property name="suffix">
+         <string> rad</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>0.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>3.150000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+        <property name="value">
+         <double>0.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="1">
+       <widget class="QDoubleSpinBox" name="linear_sigma_y">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>0.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>9999.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+        <property name="value">
+         <double>0.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="1">
+       <widget class="QDoubleSpinBox" name="linear_sigma_z">
+        <property name="suffix">
+         <string> m</string>
+        </property>
+        <property name="decimals">
+         <number>6</number>
+        </property>
+        <property name="minimum">
+         <double>0.000000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>9999.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>0.001000000000000</double>
+        </property>
+        <property name="value">
+         <double>0.000000000000000</double>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="2">
+       <widget class="QLabel" name="label_11">
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;σ yaw&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_12">
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;σ y&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="0">
+       <widget class="QLabel" name="label_13">
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;σ z&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <spacer name="verticalSpacer">
+     <property name="orientation">
+      <enum>Qt::Vertical</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>20</width>
+       <height>40</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout">
+     <item>
+      <widget class="QLabel" name="label_9">
+       <property name="text">
+        <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Setting σ to 0 will set 9999 covariance.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+     <item>
+      <widget class="QCheckBox" name="checkBox_radians">
+       <property name="text">
+        <string>Radians</string>
+       </property>
+       <property name="checked">
+        <bool>true</bool>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections>
+  <connection>
+   <sender>buttonBox</sender>
+   <signal>accepted()</signal>
+   <receiver>EditConstraintDialog</receiver>
+   <slot>accept()</slot>
+   <hints>
+    <hint type="sourcelabel">
+     <x>248</x>
+     <y>254</y>
+    </hint>
+    <hint type="destinationlabel">
+     <x>157</x>
+     <y>274</y>
+    </hint>
+   </hints>
+  </connection>
+  <connection>
+   <sender>buttonBox</sender>
+   <signal>rejected()</signal>
+   <receiver>EditConstraintDialog</receiver>
+   <slot>reject()</slot>
+   <hints>
+    <hint type="sourcelabel">
+     <x>316</x>
+     <y>260</y>
+    </hint>
+    <hint type="destinationlabel">
+     <x>286</x>
+     <y>274</y>
+    </hint>
+   </hints>
+  </connection>
+ </connections>
+</ui>

rtabmap/guilib/src/ui/exportBundlerDialog.ui

@@ -0,0 +1,333 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>ExportBundlerDialog</class>
+ <widget class="QDialog" name="ExportBundlerDialog">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>557</width>
+    <height>466</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Export Bundler</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout">
+   <item>
+    <widget class="QGroupBox" name="groupBox">
+     <property name="title">
+      <string>Motion Blur Filtering</string>
+     </property>
+     <layout class="QVBoxLayout" name="verticalLayout_2">
+      <item>
+       <widget class="QLabel" name="label_40">
+        <property name="text">
+         <string>These optional parameters can be used to avoid exporting blurred images. 0 means not used.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+        <property name="textInteractionFlags">
+         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
+        <item row="0" column="0">
+         <widget class="QDoubleSpinBox" name="doubleSpinBox_laplacianVariance">
+          <property name="suffix">
+           <string/>
+          </property>
+          <property name="decimals">
+           <number>0</number>
+          </property>
+          <property name="minimum">
+           <double>0.000000000000000</double>
+          </property>
+          <property name="maximum">
+           <double>9999.000000000000000</double>
+          </property>
+          <property name="value">
+           <double>0.000000000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="0" column="1">
+         <widget class="QLabel" name="label_38">
+          <property name="text">
+           <string>Laplacian variance threshold. Below this threshold, the image is considered blurred. 50 can be good default.</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+          <property name="textInteractionFlags">
+           <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="0">
+         <widget class="QDoubleSpinBox" name="doubleSpinBox_linearSpeed">
+          <property name="suffix">
+           <string> m/s</string>
+          </property>
+          <property name="minimum">
+           <double>0.000000000000000</double>
+          </property>
+          <property name="value">
+           <double>0.100000000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="1">
+         <widget class="QLabel" name="label_39">
+          <property name="text">
+           <string>Maximum linear speed. Images taken on fast motions would be more blurry. 0.1 m/s can be good default.</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+          <property name="textInteractionFlags">
+           <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="0">
+         <widget class="QDoubleSpinBox" name="doubleSpinBox_angularSpeed">
+          <property name="suffix">
+           <string> rad/s</string>
+          </property>
+          <property name="minimum">
+           <double>0.000000000000000</double>
+          </property>
+          <property name="value">
+           <double>0.100000000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="1">
+         <widget class="QLabel" name="label_41">
+          <property name="text">
+           <string>Maximum angular speed. Images taken on fast motions would be more blurry. 0.4 rad/s can be good default.</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+          <property name="textInteractionFlags">
+           <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <widget class="QGroupBox" name="groupBox_export_points">
+     <property name="title">
+      <string>Export 3D Points</string>
+     </property>
+     <property name="checkable">
+      <bool>true</bool>
+     </property>
+     <property name="checked">
+      <bool>false</bool>
+     </property>
+     <layout class="QVBoxLayout" name="verticalLayout_3">
+      <item>
+       <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
+        <item row="1" column="0">
+         <widget class="QSpinBox" name="sba_iterations">
+          <property name="minimum">
+           <number>0</number>
+          </property>
+          <property name="maximum">
+           <number>999999</number>
+          </property>
+          <property name="value">
+           <number>100</number>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="1">
+         <widget class="QLabel" name="label_8">
+          <property name="text">
+           <string>SBA Iterations</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="1">
+         <widget class="QLabel" name="label_variance">
+          <property name="text">
+           <string>SBA Pixel variance used by g2o.</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="0">
+         <widget class="QDoubleSpinBox" name="sba_variance">
+          <property name="suffix">
+           <string/>
+          </property>
+          <property name="decimals">
+           <number>2</number>
+          </property>
+          <property name="minimum">
+           <double>0.010000000000000</double>
+          </property>
+          <property name="maximum">
+           <double>999.000000000000000</double>
+          </property>
+          <property name="singleStep">
+           <double>0.010000000000000</double>
+          </property>
+          <property name="value">
+           <double>1.000000000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="0" column="1">
+         <widget class="QLabel" name="label_10">
+          <property name="text">
+           <string>SBA Type</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="0" column="0">
+         <widget class="QComboBox" name="comboBox_sbaType">
+          <item>
+           <property name="text">
+            <string>g2o</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>cvsba</string>
+           </property>
+          </item>
+         </widget>
+        </item>
+        <item row="3" column="1">
+         <widget class="QLabel" name="label_variance_2">
+          <property name="text">
+           <string>Rematch features.</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="3" column="0">
+         <widget class="QCheckBox" name="sba_rematchFeatures">
+          <property name="text">
+           <string/>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout">
+     <item>
+      <widget class="QLabel" name="label_42">
+       <property name="text">
+        <string>Output folder:</string>
+       </property>
+       <property name="wordWrap">
+        <bool>true</bool>
+       </property>
+       <property name="textInteractionFlags">
+        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLineEdit" name="lineEdit_path">
+       <property name="readOnly">
+        <bool>true</bool>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QToolButton" name="toolButton_path">
+       <property name="text">
+        <string>...</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <spacer name="verticalSpacer">
+     <property name="orientation">
+      <enum>Qt::Vertical</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>20</width>
+       <height>40</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
+   <item>
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok|QDialogButtonBox::RestoreDefaults</set>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections>
+  <connection>
+   <sender>buttonBox</sender>
+   <signal>accepted()</signal>
+   <receiver>ExportBundlerDialog</receiver>
+   <slot>accept()</slot>
+   <hints>
+    <hint type="sourcelabel">
+     <x>248</x>
+     <y>254</y>
+    </hint>
+    <hint type="destinationlabel">
+     <x>157</x>
+     <y>274</y>
+    </hint>
+   </hints>
+  </connection>
+  <connection>
+   <sender>buttonBox</sender>
+   <signal>rejected()</signal>
+   <receiver>ExportBundlerDialog</receiver>
+   <slot>reject()</slot>
+   <hints>
+    <hint type="sourcelabel">
+     <x>316</x>
+     <y>260</y>
+    </hint>
+    <hint type="destinationlabel">
+     <x>286</x>
+     <y>274</y>
+    </hint>
+   </hints>
+  </connection>
+ </connections>
+</ui>