diff --git a/CMakeLists.txt b/CMakeLists.txt index 0e0313ab..13eb18c5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -365,7 +365,7 @@ file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "$ set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include" "${PROJECT_SOURCE_DIR}/guilib/include" "${PROJECT_SOURCE_DIR}/utilite/include") -set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}") +set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}") IF(QT4_FOUND OR Qt5_FOUND) set(CONF_WITH_GUI ON) ELSE() diff --git a/RTABMapConfig.cmake.in b/RTABMapConfig.cmake.in index 18284c90..e24c059f 100644 --- a/RTABMapConfig.cmake.in +++ b/RTABMapConfig.cmake.in @@ -11,41 +11,41 @@ get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") #core lib -find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") -find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") +find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@) +find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@) IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE) SET(RTABMap_CORE debug ${RTABMap_CORE_DEBUG} optimized ${RTABMap_CORE_RELEASE} ) -ELSEIF(RTABMap_CORE_RELEASE) - SET(RTABMap_CORE ${RTABMap_CORE_RELEASE}) ELSEIF(RTABMap_CORE_DEBUG) SET(RTABMap_CORE ${RTABMap_CORE_DEBUG}) +ELSE() + SET(RTABMap_CORE ${RTABMap_CORE_RELEASE}) ENDIF() #utilite lib -find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") -find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") +find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@) +find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@) IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE) SET(RTABMap_UTILITE debug ${RTABMap_UTILITE_DEBUG} optimized ${RTABMap_UTILITE_RELEASE} ) -ELSEIF(RTABMap_UTILITE_RELEASE) - SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE}) ELSEIF(RTABMap_UTILITE_DEBUG) SET(RTABMap_UTILITE ${RTABMap_UTILITE_DEBUG}) +ELSE() + SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE}) ENDIF() set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE}) #gui lib (OFF if RTAB-Map is not built with Qt) if(@CONF_WITH_GUI@) - find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") - find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") + find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@) + find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@) IF(RTABMap_GUI_DEBUG AND RTABMap_GUI_RELEASE) SET(RTABMap_GUI @@ -62,12 +62,8 @@ if(@CONF_WITH_GUI@) endif(@CONF_WITH_GUI@) #backward compatibilities -if(RTABMap_CORE) - set(RTABMAP_CORE ${RTABMap_CORE}) -endif(RTABMap_CORE) -if(RTABMap_UTILITE) - set(RTABMAP_UTILITE ${RTABMap_UTILITE}) -endif(RTABMap_UTILITE) +set(RTABMAP_CORE ${RTABMap_CORE}) +set(RTABMAP_UTILITE ${RTABMap_UTILITE}) if(RTABMap_GUI) set(RTABMAP_GUI ${RTABMap_GUI}) endif(RTABMap_GUI) \ No newline at end of file diff --git a/examples/BOWMapping/CMakeLists.txt b/examples/BOWMapping/CMakeLists.txt index 83404e6e..67d5550c 100644 --- a/examples/BOWMapping/CMakeLists.txt +++ b/examples/BOWMapping/CMakeLists.txt @@ -1,11 +1,36 @@ +cmake_minimum_required(VERSION 2.8) + +IF(DEFINED PROJECT_NAME) + set(internal TRUE) +ENDIF(DEFINED PROJECT_NAME) + +if(internal) + # inside rtabmap project (see below for external build) + SET(RTABMap_INCLUDE_DIRS + ${PROJECT_SOURCE_DIR}/utilite/include + ${PROJECT_SOURCE_DIR}/corelib/include + ) + SET(RTABMap_LIBRARIES + rtabmap_core + rtabmap_utilite + ) +else() + # external build + PROJECT( MyProject ) + + FIND_PACKAGE(RTABMap REQUIRED) + FIND_PACKAGE(OpenCV REQUIRED) + FIND_PACKAGE(PCL 1.7 REQUIRED) +endif() + SET(INCLUDE_DIRS - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/utilite/include + ${RTABMap_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) SET(LIBRARIES + ${RTABMap_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ) @@ -13,7 +38,9 @@ SET(LIBRARIES INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) ADD_EXECUTABLE(bow_mapping main.cpp) -TARGET_LINK_LIBRARIES(bow_mapping rtabmap_core rtabmap_utilite ${LIBRARIES}) +TARGET_LINK_LIBRARIES(bow_mapping ${LIBRARIES}) -SET_TARGET_PROPERTIES( bow_mapping - PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-bow_mapping) \ No newline at end of file +if(internal) + SET_TARGET_PROPERTIES( bow_mapping + PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-bow_mapping) +endif(internal) \ No newline at end of file diff --git a/examples/NoEventsExample/CMakeLists.txt b/examples/NoEventsExample/CMakeLists.txt index df8c9d78..60df1003 100644 --- a/examples/NoEventsExample/CMakeLists.txt +++ b/examples/NoEventsExample/CMakeLists.txt @@ -1,19 +1,40 @@ cmake_minimum_required(VERSION 2.8) -PROJECT( MyProject ) -FIND_PACKAGE(RTABMap REQUIRED) -FIND_PACKAGE(OpenCV REQUIRED) -FIND_PACKAGE(PCL 1.7 REQUIRED) +IF(DEFINED PROJECT_NAME) + set(internal TRUE) +ENDIF(DEFINED PROJECT_NAME) -# Use Qt4 or Qt5 -SET(MY_QT_VERSION 4 CACHE STRING "Which Qt version to use") -IF("${MY_QT_VERSION}" STREQUAL "4") - message("4") - FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) -ELSE() - message("5") - FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg) -ENDIF() +if(internal) + # inside rtabmap project (see below for external build) + SET(RTABMap_INCLUDE_DIRS + ${PROJECT_SOURCE_DIR}/utilite/include + ${PROJECT_SOURCE_DIR}/corelib/include + ${PROJECT_SOURCE_DIR}/guilib/include + ) + SET(RTABMap_LIBRARIES + rtabmap_core + rtabmap_gui + rtabmap_utilite + ) + SET(MY_QT_VERSION ${RTABMAP_QT_VERSION}) +else() + # external build + PROJECT( MyProject ) + + FIND_PACKAGE(RTABMap REQUIRED) + FIND_PACKAGE(OpenCV REQUIRED) + FIND_PACKAGE(PCL 1.7 REQUIRED) + + # Use Qt4 or Qt5 + SET(MY_QT_VERSION 4 CACHE STRING "Which Qt version to use") + IF("${MY_QT_VERSION}" STREQUAL "4") + message("4") + FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) + ELSE() + message("5") + FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg) + ENDIF() +endif() SET(INCLUDE_DIRS ${RTABMap_INCLUDE_DIRS} @@ -44,6 +65,11 @@ ADD_EXECUTABLE(noEventsExample main.cpp ${moc_srcs}) TARGET_LINK_LIBRARIES(noEventsExample ${LIBRARIES}) -SET_TARGET_PROPERTIES( noEventsExample - PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-noEventsExample) +if(internal) + SET_TARGET_PROPERTIES( noEventsExample + PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-noEventsExample) +endif(internal) + + + diff --git a/examples/RGBDMapping/CMakeLists.txt b/examples/RGBDMapping/CMakeLists.txt index 293dd205..712c73d6 100644 --- a/examples/RGBDMapping/CMakeLists.txt +++ b/examples/RGBDMapping/CMakeLists.txt @@ -1,17 +1,53 @@ +cmake_minimum_required(VERSION 2.8) + +IF(DEFINED PROJECT_NAME) + set(internal TRUE) +ENDIF(DEFINED PROJECT_NAME) + +if(internal) + # inside rtabmap project (see below for external build) + SET(RTABMap_INCLUDE_DIRS + ${PROJECT_SOURCE_DIR}/utilite/include + ${PROJECT_SOURCE_DIR}/corelib/include + ${PROJECT_SOURCE_DIR}/guilib/include + ) + SET(RTABMap_LIBRARIES + rtabmap_core + rtabmap_gui + rtabmap_utilite + ) + SET(MY_QT_VERSION ${RTABMAP_QT_VERSION}) +else() + # external build + PROJECT( MyProject ) + + FIND_PACKAGE(RTABMap REQUIRED) + FIND_PACKAGE(OpenCV REQUIRED) + FIND_PACKAGE(PCL 1.7 REQUIRED) + + # Use Qt4 or Qt5 + SET(MY_QT_VERSION 4 CACHE STRING "Which Qt version to use") + IF("${MY_QT_VERSION}" STREQUAL "4") + message("4") + FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) + ELSE() + message("5") + FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg) + ENDIF() +endif() SET(INCLUDE_DIRS - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/guilib/include + ${RTABMap_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) -IF("${RTABMAP_QT_VERSION}" STREQUAL "4") +IF("${MY_QT_VERSION}" STREQUAL "4") INCLUDE(${QT_USE_FILE}) ENDIF() SET(LIBRARIES + ${RTABMap_LIBRARIES} ${OpenCV_LIBRARIES} ${QT_LIBRARIES} ${PCL_LIBRARIES} @@ -19,7 +55,7 @@ SET(LIBRARIES INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) -IF("${RTABMAP_QT_VERSION}" STREQUAL "4") +IF("${MY_QT_VERSION}" STREQUAL "4") QT4_WRAP_CPP(moc_srcs MapBuilder.h) ELSE() QT5_WRAP_CPP(moc_srcs MapBuilder.h) @@ -27,7 +63,9 @@ ENDIF() ADD_EXECUTABLE(rgbd_mapping main.cpp ${moc_srcs}) -TARGET_LINK_LIBRARIES(rgbd_mapping rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES}) +TARGET_LINK_LIBRARIES(rgbd_mapping ${LIBRARIES}) -SET_TARGET_PROPERTIES( rgbd_mapping - PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_mapping) +if(internal) + SET_TARGET_PROPERTIES( rgbd_mapping + PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_mapping) +endif(internal) diff --git a/examples/WifiMapping/CMakeLists.txt b/examples/WifiMapping/CMakeLists.txt index 86f53bd9..b7271056 100644 --- a/examples/WifiMapping/CMakeLists.txt +++ b/examples/WifiMapping/CMakeLists.txt @@ -1,20 +1,53 @@ +cmake_minimum_required(VERSION 2.8) -SET(srcs - main.cpp) +IF(DEFINED PROJECT_NAME) + set(internal TRUE) +ENDIF(DEFINED PROJECT_NAME) + +if(internal) + # inside rtabmap project (see below for external build) + SET(RTABMap_INCLUDE_DIRS + ${PROJECT_SOURCE_DIR}/utilite/include + ${PROJECT_SOURCE_DIR}/corelib/include + ${PROJECT_SOURCE_DIR}/guilib/include + ) + SET(RTABMap_LIBRARIES + rtabmap_core + rtabmap_gui + rtabmap_utilite + ) + SET(MY_QT_VERSION ${RTABMAP_QT_VERSION}) +else() + # external build + PROJECT( MyProject ) + + FIND_PACKAGE(RTABMap REQUIRED) + FIND_PACKAGE(OpenCV REQUIRED) + FIND_PACKAGE(PCL 1.7 REQUIRED) + + # Use Qt4 or Qt5 + SET(MY_QT_VERSION 4 CACHE STRING "Which Qt version to use") + IF("${MY_QT_VERSION}" STREQUAL "4") + message("4") + FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) + ELSE() + message("5") + FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg) + ENDIF() +endif() SET(INCLUDE_DIRS - ${PROJECT_SOURCE_DIR}/utilite/include - ${PROJECT_SOURCE_DIR}/corelib/include - ${PROJECT_SOURCE_DIR}/guilib/include + ${RTABMap_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) -IF("${RTABMAP_QT_VERSION}" STREQUAL "4") +IF("${MY_QT_VERSION}" STREQUAL "4") INCLUDE(${QT_USE_FILE}) ENDIF() SET(LIBRARIES + ${RTABMap_LIBRARIES} ${OpenCV_LIBRARIES} ${QT_LIBRARIES} ${PCL_LIBRARIES} @@ -22,12 +55,15 @@ SET(LIBRARIES INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) -IF("${RTABMAP_QT_VERSION}" STREQUAL "4") - QT4_WRAP_CPP(moc_srcs ../RGBDMapping/MapBuilder.h MapBuilderWifi.h) +IF("${MY_QT_VERSION}" STREQUAL "4") + QT4_WRAP_CPP(moc_srcs MapBuilder.h MapBuilderWifi.h) ELSE() - QT5_WRAP_CPP(moc_srcs ../RGBDMapping/MapBuilder.h MapBuilderWifi.h) + QT5_WRAP_CPP(moc_srcs MapBuilder.h MapBuilderWifi.h) ENDIF() +SET(srcs + main.cpp) + IF(APPLE) FIND_LIBRARY(CoreWLAN_LIBRARY CoreWLAN) FIND_LIBRARY(Foundation_LIBRARY Foundation) @@ -44,7 +80,9 @@ IF(APPLE) ENDIF(APPLE) ADD_EXECUTABLE(wifi_mapping ${srcs} ${moc_srcs}) -TARGET_LINK_LIBRARIES(wifi_mapping rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES}) +TARGET_LINK_LIBRARIES(wifi_mapping ${LIBRARIES}) -SET_TARGET_PROPERTIES( wifi_mapping - PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-wifi_mapping) +if(internal) + SET_TARGET_PROPERTIES( wifi_mapping + PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-wifi_mapping) +endif(internal) diff --git a/examples/WifiMapping/MapBuilder.h b/examples/WifiMapping/MapBuilder.h new file mode 100644 index 00000000..685219b9 --- /dev/null +++ b/examples/WifiMapping/MapBuilder.h @@ -0,0 +1,288 @@ +/* +Copyright (c) 2010-2014, 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. +*/ + +#ifndef MAPBUILDER_H_ +#define MAPBUILDER_H_ + +#include +#include +#include + +#ifndef Q_MOC_RUN // Mac OS X issue +#include "rtabmap/gui/CloudViewer.h" +#include "rtabmap/core/util3d.h" +#include "rtabmap/core/util3d_filtering.h" +#include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/core/RtabmapEvent.h" +#endif +#include "rtabmap/utilite/UStl.h" +#include "rtabmap/utilite/UConversion.h" +#include "rtabmap/utilite/UEventsHandler.h" +#include "rtabmap/utilite/ULogger.h" +#include "rtabmap/core/OdometryEvent.h" +#include "rtabmap/core/CameraThread.h" + +using namespace rtabmap; + +// This class receives RtabmapEvent and construct/update a 3D Map +class MapBuilder : public QWidget, public UEventsHandler +{ + Q_OBJECT +public: + //Camera ownership is not transferred! + MapBuilder(CameraThread * camera = 0) : + camera_(camera), + odometryCorrection_(Transform::getIdentity()), + processingStatistics_(false), + lastOdometryProcessed_(true) + { + this->setWindowFlags(Qt::Dialog); + this->setWindowTitle(tr("3D Map")); + this->setMinimumWidth(800); + this->setMinimumHeight(600); + + cloudViewer_ = new CloudViewer(this); + + QVBoxLayout *layout = new QVBoxLayout(); + layout->addWidget(cloudViewer_); + this->setLayout(layout); + + qRegisterMetaType("rtabmap::OdometryEvent"); + qRegisterMetaType("rtabmap::Statistics"); + + QAction * pause = new QAction(this); + this->addAction(pause); + pause->setShortcut(Qt::Key_Space); + connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection())); + } + + virtual ~MapBuilder() + { + this->unregisterFromEventsManager(); + } + +protected slots: + virtual void pauseDetection() + { + UWARN(""); + if(camera_) + { + if(camera_->isCapturing()) + { + camera_->join(true); + } + else + { + camera_->start(); + } + } + } + + virtual void processOdometry(const rtabmap::OdometryEvent & odom) + { + if(!this->isVisible()) + { + return; + } + + Transform pose = odom.pose(); + if(pose.isNull()) + { + //Odometry lost + cloudViewer_->setBackgroundColor(Qt::darkRed); + + pose = lastOdomPose_; + } + else + { + cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor()); + } + if(!pose.isNull()) + { + lastOdomPose_ = pose; + + // 3d cloud + if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols && + odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows && + !odom.data().depthOrRightRaw().empty() && + (odom.data().stereoCameraModel().isValidForProjection() || odom.data().cameraModels().size())) + { + pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( + odom.data(), + 2, // decimation + 4.0f); // max depth + if(cloud->size()) + { + if(!cloudViewer_->addCloud("cloudOdom", cloud, odometryCorrection_*pose)) + { + UERROR("Adding cloudOdom to viewer failed!"); + } + } + else + { + cloudViewer_->setCloudVisibility("cloudOdom", false); + UWARN("Empty cloudOdom!"); + } + } + + if(!odom.pose().isNull()) + { + // update camera position + cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose()); + } + } + cloudViewer_->update(); + + lastOdometryProcessed_ = true; + } + + + virtual void processStatistics(const rtabmap::Statistics & stats) + { + processingStatistics_ = true; + + //============================ + // Add RGB-D clouds + //============================ + const std::map & poses = stats.poses(); + QMap clouds = cloudViewer_->getAddedClouds(); + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + if(!iter->second.isNull()) + { + std::string cloudName = uFormat("cloud%d", iter->first); + + // 3d point cloud + if(clouds.contains(cloudName)) + { + // Update only if the pose has changed + Transform tCloud; + cloudViewer_->getPose(cloudName, tCloud); + if(tCloud.isNull() || iter->second != tCloud) + { + if(!cloudViewer_->updateCloudPose(cloudName, iter->second)) + { + UERROR("Updating pose cloud %d failed!", iter->first); + } + } + cloudViewer_->setCloudVisibility(cloudName, true); + } + else if(uContains(stats.getSignatures(), iter->first)) + { + Signature s = stats.getSignatures().at(iter->first); + s.sensorData().uncompressData(); // make sure data is uncompressed + // Add the new cloud + pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( + s.sensorData(), + 4, // decimation + 4.0f); // max depth + if(cloud->size()) + { + if(!cloudViewer_->addCloud(cloudName, cloud, iter->second)) + { + UERROR("Adding cloud %d to viewer failed!", iter->first); + } + } + else + { + UWARN("Empty cloud %d!", iter->first); + } + } + } + else + { + UWARN("Null pose for %d ?!?", iter->first); + } + } + + //============================ + // Add 3D graph (show all poses) + //============================ + cloudViewer_->removeAllGraphs(); + cloudViewer_->removeCloud("graph_nodes"); + if(poses.size()) + { + // Set graph + pcl::PointCloud::Ptr graph(new pcl::PointCloud); + pcl::PointCloud::Ptr graphNodes(new pcl::PointCloud); + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z())); + } + *graphNodes = *graph; + + + // add graph + cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray); + cloudViewer_->addCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green); + cloudViewer_->setCloudPointSize("graph_nodes", 5); + } + + odometryCorrection_ = stats.mapCorrection(); + + cloudViewer_->update(); + + processingStatistics_ = false; + } + + virtual void handleEvent(UEvent * event) + { + if(event->getClassName().compare("RtabmapEvent") == 0) + { + RtabmapEvent * rtabmapEvent = (RtabmapEvent *)event; + const Statistics & stats = rtabmapEvent->getStats(); + // Statistics must be processed in the Qt thread + if(this->isVisible()) + { + QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats)); + } + } + else if(event->getClassName().compare("OdometryEvent") == 0) + { + OdometryEvent * odomEvent = (OdometryEvent *)event; + // Odometry must be processed in the Qt thread + if(this->isVisible() && + lastOdometryProcessed_ && + !processingStatistics_) + { + lastOdometryProcessed_ = false; // if we receive too many odometry events! + QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent)); + } + } + } + +protected: + CloudViewer * cloudViewer_; + CameraThread * camera_; + Transform lastOdomPose_; + Transform odometryCorrection_; + bool processingStatistics_; + bool lastOdometryProcessed_; +}; + + +#endif /* MAPBUILDER_H_ */ diff --git a/examples/WifiMapping/MapBuilderWifi.h b/examples/WifiMapping/MapBuilderWifi.h index 77c888a4..767a11ae 100644 --- a/examples/WifiMapping/MapBuilderWifi.h +++ b/examples/WifiMapping/MapBuilderWifi.h @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef MAPBUILDERWIFI_H_ #define MAPBUILDERWIFI_H_ -#include "../RGBDMapping/MapBuilder.h" +#include "MapBuilder.h" #include "rtabmap/core/UserDataEvent.h" using namespace rtabmap;