diff --git a/CMakeLists.txt b/CMakeLists.txt index ad81c2e73b..2ac74a109e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -205,6 +205,7 @@ option(WITH_REALSENSE2 "Include RealSense support" ON) option(WITH_MYNTEYE "Include mynteye-s support" ON) option(WITH_DEPTHAI "Include depthai-core support" OFF) option(WITH_XVSDK "Include XVisio SDK support" OFF) +option(WITH_OUSTER "Include Ouster SDK support" ON) option(WITH_OCTOMAP "Include OctoMap support" ON) option(WITH_GRIDMAP "Include GridMap support" ON) option(WITH_CPUTSDF "Include CPUTSDF support" OFF) @@ -671,6 +672,13 @@ IF(WITH_XVSDK) ENDIF(xvsdk_FOUND) ENDIF(WITH_XVSDK) +IF(WITH_OUSTER) + FIND_PACKAGE(OusterSDK QUIET) + IF(OusterSDK_FOUND) + MESSAGE(STATUS "Found OusterSDK (targets)") + ENDIF(OusterSDK_FOUND) +ENDIF(WITH_OUSTER) + IF(WITH_OCTOMAP) FIND_PACKAGE(octomap QUIET) IF(octomap_FOUND) @@ -1018,6 +1026,9 @@ IF(NOT xvsdk_FOUND) ELSE() SET(CONF_WITH_XVSDK 1) ENDIF() +IF(NOT OusterSDK_FOUND) + SET(OUSTER "//") +ENDIF() IF(NOT octomap_FOUND) SET(OCTOMAP "//") SET(CONF_WITH_OCTOMAP 0) @@ -1674,6 +1685,21 @@ ELSE() MESSAGE(STATUS " With XVisio SDK = NO (xvsdk not found)") ENDIF() +MESSAGE(STATUS "") +MESSAGE(STATUS " LiDAR Drivers:") +IF(PCL_VERSION VERSION_GREATER_EQUAL "1.8.0") +MESSAGE(STATUS " With Velodyne VLP16 = YES") +ELSE() +MESSAGE(STATUS " With Velodyne VLP16 = NO (PCL>=1.8 required)") +ENDIF() +IF(OusterSDK_FOUND) +MESSAGE(STATUS " With Ouster SDK ${OusterSDK_VERSION} = YES") +ELSEIF(NOT WITH_OUSTER) +MESSAGE(STATUS " With Ouster SDK = NO (WITH_OUSTER=OFF)") +ELSE() +MESSAGE(STATUS " With Ouster SDK = NO (OusterSDK not found)") +ENDIF() + MESSAGE(STATUS "") MESSAGE(STATUS " Odometry Approaches:") IF(loam_velodyne_FOUND) diff --git a/Version.h.in b/Version.h.in index 376291f23b..61550049f3 100644 --- a/Version.h.in +++ b/Version.h.in @@ -70,6 +70,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @MYNTEYE@#define RTABMAP_MYNTEYE @DEPTHAI@#define RTABMAP_DEPTHAI @XVSDK@#define RTABMAP_XVSDK +@OUSTER@#define RTABMAP_OUSTER @OCTOMAP@#define RTABMAP_OCTOMAP @GRIDMAP@#define RTABMAP_GRIDMAP @CPUTSDF@#define RTABMAP_CPUTSDF diff --git a/corelib/include/rtabmap/core/IMUFilter.h b/corelib/include/rtabmap/core/IMUFilter.h index 6bd97d4776..7dc756859a 100644 --- a/corelib/include/rtabmap/core/IMUFilter.h +++ b/corelib/include/rtabmap/core/IMUFilter.h @@ -41,7 +41,6 @@ class RTABMAP_CORE_EXPORT IMUFilter kMadgwick=0, kComplementaryFilter=1}; public: - static IMUFilter * create(const ParametersMap & parameters = ParametersMap()); static IMUFilter * create(IMUFilter::Type type, const ParametersMap & parameters = ParametersMap()); public: diff --git a/corelib/include/rtabmap/core/lidar/LidarOuster.h b/corelib/include/rtabmap/core/lidar/LidarOuster.h new file mode 100644 index 0000000000..ab33ffffe2 --- /dev/null +++ b/corelib/include/rtabmap/core/lidar/LidarOuster.h @@ -0,0 +1,72 @@ +/* +Copyright (c) 2010-2024, Mathieu Labbe +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 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 CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDAROUSTER_H_ +#define CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDAROUSTER_H_ + +#include + +namespace rtabmap { + +class IMUFilter; +class OusterCaptureThread; + +class RTABMAP_CORE_EXPORT LidarOuster :public Lidar { +public: + static bool available(); +public: + LidarOuster( + const std::string& ipOrHostnameOrPcapOrOsf, + const std::string& dataDestinationOrJson = "", + int lidarMode = 0, + int timestampMode = 0, + bool useReflectivityForIntensityChannel = true, + bool publishIMU = false, + float frameRate = 0.0f, + Transform localTransform = Transform::getIdentity()); + virtual ~LidarOuster(); + + SensorData takeScan(SensorCaptureInfo * info = 0) {return takeData(info);} + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") override; + virtual std::string getSerial() const override; + +protected: + virtual SensorData captureData(SensorCaptureInfo * info = 0) override; + +private: + OusterCaptureThread * ousterCaptureThread_; + bool imuPublished_; + bool useReflectivityForIntensityChannel_; + std::string ipOrHostnameOrPcapOrOsf_; + std::string dataDestinationOrJson_; + int lidarMode_; + int timestampMode_; + +}; + +} /* namespace rtabmap */ + +#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDAROUSTER_H_ */ diff --git a/corelib/include/rtabmap/core/lidar/LidarVLP16.h b/corelib/include/rtabmap/core/lidar/LidarVLP16.h index bb8df04bdb..51eba9552b 100644 --- a/corelib/include/rtabmap/core/lidar/LidarVLP16.h +++ b/corelib/include/rtabmap/core/lidar/LidarVLP16.h @@ -43,6 +43,8 @@ struct PointXYZIT { }; class RTABMAP_CORE_EXPORT LidarVLP16 :public Lidar, public pcl::VLPGrabber { +public: + static bool available(); public: LidarVLP16( const std::string& pcapFile, diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index f43d818b5c..f946c40199 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -41,6 +41,8 @@ SET(SRC_FILES camera/CameraMyntEye.cpp camera/CameraDepthAI.cpp camera/CameraSeerSense.cpp + + lidar/LidarOuster.cpp EpipolarGeometry.cpp VisualWord.cpp @@ -391,6 +393,15 @@ IF(xvsdk_FOUND) ) ENDIF(xvsdk_FOUND) +IF(OusterSDK_FOUND) + SET(LIBRARIES + ${LIBRARIES} + OusterSDK::ouster_client + OusterSDK::ouster_pcap + OusterSDK::ouster_osf + ) +ENDIF(OusterSDK_FOUND) + IF(TARGET OpenMP::OpenMP_CXX) SET(LIBRARIES ${LIBRARIES} diff --git a/corelib/src/IMUFilter.cpp b/corelib/src/IMUFilter.cpp index a320d36015..dc07367e02 100644 --- a/corelib/src/IMUFilter.cpp +++ b/corelib/src/IMUFilter.cpp @@ -35,13 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -IMUFilter * IMUFilter::create(const ParametersMap & parameters) -{ - int type = Parameters::defaultKpDetectorStrategy(); - Parameters::parse(parameters, Parameters::kKpDetectorStrategy(), type); - return create((IMUFilter::Type)type, parameters); -} - IMUFilter * IMUFilter::create(IMUFilter::Type type, const ParametersMap & parameters) { #ifndef RTABMAP_MADGWICK @@ -62,7 +55,6 @@ IMUFilter * IMUFilter::create(IMUFilter::Type type, const ParametersMap & parame #endif default: filter = new ComplementaryFilter(parameters); - type = IMUFilter::kComplementaryFilter; break; } diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 7d7bd1775a..e56bd643dd 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -646,7 +646,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet dt > 0 && !guess.isNull()) { - UDEBUG("Deskewing begin"); + UDEBUG("Deskewing begin (with imu=%d)", !imus_.empty()?1:0); // Recompute velocity float vx,vy,vz, vroll,vpitch,vyaw; guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw); @@ -672,7 +672,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet Transform imuLastScan = Transform::getTransform(imus_, data.stamp() + data.laserScanRaw().data().ptr(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]); - if(!imuFirstScan.isNull() && !imuLastScan.isNull()) + if(!imuFirstScan.isNull() || !imuLastScan.isNull()) { Transform orientation = imuFirstScan.inverse() * imuLastScan; orientation.getEulerAngles(vroll, vpitch, vyaw); @@ -689,6 +689,18 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet vyaw /= scanTime; } } + if(imuFirstScan.isNull()) + { + UWARN("We are receiving IMUs but we could not find one for the " + "first lidar stamp %f to be used for deskewing.", + data.stamp() + data.laserScanRaw().data().ptr(0, 0)[data.laserScanRaw().getTimeOffset()]); + } + if(imuLastScan.isNull()) + { + UWARN("We are receiving IMUs but we could not find one for the " + "last lidar stamp %f to be used for deskewing.", + data.stamp() + data.laserScanRaw().data().ptr(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]); + } } Transform velocity(vx,vy,vz,vroll,vpitch,vyaw); @@ -703,6 +715,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet if(data.laserScanRaw().isOrganized()) { // Laser scans should be dense passing this point + UDEBUG("Densify scan"); data.setLaserScan(data.laserScanRaw().densify()); } diff --git a/corelib/src/Parameters.cpp b/corelib/src/Parameters.cpp index 0d6d3a22b0..12f6c5a8a6 100644 --- a/corelib/src/Parameters.cpp +++ b/corelib/src/Parameters.cpp @@ -814,6 +814,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; #else std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; +#endif + str = "With Ouster SDK:"; +#ifdef RTABMAP_OUSTER + std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; +#else + std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; #endif str = "With libpointmatcher:"; #ifdef RTABMAP_POINTMATCHER diff --git a/corelib/src/lidar/LidarOuster.cpp b/corelib/src/lidar/LidarOuster.cpp new file mode 100644 index 0000000000..a55b1055f0 --- /dev/null +++ b/corelib/src/lidar/LidarOuster.cpp @@ -0,0 +1,700 @@ +/* +Copyright (c) 2010-2024, Mathieu Labbe +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 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 +#ifdef RTABMAP_OUSTER +// Ouster should be included first to avoid conflicts +// with defines set by Windows.h included indirectly below +#include "ouster/client.h" +#include "ouster/os_pcap.h" +#include "ouster/types.h" +#include "ouster/impl/build.h" +#include "ouster/lidar_scan.h" +#include "ouster/osf/reader.h" +#include "ouster/osf/stream_lidar_scan.h" +#include "ouster/osf/meta_lidar_sensor.h" +#include "ouster/osf/meta_extrinsics.h" +#include "ouster/osf/meta_streaming_info.h" +#endif + +#include +#include +#include +#include +#include +#include + +namespace rtabmap { + +#ifdef RTABMAP_OUSTER +class OusterCaptureThread : public UThread { + public: + OusterCaptureThread( + std::shared_ptr & clientHandle, + const ouster::sensor::sensor_info & info, + bool imuPublished, + bool useReflectivityForIntensityChannel) : + scanBatcher_(info), + pf_(ouster::sensor::get_format(info)), + clientHandle_(clientHandle), + info_(info), + intensityChannel_(useReflectivityForIntensityChannel? + ouster::sensor::ChanField::REFLECTIVITY: + ouster::sensor::ChanField::SIGNAL), + endOfFileReached_(false) + { + UDEBUG(""); + init(imuPublished); + } + OusterCaptureThread( + std::shared_ptr & pcapHandle, + const ouster::sensor::sensor_info & info, + bool imuPublished, + bool useReflectivityForIntensityChannel) : + scanBatcher_(info), + pf_(ouster::sensor::get_format(info)), + pcapHandle_(pcapHandle), + info_(info), + intensityChannel_(useReflectivityForIntensityChannel? + ouster::sensor::ChanField::REFLECTIVITY: + ouster::sensor::ChanField::SIGNAL), + endOfFileReached_(false) + { + UDEBUG(""); + init(imuPublished); + } + + OusterCaptureThread( + std::shared_ptr & osfReader, + const ouster::sensor::sensor_info & info, + bool imuPublished, + bool useReflectivityForIntensityChannel) : + scanBatcher_(info), + pf_(ouster::sensor::get_format(info)), + osfReader_(osfReader), + osfIter_(osfReader_->messages().begin()), + info_(info), + intensityChannel_(useReflectivityForIntensityChannel? + ouster::sensor::ChanField::REFLECTIVITY: + ouster::sensor::ChanField::SIGNAL), + endOfFileReached_(false) + { + UDEBUG(""); + init(imuPublished); + } + + virtual ~OusterCaptureThread() + { + this->join(true); + delete imuFilter_; + } + + const ouster::sensor::sensor_info & getInfo() const { + return info_; + } + + bool getScan(LaserScan & output, double & stamp) + { + if(clientHandle_.get() == 0) + { + // We are reading from file, call mainLoop till a scan is received or end of stream + while(scanReady_.value() == 0 && !endOfFileReached_) + this->mainLoop(); + } + + if(!endOfFileReached_ && !scanReady_.acquire(1, 5000)) + { + UERROR("Not received any frames since 5 seconds, try to restart the camera again."); + } + else if(!endOfFileReached_ || this->isRunning()) + { + ouster::LidarScan scan; + { + UScopeMutex s(scanMutex_); + scan = lastScan_; + lastScan_ = ouster::LidarScan(); + } + + UASSERT(scan.has_field(ouster::sensor::ChanField::RANGE)); + UASSERT(scan.has_field(intensityChannel_)); + + ouster::LidarScan::Points cloud = ouster::cartesian(scan, lut_); + + // Convert to our LaserScan format XYZIT + output = LaserScan(cv::Mat(cv::Size(scan.w,scan.h), CV_32FC(5)), LaserScan::kXYZIT, 0, 0, 0, 0, 0, lidarSensorT_); + auto timestamp = scan.timestamp(); + auto scan_ts = timestamp[scan.w-1]; // stamp with last reading + + //UWARN("Prepare scan %f -> %f", double(timestamp[0])/10e8, double(timestamp[scan.w-1])/10e8); + auto intensityOffset = output.getIntensityOffset(); + auto timeOffset = output.getTimeOffset(); + float bad_point = std::numeric_limits::quiet_NaN (); + ouster::FieldType intensityType = scan.field_type(intensityChannel_==ouster::sensor::ChanField::REFLECTIVITY?"REFLECTIVITY":"SIGNAL"); + for (size_t u = 0; u < scan.h; u++) { + for (size_t v = 0; v < scan.w; v++) { + auto ts = scan_ts-timestamp[v]; + auto index = u*scan.w+v; + if(cloud(index, 0) != 0.0) + { + output.field(index, 0) = cloud(index, 0); + output.field(index, 1) = cloud(index, 1); + output.field(index, 2) = cloud(index, 2); + } + else + { + output.field(index, 0) = output.field(index, 1) = output.field(index, 2) = bad_point; + } + if(intensityType.element_type == ouster::sensor::ChanFieldType::UINT32) + { + output.field(index, intensityOffset) = scan.field(intensityChannel_).data()[index]; + } + else if(intensityType.element_type == ouster::sensor::ChanFieldType::UINT16) + { + output.field(index, intensityOffset) = scan.field(intensityChannel_).data()[index]; + } + else if(intensityType.element_type == ouster::sensor::ChanFieldType::UINT8) + { + output.field(index, intensityOffset) = scan.field(intensityChannel_).data()[index]; + } + else + { + UFATAL("Intensity format %d not supported!", intensityType.element_type); + } + output.field(index, timeOffset) = -float(ts)/10e8; + } + } + stamp = double(scan_ts)/10e8; + return true; + } + return false; + } + + IMU getIMU(double stamp, int maxWaitTimeMs = 100) + { + IMU imu; + if(!imuBuffer_.empty()) + { + imuMutex_.lock(); + int waitTry = 0; + while(maxWaitTimeMs>0 && imuBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs) + { + imuMutex_.unlock(); + ++waitTry; + uSleep(1); + imuMutex_.lock(); + } + + if(imuBuffer_.rbegin()->first < stamp) + { + if(maxWaitTimeMs > 0) + { + UWARN("Could not find imus to interpolate at scan time %f after waiting %d ms (last is %f)...", stamp/1000.0, maxWaitTimeMs, imuBuffer_.rbegin()->first/1000.0); + } + } + else + { + std::map::const_iterator iterB = imuBuffer_.lower_bound(stamp); + std::map::const_iterator iterA = iterB; + if(iterA != imuBuffer_.begin()) + { + iterA = --iterA; + } + if(iterB == imuBuffer_.end()) + { + iterB = --iterB; + } + if(stamp >= iterA->first && stamp <= iterB->first) + { + float t = (stamp-iterA->first) / (iterB->first-iterA->first); + + cv::Vec4d quatA = iterA->second.orientation(); + cv::Vec3d accA = iterA->second.linearAcceleration(); + cv::Vec3d gyroA = iterA->second.angularVelocity(); + cv::Vec4d quatB = iterB->second.orientation(); + cv::Vec3d accB = iterB->second.linearAcceleration(); + cv::Vec3d gyroB = iterB->second.angularVelocity(); + + Eigen::Quaterniond qa(quatA[3], quatA[0], quatA[1], quatA[2]); + Eigen::Quaterniond qb(quatB[3], quatB[0], quatB[1], quatB[2]); + Eigen::Quaterniond qres = qa.slerp(t, qb); + + accA[0] = accA[0] + t*(accB[0] - accA[0]); + accA[1] = accA[1] + t*(accB[1] - accA[1]); + accA[2] = accA[2] + t*(accB[2] - accA[2]); + + gyroA[0] = gyroA[0] + t*(gyroB[0] - gyroA[0]); + gyroA[1] = gyroA[1] + t*(gyroB[1] - gyroA[1]); + gyroA[2] = gyroA[2] + t*(gyroB[2] - gyroA[2]); + + imu = IMU(cv::Vec4d(qres.x(), qres.y(), qres.z(), qres.w()), iterA->second.orientationCovariance(), + gyroA, iterA->second.angularVelocityCovariance(), + accA, iterA->second.linearAccelerationCovariance(), + imuLocalTransform_); + } + } + imuMutex_.unlock(); + } + return imu; + } + +private: + void init(bool imuPublished) + { + imuFilter_ = 0; + + UINFO("imuPublished=%d", imuPublished?1:0); + size_t w = info_.format.columns_per_frame; + size_t h = info_.format.pixels_per_column; + + lut_ = ouster::make_xyz_lut(info_); + + // Buffer to store raw packet data + lidarPacket_ = ouster::sensor::LidarPacket (pf_.lidar_packet_size); + imuPacket_ = ouster::sensor::ImuPacket(pf_.imu_packet_size); + + const std::array reducedSlots{ + {{ouster::sensor::ChanField::RANGE, ouster::sensor::ChanFieldType::UINT32}, + {intensityChannel_, ouster::sensor::ChanFieldType::UINT32}}}; + scanBuffer_ = ouster::LidarScan(w, h, reducedSlots.begin(), reducedSlots.end()); + + lidarSensorT_ = Transform::fromEigen4d(info_.lidar_to_sensor_transform); + lidarSensorT_.x() /= 1000; + lidarSensorT_.y() /= 1000; + lidarSensorT_.z() /= 1000; + UINFO("Lidar to sensor: %s", lidarSensorT_.prettyPrint().c_str()); + + if(imuPublished) + { + imuLocalTransform_ = Transform::fromEigen4d(info_.imu_to_sensor_transform); + imuLocalTransform_.x() /= 1000; + imuLocalTransform_.y() /= 1000; + imuLocalTransform_.z() /= 1000; + UINFO("IMU to sensor: %s", imuLocalTransform_.prettyPrint().c_str()); + + imuFilter_ = IMUFilter::create(IMUFilter::kMadgwick); + } + } + + virtual void mainLoop() + { + bool isLidarPacket = false; + bool isImuPacket = false; + + if(clientHandle_) + { + // wait until sensor data is available + ouster::sensor::client_state st = ouster::sensor::poll_client(*clientHandle_); + // check for timeout + if (st == ouster::sensor::TIMEOUT){ + UERROR("Client has timed out"); + this->kill(); + return; + } + + if (st & ouster::sensor::EXIT){ + UERROR("Exit was requested"); + this->kill(); + return; + } + + // check for error status + if (st & ouster::sensor::CLIENT_ERROR) { + UERROR("Sensor client returned error state!"); + this->kill(); + return; + } + + if (st & ouster::sensor::LIDAR_DATA) { + if (!ouster::sensor::read_lidar_packet(*clientHandle_, lidarPacket_)) { + UERROR("Failed to read a lidar packet of the expected size!"); + this->kill(); + return; + } + isLidarPacket = true; + } + + if (imuFilter_ && st & ouster::sensor::IMU_DATA) { + if (!ouster::sensor::read_imu_packet(*clientHandle_, imuPacket_)) { + UERROR("Failed to read an imu packet of the expected size!"); + this->kill(); + return; + } + isImuPacket = true; + } + } + else if(pcapHandle_) //pcap + { + ouster::sensor_utils::packet_info packet_info; + if(!ouster::sensor_utils::next_packet_info(*pcapHandle_, packet_info)) + { + UWARN("No more data"); + endOfFileReached_ = true; + return; + } + + if(packet_info.dst_port == info_.config.udp_port_lidar) + { + auto packet_size = ouster::sensor_utils::read_packet( + *pcapHandle_, lidarPacket_.buf.data(), lidarPacket_.buf.size()); + + if (packet_size == pf_.lidar_packet_size) { + isLidarPacket = true; + } + } + else if(imuFilter_ && packet_info.dst_port == info_.config.udp_port_imu) + { + // async IMU, can be used for deskewing in Odometry + auto packet_size = ouster::sensor_utils::read_packet( + *pcapHandle_, imuPacket_.buf.data(), imuPacket_.buf.size()); + if (packet_size == pf_.imu_packet_size) { + isImuPacket = true; + } + } + } + else if(osfReader_.get()) + { + // read next message from OSF in timestamp order + if(osfIter_ == osfReader_->messages().end()) + { + UWARN("No more data"); + endOfFileReached_ = true; + return; + } + if(osfIter_->is()) + { + // Decoding LidarScan messages + auto ls = osfIter_->decode_msg(); + { + UScopeMutex s(scanMutex_); + bool notify = lastScan_.w == 0; + lastScan_ = *ls; + if(lastScan_.w != 0 && notify) + { + scanReady_.release(); + } + } + } + //else if(m.is()) // Seems not implemented in ouster-sdk + + ++osfIter_; + } + else { + UFATAL(""); + } + + if (isLidarPacket) { + // batcher will return "true" when the current scan is complete + if (scanBatcher_(lidarPacket_, scanBuffer_)) { + // retry until we receive a full set of valid measurements + // (accounting for azimuth_window settings if any) + if (scanBuffer_.complete(info_.format.column_window)) + { + { + UScopeMutex s(scanMutex_); + bool notify = lastScan_.w == 0; + lastScan_ = scanBuffer_; + if(lastScan_.w != 0 && notify) + { + scanReady_.release(); + } + } + } + } + } + + // async IMU, can be used for deskewing in Odometry + if (isImuPacket) { + static const double standard_g = 9.80665; + uchar* buf = imuPacket_.buf.data(); + uint64_t ts = pf_.imu_gyro_ts(buf); + double imuStamp = double(ts)/10e8; + cv::Vec3d linearAcc; + cv::Vec3d angularVel; + linearAcc[0] = pf_.imu_la_x(buf) * standard_g; + linearAcc[1] = pf_.imu_la_y(buf) * standard_g; + linearAcc[2] = pf_.imu_la_z(buf) * standard_g; + angularVel[0] = pf_.imu_av_x(buf) * M_PI / 180.0; + angularVel[1] = pf_.imu_av_y(buf) * M_PI / 180.0; + angularVel[2] = pf_.imu_av_z(buf) * M_PI / 180.0; + imuFilter_->update( + angularVel[0], angularVel[1], angularVel[2], + linearAcc[0], linearAcc[1], linearAcc[2], + imuStamp); + cv::Vec4d quat; + imuFilter_->getOrientation(quat[0], quat[1], quat[2], quat[3]); + IMU imu(quat, cv::Mat::eye(3,3,CV_64FC1)*6e-4, + angularVel, cv::Mat::eye(3,3,CV_64FC1)*6e-4, + linearAcc, cv::Mat::eye(3,3,CV_64FC1)*0.01, + imuLocalTransform_); + UEventsManager::post(new IMUEvent(imu, imuStamp)); + UScopeMutex s(imuMutex_); + imuBuffer_.insert(imuBuffer_.end(), std::make_pair(imuStamp, imu)); + if(imuBuffer_.size() > 1000) + { + imuBuffer_.erase(imuBuffer_.begin()); + } + } + } + + virtual void mainLoopEnd() + { + scanReady_.release(); + } + + private: + UMutex scanMutex_; + UMutex imuMutex_; + USemaphore scanReady_; + ouster::ScanBatcher scanBatcher_; + ouster::sensor::packet_format pf_; + ouster::LidarScan scanBuffer_; + ouster::LidarScan lastScan_; + ouster::sensor::LidarPacket lidarPacket_; + ouster::sensor::ImuPacket imuPacket_; + std::shared_ptr clientHandle_; + std::shared_ptr pcapHandle_; + std::shared_ptr osfReader_; + ouster::osf::MessagesStreamingIter osfIter_; + ouster::sensor::sensor_info info_; + ouster::sensor::cf_type intensityChannel_; + ouster::XYZLut lut_; + Transform lidarSensorT_; + Transform imuLocalTransform_; + IMUFilter * imuFilter_; + std::map imuBuffer_; + bool endOfFileReached_; +}; +#endif + +LidarOuster::LidarOuster( + const std::string& ipOrHostnameOrPcapOrOsf, + const std::string& dataDestinationOrJson, + int lidarMode, + int timestampMode, + bool useReflectivityForIntensityChannel, + bool publishIMU, + float frameRate, + Transform localTransform) : + Lidar(frameRate, localTransform), + ousterCaptureThread_(0), + imuPublished_(publishIMU), + useReflectivityForIntensityChannel_(useReflectivityForIntensityChannel), + ipOrHostnameOrPcapOrOsf_(ipOrHostnameOrPcapOrOsf), + dataDestinationOrJson_(dataDestinationOrJson), + lidarMode_(lidarMode), + timestampMode_(timestampMode) +{ + UASSERT(!ipOrHostnameOrPcapOrOsf.empty()); +#ifdef RTABMAP_OUSTER + UASSERT(lidarMode>=ouster::sensor::lidar_mode::MODE_UNSPEC && lidarMode<=ouster::sensor::lidar_mode::MODE_4096x5); + UASSERT(timestampMode>=ouster::sensor::timestamp_mode::TIME_FROM_UNSPEC && timestampMode<=ouster::sensor::timestamp_mode::TIME_FROM_PTP_1588); +#endif +} + +LidarOuster::~LidarOuster() +{ +#ifdef RTABMAP_OUSTER + if(ousterCaptureThread_) + ousterCaptureThread_->join(true); + delete ousterCaptureThread_; +#endif +} + +bool LidarOuster::available() +{ +#ifdef RTABMAP_OUSTER + return true; +#else + return false; +#endif +} + +std::string LidarOuster::getSerial() const +{ +#ifdef RTABMAP_OUSTER + if(ousterCaptureThread_) { + return ousterCaptureThread_->getInfo().sn.c_str(); + } +#endif + return ""; +} + +bool LidarOuster::init(const std::string &, const std::string &) +{ +#ifdef RTABMAP_OUSTER + if(ousterCaptureThread_) + ousterCaptureThread_->join(true); + delete ousterCaptureThread_; + ousterCaptureThread_ = 0; + + bool readingFromFile = false; + ouster::sensor::sensor_info info; + std::string ext = uToLowerCase(UFile::getExtension(ipOrHostnameOrPcapOrOsf_)); + if(ext == "pcap") + { + UINFO("Using PCAP file \"%s\" and JSON file \"%s\"", ipOrHostnameOrPcapOrOsf_.c_str(), dataDestinationOrJson_.c_str()); + if(dataDestinationOrJson_.empty()) + { + UERROR("A JSON path should be provided when a PCAP path is used."); + return false; + } + + UDEBUG(""); + auto pcapHandle = ouster::sensor_utils::replay_initialize(ipOrHostnameOrPcapOrOsf_); + if(pcapHandle.get() == 0) { + UERROR("Failed to open pcap file \"%s\"!", ipOrHostnameOrPcapOrOsf_.c_str()); + return false; + } + info = ouster::sensor::metadata_from_json(dataDestinationOrJson_); + + ousterCaptureThread_ = new OusterCaptureThread( + pcapHandle, + info, + imuPublished_, + useReflectivityForIntensityChannel_); + readingFromFile = true; + } + else if(ext == "osf") + { + UINFO("Using OSF file \"%s\"", ipOrHostnameOrPcapOrOsf_.c_str()); + if(imuPublished_) + { + UWARN("Imu publishing it not supported for OSF files."); + imuPublished_ = false; + } + try { + std::shared_ptr osfReader(new ouster::osf::Reader(ipOrHostnameOrPcapOrOsf_)); + auto sensors = osfReader->meta_store().find(); + UASSERT(sensors.size() >= 1); + + // Use first sensor and get its sensor_info + info = sensors.begin()->second->info(); + + ousterCaptureThread_ = new OusterCaptureThread( + osfReader, + info, + imuPublished_, + useReflectivityForIntensityChannel_); + + readingFromFile = true; + } + catch(std::exception & e) { + UERROR("Failed creating OSF reader: %s", e.what()); + return false; + } + } + else // ip / hostname + { + UINFO("Using sensor hostname \"%s\" and destination (optional) \"%s\"", + ipOrHostnameOrPcapOrOsf_.c_str(), dataDestinationOrJson_.c_str()); + auto clientHandle = ouster::sensor::init_client( + ipOrHostnameOrPcapOrOsf_, + dataDestinationOrJson_, + (ouster::sensor::lidar_mode)lidarMode_, + (ouster::sensor::timestamp_mode)timestampMode_); + + if(!clientHandle) { + UERROR("Failed to connect to sensor! Verify that the Ouster can be reached on the network at this address \"%s\".", + ipOrHostnameOrPcapOrOsf_.c_str()); + return false; + } + + UINFO("Connection to sensor succeeded"); + + auto metadata = ouster::sensor::get_metadata(*clientHandle); + + // Raw metadata can be parsed into a `sensor_info` struct + info = ouster::sensor::sensor_info(metadata); + + ousterCaptureThread_ = new OusterCaptureThread( + clientHandle, + info, + imuPublished_, + useReflectivityForIntensityChannel_); + } + + UINFO("Firmware version: %s", info.fw_rev.c_str()); + UINFO("Serial number: %s", info.sn.c_str()); + UINFO("Product line: %s", info.prod_line.c_str()); + UINFO("Scan dimensions: %ld x %ld", info.format.columns_per_frame, info.format.pixels_per_column); + //UINFO("Column window: [%d,%d]", info.column_window.first, info.column_window.second); + + if(!readingFromFile) + { + ousterCaptureThread_->start(); + } + else if(this->getFrameRate() == 0.0f) + { + std::string lidarMode = ouster::sensor::to_string(info.config.lidar_mode.value_or(ouster::sensor::lidar_mode::MODE_UNSPEC)); + std::list lidarModeSplit = uSplit(lidarMode, 'x'); + if(lidarModeSplit.size() == 2) { + int rate = uStr2Int(*lidarModeSplit.rbegin()); + UASSERT(rate > 0); + this->setFrameRate(rate); + UINFO("Setting frame rate to %d Hz (lidar mode = %s)", rate, lidarMode.c_str()); + } + else + { + UERROR("Unknown lidar mode \"%s\", framerate is set to 10.", lidarMode.c_str()); + this->setFrameRate(10); + } + } + + return true; +#else + UERROR("RTAB-Map is not built with OusterSDK"); + return false; +#endif +} + +SensorData LidarOuster::captureData(SensorCaptureInfo * info) +{ + SensorData data; +#ifdef RTABMAP_OUSTER + double stamp = 0.0; + LaserScan scan; + if(!ousterCaptureThread_->getScan(scan, stamp)) + { + // End of stream + return data; + } + + data.setLaserScan(scan); + data.setStamp(stamp); + + IMU imu = ousterCaptureThread_->getIMU(stamp); + if(!imu.empty()) + { + data.setIMU(imu); + } +#else + UERROR("RTAB-Map is not built with OusterSDK"); +#endif + return data; +} + + +} /* namespace rtabmap */ diff --git a/corelib/src/lidar/LidarVLP16.cpp b/corelib/src/lidar/LidarVLP16.cpp index 2fa758a0ef..5fa04c5bab 100644 --- a/corelib/src/lidar/LidarVLP16.cpp +++ b/corelib/src/lidar/LidarVLP16.cpp @@ -124,6 +124,15 @@ LidarVLP16::~LidarVLP16() UDEBUG("Stopped lidar!"); } +bool LidarVLP16::available() +{ +#if PCL_VERSION_COMPARE(>=, 1, 8, 0) + return true; +#else + return false; +#endif +} + void LidarVLP16::setOrganized(bool enable) { organized_ = true; diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index 6239a8d0aa..325e76da78 100644 --- a/corelib/src/odometry/OdometryF2M.cpp +++ b/corelib/src/odometry/OdometryF2M.cpp @@ -1148,6 +1148,7 @@ Transform OdometryF2M::computeTransform( } else { + UDEBUG("Initialize first key frame"); // Just generate keypoints for the new signature // For scan, we want to use reading filters, so set dummy's scan and set back to reference afterwards Signature dummy; diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index 134cb6a235..d492f04238 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -328,6 +328,7 @@ LaserScan commonFiltering( scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,10), groundNormalsUp); } } + UDEBUG("scan size=%d format=%d, organized=%d", scan.size(), (int)scan.format(), scan.isOrganized()?1:0); return scan; } diff --git a/data/presets/lidar3d_icp.ini b/data/presets/lidar3d_icp.ini index 57caca42a5..89a46d7642 100644 --- a/data/presets/lidar3d_icp.ini +++ b/data/presets/lidar3d_icp.ini @@ -1,3 +1,5 @@ +# Kept for backward compatibility, see also lidar3d_icp_indoor.ini or lidar3d_icp_outdoor.ini + # Could be used with LiDARs like Velodyne, RoboSense, Ouster [Camera] diff --git a/data/presets/lidar3d_icp_indoor.ini b/data/presets/lidar3d_icp_indoor.ini new file mode 100644 index 0000000000..9c85d79fbc --- /dev/null +++ b/data/presets/lidar3d_icp_indoor.ini @@ -0,0 +1,46 @@ +# Could be used with LiDARs like Velodyne, RoboSense, Ouster + +[Camera] +Scan\downsampleStep = 1 +Scan\fromDepth = false +Scan\normalsK = 0 +Scan\normalsRadius = 0 +Scan\normalsUp = false +Scan\rangeMax = 0 +Scan\rangeMin = 0 +Scan\voxelSize = 0 + +[Gui] +General\showClouds0 = false +General\showClouds1 = false + +[Core] +# Would be 0.01 for odom and 0.2 for mapping: +Icp\CorrespondenceRatio = 0.1 +Icp\Epsilon = 0.001 +Icp\FiltersEnabled = 2 +Icp\Iterations = 10 +Icp\OutlierRatio = 0.7 +# ~10x the voxel size: +Icp\MaxCorrespondenceDistance = 1 +Icp\MaxTranslation = 2 +# Uncomment if lidar can see ground most of the time (on a car or wheeled robot): +#Icp\PointToPlaneGroundNormalsUp = 0.8 +Icp\PointToPlaneK = 20 +Icp\PointToPlaneMinComplexity = 0 +# Make sure PM is used: +Icp\Strategy = 1 +Icp\VoxelSize = 0.1 +Mem\NotLinkedNodesKept = false +Mem\STMSize = 30 +Odom\Deskewing = true +Odom\GuessSmoothingDelay = 0.3 +Odom\ScanKeyFrameThr = 0.6 +OdomF2M\ScanMaxSize = 15000 +# Match voxel size: +OdomF2M\ScanSubtractRadius = 0.1 +RGBD\AngularUpdate = 0.05 +RGBD\LinearUpdate = 0.05 +RGBD\ProximityMaxGraphDepth = 0 +RGBD\ProximityPathMaxNeighbors = 1 +Reg\Strategy = 1 diff --git a/data/presets/lidar3d_icp_outdoor.ini b/data/presets/lidar3d_icp_outdoor.ini new file mode 100644 index 0000000000..3d7fbd57ea --- /dev/null +++ b/data/presets/lidar3d_icp_outdoor.ini @@ -0,0 +1,46 @@ +# Could be used with LiDARs like Velodyne, RoboSense, Ouster + +[Camera] +Scan\downsampleStep = 1 +Scan\fromDepth = false +Scan\normalsK = 0 +Scan\normalsRadius = 0 +Scan\normalsUp = false +Scan\rangeMax = 0 +Scan\rangeMin = 0 +Scan\voxelSize = 0 + +[Gui] +General\showClouds0 = false +General\showClouds1 = false + +[Core] +# Would be 0.01 for odom and 0.2 for mapping: +Icp\CorrespondenceRatio = 0.1 +Icp\Epsilon = 0.001 +Icp\FiltersEnabled = 2 +Icp\Iterations = 10 +Icp\OutlierRatio = 0.7 +# ~10x the voxel size: +Icp\MaxCorrespondenceDistance = 5 +Icp\MaxTranslation = 5 +# Uncomment if lidar can see ground most of the time (on a car or wheeled robot): +#Icp\PointToPlaneGroundNormalsUp = 0.8 +Icp\PointToPlaneK = 20 +Icp\PointToPlaneMinComplexity = 0 +# Make sure PM is used: +Icp\Strategy = 1 +Icp\VoxelSize = 0.5 +Mem\NotLinkedNodesKept = false +Mem\STMSize = 30 +Odom\Deskewing = true +Odom\GuessSmoothingDelay = 0.3 +Odom\ScanKeyFrameThr = 0.6 +OdomF2M\ScanMaxSize = 15000 +# Match voxel size: +OdomF2M\ScanSubtractRadius = 0.5 +RGBD\AngularUpdate = 0.05 +RGBD\LinearUpdate = 0.05 +RGBD\ProximityMaxGraphDepth = 0 +RGBD\ProximityPathMaxNeighbors = 1 +Reg\Strategy = 1 diff --git a/examples/LidarMapping/MapBuilder.h b/examples/LidarMapping/MapBuilder.h index b6e6b5ded5..6d530804a8 100644 --- a/examples/LidarMapping/MapBuilder.h +++ b/examples/LidarMapping/MapBuilder.h @@ -190,9 +190,21 @@ protected Q_SLOTS: { // update camera position cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose()); + + if( odom.data().imu().orientation().val[0]!=0 || + odom.data().imu().orientation().val[1]!=0 || + odom.data().imu().orientation().val[2]!=0 || + odom.data().imu().orientation().val[3]!=0) + { + Eigen::Vector3f gravity(0,0,-1); + Transform orientation(0,0,0, odom.data().imu().orientation()[0], odom.data().imu().orientation()[1], odom.data().imu().orientation()[2], odom.data().imu().orientation()[3]); + gravity = (orientation* odom.data().imu().localTransform().inverse()*(odometryCorrection_*odom.pose()).rotation().inverse()).toEigen3f()*gravity; + cloudViewer_->addOrUpdateLine("odom_imu_orientation", odometryCorrection_*odom.pose(), (odometryCorrection_*odom.pose()).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*odom.pose().rotation().inverse(), Qt::yellow, true, true); + } } } cloudViewer_->update(); + cloudViewer_->refreshView(); lastOdometryProcessed_ = true; } @@ -295,6 +307,7 @@ protected Q_SLOTS: odometryCorrection_ = stats.mapCorrection(); cloudViewer_->update(); + cloudViewer_->refreshView(); processingStatistics_ = false; } diff --git a/examples/LidarMapping/main.cpp b/examples/LidarMapping/main.cpp index 3bf0eff5e9..a251c59ad4 100644 --- a/examples/LidarMapping/main.cpp +++ b/examples/LidarMapping/main.cpp @@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // Should be first on windows to avoid "WinSock.h has already been included" error #include "rtabmap/core/lidar/LidarVLP16.h" +#include "rtabmap/core/lidar/LidarOuster.h" #include #include "rtabmap/core/Rtabmap.h" @@ -35,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UEventsManager.h" #include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UDirectory.h" +#include "rtabmap/utilite/UFile.h" #include #include #include @@ -47,11 +49,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. void showUsage() { printf("\nUsage:\n" - "rtabmap-lidar_mapping IP PORT\n" - "rtabmap-lidar_mapping PCAP_FILEPATH\n" + "rtabmap-lidar_mapping [OPTIONS] DRIVER ...\n" + "rtabmap-lidar_mapping [OPTIONS] 0 IP PORT (with VLP16 LiDAR)\n" + "rtabmap-lidar_mapping [OPTIONS] 1 IP (with Ouster LiDAR)\n" + "rtabmap-lidar_mapping [OPTIONS] DRIVER PCAP_FILEPATH [JSON_FILEPATH]\n" "\n" - "Example:" - " rtabmap-lidar_mapping 192.168.1.201 2368\n\n"); + "DRIVER: 0=VLP16, 1=Ouster\n" + "JSON_FILEPATH: should be set if DRIVER=1\n" + "Options:\n" + " --resolution # Resolution of the map (default 0.05 m), indoor: 0.05-0.3, outdoor: 0.3-0.5\n" + "\n" + "Examples:\n" + " rtabmap-lidar_mapping 0 192.168.1.201 2368\n" + " rtabmap-lidar_mapping 0 scan.pcap\n" + " rtabmap-lidar_mapping 1 scan.pcap config.json\n\n"); exit(1); } @@ -59,42 +70,109 @@ using namespace rtabmap; int main(int argc, char * argv[]) { ULogger::setType(ULogger::kTypeConsole); - ULogger::setLevel(ULogger::kWarning); + ULogger::setLevel(ULogger::kInfo); - std::string filepath; + std::string pcapFile; + std::string jsonFile; std::string ip; int port = 2368; - if(argc < 2) + int driver = 0; + float resolution = 0.05; + if(argc < 3) { showUsage(); } - else if(argc == 2) - { - filepath = argv[1]; - } else { - ip = argv[1]; - port = uStr2Int(argv[2]); + int i=1; + while(i1) + { + printf("Not supported driver %d!\n", driver); + showUsage(); + } + std::string ext = UFile::getExtension(uToLowerCase(argv[i])); + if(ext == "pcap" || ext == "osf") + { + pcapFile = argv[i++]; + if(driver == 1 && ext == "pcap") + { + if(i "SensorEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent" // Create the Lidar sensor, it will send a SensorEvent - LidarVLP16 * lidar; + Lidar * lidar = 0; if(!ip.empty()) { - printf("Using ip=%s port=%d\n", ip.c_str(), port); - lidar = new LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port); + if(driver == 0) + { + printf("Using ip=%s port=%d\n", ip.c_str(), port); + lidar = new LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port); + ((LidarVLP16*)lidar)->setOrganized(true); //faster deskewing + } + else if(driver == 1) + { + printf("Using ip=%s\n", ip.c_str()); + lidar = new LidarOuster(ip, 0, 0); + } } else { - filepath = uReplaceChar(filepath, '~', UDirectory::homeDir()); - printf("Using file=%s\n", filepath.c_str()); - lidar = new LidarVLP16(filepath); + pcapFile = uReplaceChar(pcapFile, '~', UDirectory::homeDir()); + printf("Using file=%s\n", pcapFile.c_str()); + if(driver == 0) + { + lidar = new LidarVLP16(pcapFile); + ((LidarVLP16*)lidar)->setOrganized(true); //faster deskewing + lidar->setFrameRate(10); // default + } + else if(driver == 1) + { + jsonFile = uReplaceChar(jsonFile, '~', UDirectory::homeDir()); + printf("Using config=%s\n", jsonFile.c_str()); + lidar = new LidarOuster(pcapFile, jsonFile); + // frame rate is set based on the lidar mode + } } - lidar->setOrganized(true); //faster deskewing if(!lidar->init()) { @@ -112,8 +190,6 @@ int main(int argc, char * argv[]) ParametersMap params; - float resolution = 0.05; - // ICP parameters params.insert(ParametersPair(Parameters::kRegStrategy(), "1")); params.insert(ParametersPair(Parameters::kIcpFiltersEnabled(), "2")); @@ -211,16 +287,15 @@ int main(int argc, char * argv[]) } if(cloud->size()) { - printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)cloud->size()); - cloud = util3d::voxelize(cloud, 0.01f); + printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", resolution, (int)cloud->size()); + cloud = util3d::voxelize(cloud, resolution); - printf("Saving rtabmap_cloud.pcd... done! (%d points)\n", (int)cloud->size()); - pcl::io::savePCDFile("rtabmap_cloud.pcd", *cloud); - //pcl::io::savePLYFile("rtabmap_cloud.ply", *cloud); // to save in PLY format + pcl::io::savePLYFile("rtabmap_cloud.ply", *cloud); // to save in PLY format + printf("Saving rtabmap_cloud.ply... done! (%d points)\n", (int)cloud->size()); } else { - printf("Saving rtabmap_cloud.pcd... failed! The cloud is empty.\n"); + printf("Saving point cloud failed! The cloud is empty.\n"); } // Save trajectory diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index c0badb3981..be59c3d2ef 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -194,6 +194,7 @@ protected Q_SLOTS: void selectDepthAIOAKDPro(); void selectXvisioSeerSense(); void selectVLP16(); + void selectOuster(); void dumpTheMemory(); void dumpThePrediction(); void sendGoal(); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index b5bb410497..7d4cf39201 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -121,6 +121,7 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog kSrcLidar = 400, kSrcLidarVLP16 = 400, + kSrcLidarOuster = 401, }; public: @@ -403,6 +404,8 @@ private Q_SLOTS: void selectSourceRealsense2JsonPath(); void selectSourceDepthaiBlobPath(); void selectVlp16PcapPath(); + void selectOusterPcapPath(); + void selectOusterJsonPath(); void updateSourceGrpVisibility(); void testOdometry(); void testCamera(); diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index a5713caa17..3de81b0fd6 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Parameters.h" #include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/CameraStereo.h" +#include "rtabmap/core/lidar/LidarOuster.h" #include "rtabmap/core/Optimizer.h" #include "ui_aboutDialog.h" #include @@ -158,6 +159,8 @@ AboutDialog::AboutDialog(QWidget * parent) : _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_ouster->setText(LidarOuster::available() ? "Yes" : "No"); + _ui->label_ouster_license->setEnabled(LidarOuster::available()); _ui->label_toro->setText(Optimizer::isAvailable(Optimizer::kTypeTORO)?"Yes":"No"); _ui->label_toro_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeTORO)?true:false); diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt index 8eb534336f..37ccf9201e 100644 --- a/guilib/src/CMakeLists.txt +++ b/guilib/src/CMakeLists.txt @@ -181,6 +181,8 @@ add_definitions(${PCL_DEFINITIONS}) SET(RESOURCES ${PROJECT_SOURCE_DIR}/data/presets/camera_tof_icp.ini ${PROJECT_SOURCE_DIR}/data/presets/lidar3d_icp.ini + ${PROJECT_SOURCE_DIR}/data/presets/lidar3d_icp_indoor.ini + ${PROJECT_SOURCE_DIR}/data/presets/lidar3d_icp_outdoor.ini ) foreach(arg ${RESOURCES}) diff --git a/guilib/src/GuiLib.qrc b/guilib/src/GuiLib.qrc index 5880443f6a..8e269adc51 100644 --- a/guilib/src/GuiLib.qrc +++ b/guilib/src/GuiLib.qrc @@ -46,5 +46,7 @@ images/astra.png images/oakdpro.png images/seer_sense_DS80.png + images/ouster.png + images/vlp16.png diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 185ccc886a..446b7d10ec 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -25,6 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +// Should be first on windows to avoid "WinSock.h has already been included" error +#include "rtabmap/core/lidar/LidarVLP16.h" + #include "rtabmap/gui/MainWindow.h" #include "ui_mainWindow.h" @@ -32,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/CameraRGB.h" #include "rtabmap/core/CameraStereo.h" #include "rtabmap/core/Lidar.h" +#include "rtabmap/core/lidar/LidarOuster.h" #include "rtabmap/core/IMUThread.h" #include "rtabmap/core/DBReader.h" #include "rtabmap/core/Parameters.h" @@ -471,6 +475,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh connect(_ui->actionDepthAI_oakdpro, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDPro())); connect(_ui->actionXvisio_SeerSense, SIGNAL(triggered()), this, SLOT(selectXvisioSeerSense())); connect(_ui->actionVelodyne_VLP_16, SIGNAL(triggered()), this, SLOT(selectVLP16())); + connect(_ui->actionOuster_SDK, SIGNAL(triggered()), this, SLOT(selectOuster())); _ui->actionFreenect->setEnabled(CameraFreenect::available()); _ui->actionOpenNI_PCL->setEnabled(CameraOpenni::available()); _ui->actionOpenNI_PCL_ASUS->setEnabled(CameraOpenni::available()); @@ -499,6 +504,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _ui->actionDepthAI_oakdlite->setEnabled(CameraDepthAI::available()); _ui->actionDepthAI_oakdpro->setEnabled(CameraDepthAI::available()); _ui->actionXvisio_SeerSense->setEnabled(CameraSeerSense::available()); + _ui->actionVelodyne_VLP_16->setEnabled(LidarVLP16::available()); + _ui->actionOuster_SDK->setEnabled(LidarOuster::available()); this->updateSelectSourceMenu(); connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences())); @@ -5305,6 +5312,7 @@ void MainWindow::updateSelectSourceMenu() _ui->actionDepthAI_oakdpro->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI); _ui->actionXvisio_SeerSense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcSeerSense); _ui->actionVelodyne_VLP_16->setChecked(_preferencesDialog->getLidarSourceDriver() == PreferencesDialog::kSrcLidarVLP16); + _ui->actionOuster_SDK->setChecked(_preferencesDialog->getLidarSourceDriver() == PreferencesDialog::kSrcLidarOuster); } void MainWindow::changeImgRateSetting() @@ -7248,6 +7256,11 @@ void MainWindow::selectVLP16() _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcLidarVLP16); } +void MainWindow::selectOuster() +{ + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcLidarOuster); +} + void MainWindow::dumpTheMemory() { this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory)); diff --git a/guilib/src/OdometryViewer.cpp b/guilib/src/OdometryViewer.cpp index 27c6125565..a0ca441125 100644 --- a/guilib/src/OdometryViewer.cpp +++ b/guilib/src/OdometryViewer.cpp @@ -185,6 +185,8 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) bool lost = false; bool lostStateChanged = false; + imageView_->setVisible(!odom.data().imageRaw().empty() || !odom.data().depthOrRightRaw().empty()); + if(odom.pose().isNull()) { UDEBUG("odom lost"); // use last pose @@ -211,7 +213,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) cloudView_->setBackgroundColor(Qt::black); } - timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation)); + timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation)); if(cloudShown_->isChecked() && !odom.data().imageRaw().empty() && @@ -219,8 +221,8 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) (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().depthRaw().empty()) { if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 && odom.data().imageRaw().rows % decimationSpin_->value() == 0) @@ -238,14 +240,14 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) } } else - { - validDecimationValue_ = decimationSpin_->value(); + { + validDecimationValue_ = decimationSpin_->value(); } // visualization: buffering the clouds // Create the new cloud pcl::PointCloud::Ptr cloud; - pcl::IndicesPtr validIndices(new std::vector); + pcl::IndicesPtr validIndices(new std::vector); cloud = util3d::cloudRGBFromSensorData( odom.data(), validDecimationValue_, @@ -257,7 +259,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) if(voxelSpin_->value()) { cloud = util3d::voxelize(cloud, validIndices, voxelSpin_->value()); - } + } if(cloud->size()) { @@ -489,6 +491,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) imageView_->update(); cloudView_->update(); + cloudView_->refreshView(); QApplication::processEvents(); processingData_ = false; } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 19a51382bf..bf99105b34 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -27,10 +27,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include -#if PCL_VERSION_COMPARE(>=, 1, 8, 0) // Should be first on windows to avoid "WinSock.h has already been included" error #include "rtabmap/core/lidar/LidarVLP16.h" -#endif +#include "rtabmap/core/lidar/LidarOuster.h" #include "rtabmap/gui/PreferencesDialog.h" #include "rtabmap/gui/DatabaseViewer.h" @@ -93,6 +92,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // Presets #include "camera_tof_icp_ini.h" #include "lidar3d_icp_ini.h" +#include "lidar3d_icp_indoor_ini.h" +#include "lidar3d_icp_outdoor_ini.h" #include #include @@ -430,6 +431,16 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable()); _ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable()); + //LiDARs + if (!LidarVLP16::available()) + { + _ui->comboBox_lidar_src->setItemData(kSrcLidarVLP16 - kSrcLidar, 0, Qt::UserRole - 1); + } + if (!LidarOuster::available()) + { + _ui->comboBox_lidar_src->setItemData(kSrcLidarOuster - kSrcLidar, 0, Qt::UserRole - 1); + } + #if PCL_VERSION_COMPARE(<, 1, 7, 2) _ui->checkBox_showFrustums->setEnabled(false); _ui->checkBox_showFrustums->setChecked(false); @@ -480,6 +491,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->pushButton_resetConfig, SIGNAL(clicked()), this, SLOT(resetConfig())); connect(_ui->pushButton_presets_camera_tof_icp, SIGNAL(clicked()), this, SLOT(loadPreset())); connect(_ui->pushButton_presets_lidar_3d_icp, SIGNAL(clicked()), this, SLOT(loadPreset())); + connect(_ui->pushButton_presets_lidar_3d_icp_indoor, SIGNAL(clicked()), this, SLOT(loadPreset())); + connect(_ui->pushButton_presets_lidar_3d_icp_outdoor, SIGNAL(clicked()), this, SLOT(loadPreset())); connect(_ui->radioButton_basic, SIGNAL(toggled(bool)), this, SLOT(setupTreeView())); connect(_ui->pushButton_testOdometry, SIGNAL(clicked()), this, SLOT(testOdometry())); connect(_ui->pushButton_test_camera, SIGNAL(clicked()), this, SLOT(testCamera())); @@ -917,6 +930,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkBox_vlp16_organized, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_vlp16_hostTime, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_vlp16_stamp_last, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->lineEdit_ouster_ip_hostname, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_ouster_lidar_mode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_ouster_timestamp, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->toolButton_ouster_pcap_path, SIGNAL(clicked()), this, SLOT(selectOusterPcapPath())); + connect(_ui->toolButton_ouster_json_path, SIGNAL(clicked()), this, SLOT(selectOusterJsonPath())); + connect(_ui->lineEdit_ouster_pcap_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->lineEdit_ouster_json_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); //Rtabmap basic connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double))); @@ -2286,6 +2306,13 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkBox_vlp16_organized->setChecked(false); _ui->checkBox_vlp16_hostTime->setChecked(true); _ui->checkBox_vlp16_stamp_last->setChecked(true); + _ui->lineEdit_ouster_ip_hostname->clear(); + _ui->comboBox_ouster_lidar_mode->setCurrentIndex(0); + _ui->comboBox_ouster_timestamp->setCurrentIndex(0); + _ui->lineEdit_ouster_pcap_path->clear(); + _ui->lineEdit_ouster_json_path->clear(); + _ui->checkBox_ouster_reflectivity->setChecked(true); + _ui->checkBox_ouster_imu->setChecked(false); _ui->groupBox_depthFromScan->setChecked(false); _ui->groupBox_depthFromScan_fillHoles->setChecked(true); @@ -2850,6 +2877,16 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->checkBox_vlp16_stamp_last->setChecked(settings.value("stampLast", _ui->checkBox_vlp16_stamp_last->isChecked()).toBool()); settings.endGroup(); // VLP16 + settings.beginGroup("Ouster"); + _ui->lineEdit_ouster_ip_hostname->setText(settings.value("ip",_ui->lineEdit_ouster_ip_hostname->text()).toString()); + _ui->comboBox_ouster_lidar_mode->setCurrentIndex(settings.value("lidarMode", _ui->comboBox_ouster_lidar_mode->currentIndex()).toInt()); + _ui->comboBox_ouster_timestamp->setCurrentIndex(settings.value("timestamp", _ui->comboBox_ouster_timestamp->currentIndex()).toInt()); + _ui->lineEdit_ouster_pcap_path->setText(settings.value("pcapPath",_ui->lineEdit_ouster_pcap_path->text()).toString()); + _ui->lineEdit_ouster_json_path->setText(settings.value("jsonPath",_ui->lineEdit_ouster_json_path->text()).toString()); + _ui->checkBox_ouster_reflectivity->setChecked(settings.value("reflectivity",_ui->checkBox_ouster_reflectivity->isChecked()).toBool()); + _ui->checkBox_ouster_imu->setChecked(settings.value("imu",_ui->checkBox_ouster_imu->isChecked()).toBool()); + settings.endGroup(); // Ouster + settings.endGroup(); // Lidar _calibrationDialog->loadSettings(settings, "CalibrationDialog"); @@ -3004,6 +3041,14 @@ void PreferencesDialog::loadPreset() { loadPreset(LIDAR3D_ICP_INI); } + else if(sender() == _ui->pushButton_presets_lidar_3d_icp_indoor) + { + loadPreset(LIDAR3D_ICP_INDOOR_INI); + } + else if(sender() == _ui->pushButton_presets_lidar_3d_icp_outdoor) + { + loadPreset(LIDAR3D_ICP_OUTDOOR_INI); + } else { UERROR("Unknown sender!"); @@ -3448,6 +3493,16 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("stampLast", _ui->checkBox_vlp16_stamp_last->isChecked()); settings.endGroup(); // VLP16 + settings.beginGroup("Ouster"); + settings.setValue("ip",_ui->lineEdit_ouster_ip_hostname->text()); + settings.setValue("lidarMode", _ui->comboBox_ouster_lidar_mode->currentIndex()); + settings.setValue("timestamp", _ui->comboBox_ouster_timestamp->currentIndex()); + settings.setValue("pcapPath",_ui->lineEdit_ouster_pcap_path->text()); + settings.setValue("jsonPath",_ui->lineEdit_ouster_json_path->text()); + settings.setValue("reflectivity",_ui->checkBox_ouster_reflectivity->isChecked()); + settings.setValue("imu",_ui->checkBox_ouster_imu->isChecked()); + settings.endGroup(); // Ouster + settings.endGroup(); // Lidar _calibrationDialog->saveSettings(settings, "CalibrationDialog"); @@ -4263,7 +4318,7 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant) } else if(src >= kSrcLidar) { - _ui->comboBox_lidar_src->setCurrentIndex(kSrcLidarVLP16 - kSrcLidar + 1); + _ui->comboBox_lidar_src->setCurrentIndex(src - kSrcLidar + 1); } if(previousCameraSrc == kSrcUndef && src < kSrcDatabase && @@ -4277,7 +4332,7 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant) } else if(previousLidarSrc== kSrcUndef && src >= kSrcLidar && QMessageBox::question(this, tr("LiDAR Source..."), - tr("Do you want to use \"LiDAR 3D ICP\" preset?"), + tr("Do you want to use \"LiDAR 3D ICP\" preset? You can open Preferences->General Settings to select indoor or outdoor presets afterwards."), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes) == QMessageBox::Yes) { loadPreset(LIDAR3D_ICP_INI); @@ -4748,6 +4803,38 @@ void PreferencesDialog::selectVlp16PcapPath() } } +void PreferencesDialog::selectOusterPcapPath() +{ + QString dir = _ui->lineEdit_ouster_pcap_path->text(); + if (dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Ouster recording (*.pcap *.osf)")); + if (!path.isEmpty()) + { + _ui->lineEdit_ouster_pcap_path->setText(path); + if(QFileInfo(path).suffix() == "pcap") + { + selectOusterJsonPath(); + } + } +} + +void PreferencesDialog::selectOusterJsonPath() +{ + QString dir = _ui->lineEdit_ouster_json_path->text(); + if (dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Ouster recording config (*.json)")); + if (!path.isEmpty()) + { + _ui->lineEdit_ouster_json_path->setText(path); + } +} + void PreferencesDialog::setParameter(const std::string & key, const std::string & value) { UDEBUG("%s=%s", key.c_str(), value.c_str()); @@ -5698,6 +5785,7 @@ void PreferencesDialog::updateSourceGrpVisibility() } _ui->stackedWidget_lidar_src->setVisible(_ui->comboBox_lidar_src->currentIndex() > 0); _ui->groupBox_vlp16->setVisible(_ui->comboBox_lidar_src->currentIndex()-1 == kSrcLidarVLP16-kSrcLidar); + _ui->groupBox_ouster->setVisible(_ui->comboBox_lidar_src->currentIndex()-1 == kSrcLidarOuster-kSrcLidar); _ui->frame_lidar_sensor->setVisible(_ui->comboBox_lidar_src->currentIndex() > 0 || _ui->checkBox_source_scanFromDepth->isChecked()); // Not Lidar None or database input _ui->pushButton_test_lidar->setEnabled(_ui->comboBox_lidar_src->currentIndex() > 0); @@ -6987,9 +7075,8 @@ Lidar * PreferencesDialog::createLidar() { Lidar * lidar = 0; Src driver = getLidarSourceDriver(); - if(driver == kSrcLidarVLP16) + if(driver >= kSrcLidarVLP16 && driver <= kSrcLidarOuster) { -#if PCL_VERSION_COMPARE(>=, 1, 8, 0) Transform localTransform = Transform::fromString(_ui->lineEdit_lidar_local_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString()); if(localTransform.isNull()) { @@ -6997,33 +7084,73 @@ Lidar * PreferencesDialog::createLidar() _ui->lineEdit_lidar_local_transform->text().toStdString().c_str()); localTransform = Transform::getIdentity(); } - if(!_ui->lineEdit_vlp16_pcap_path->text().isEmpty()) + if(driver == kSrcLidarVLP16) { - // PCAP mode - lidar = new LidarVLP16( - _ui->lineEdit_vlp16_pcap_path->text().toStdString(), - _ui->checkBox_vlp16_organized->isChecked(), - _ui->checkBox_vlp16_stamp_last->isChecked(), - this->getGeneralInputRate(), - localTransform); + if(!_ui->lineEdit_vlp16_pcap_path->text().isEmpty()) + { + // PCAP mode + lidar = new LidarVLP16( + _ui->lineEdit_vlp16_pcap_path->text().toStdString(), + _ui->checkBox_vlp16_organized->isChecked(), + _ui->checkBox_vlp16_stamp_last->isChecked(), + this->getGeneralInputRate(), + localTransform); + } + else + { + // Connect to sensor + + lidar = new LidarVLP16( + boost::asio::ip::address_v4::from_string(uFormat("%ld.%ld.%ld.%ld", + (size_t)_ui->spinBox_vlp16_ip1->value(), + (size_t)_ui->spinBox_vlp16_ip2->value(), + (size_t)_ui->spinBox_vlp16_ip3->value(), + (size_t)_ui->spinBox_vlp16_ip4->value())), + _ui->spinBox_vlp16_port->value(), + _ui->checkBox_vlp16_organized->isChecked(), + _ui->checkBox_vlp16_hostTime->isChecked(), + _ui->checkBox_vlp16_stamp_last->isChecked(), + this->getGeneralInputRate(), + localTransform); + + } } - else + else // Ouster { - // Connect to sensor - - lidar = new LidarVLP16( - boost::asio::ip::address_v4::from_string(uFormat("%ld.%ld.%ld.%ld", - (size_t)_ui->spinBox_vlp16_ip1->value(), - (size_t)_ui->spinBox_vlp16_ip2->value(), - (size_t)_ui->spinBox_vlp16_ip3->value(), - (size_t)_ui->spinBox_vlp16_ip4->value())), - _ui->spinBox_vlp16_port->value(), - _ui->checkBox_vlp16_organized->isChecked(), - _ui->checkBox_vlp16_hostTime->isChecked(), - _ui->checkBox_vlp16_stamp_last->isChecked(), - this->getGeneralInputRate(), - localTransform); - + if(!_ui->lineEdit_ouster_pcap_path->text().isEmpty()) + { + // PCAP/OSF mode + lidar = new LidarOuster( + _ui->lineEdit_ouster_pcap_path->text().toStdString(), + _ui->lineEdit_ouster_json_path->text().toStdString(), + 0, // lidar mode ignored + 0, // timestamp mode ignored + _ui->checkBox_ouster_reflectivity->isChecked(), + _ui->checkBox_ouster_imu->isChecked(), + this->getGeneralInputRate(), + localTransform); + } + else if(!_ui->lineEdit_ouster_ip_hostname->text().isEmpty()) + { + // Connect to sensor + + lidar = new LidarOuster( + _ui->lineEdit_ouster_ip_hostname->text().toStdString(), + "", // data destination ignored + _ui->comboBox_ouster_lidar_mode->currentIndex(), + _ui->comboBox_ouster_timestamp->currentIndex(), + _ui->checkBox_ouster_reflectivity->isChecked(), + _ui->checkBox_ouster_imu->isChecked(), + this->getGeneralInputRate(), + localTransform); + } + else + { + QMessageBox::warning(this, + tr("RTAB-Map"), + tr("Ouster: An IP/hostname or PCAP path should be set...")); + return lidar; // 0 + } } if(!lidar->init()) { @@ -7034,12 +7161,6 @@ Lidar * PreferencesDialog::createLidar() delete lidar; lidar = 0; } -#else - UWARN("Lidar cannot be used with rtabmap built with PCL < 1.8... "); - QMessageBox::warning(this, - tr("RTAB-Map"), - tr("Lidar initialization failed...")); -#endif } return lidar; } @@ -7182,8 +7303,9 @@ void PreferencesDialog::setSLAMMode(bool enabled) void PreferencesDialog::testOdometry() { - Camera * camera = this->createCamera(); - if(!camera) + Camera * camera = this->getSourceDriver()!=-1?this->createCamera():0; + Lidar * lidar = this->getLidarSourceDriver()!=-1?this->createLidar():0; + if(!camera && !lidar) { return; } @@ -7241,18 +7363,30 @@ void PreferencesDialog::testOdometry() odomViewer->resize(1280, 480+QPushButton().minimumHeight()); odomViewer->registerToEventsManager(); - SensorCaptureThread cameraThread(camera, this->getAllParameters()); // take ownership of camera - cameraThread.setMirroringEnabled(isSourceMirroring()); - cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked()); - cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value()); - cameraThread.setHistogramMethod(_ui->comboBox_source_histogramMethod->currentIndex()); + SensorCaptureThread * cameraThread; + if(lidar && camera) + { + cameraThread = new SensorCaptureThread(lidar, camera, this->getAllParameters()); // take ownership of lidar and camera + } + else if(camera) + { + cameraThread = new SensorCaptureThread(camera, this->getAllParameters()); // take ownership of camera + } + else // lidar + { + cameraThread = new SensorCaptureThread(lidar, this->getAllParameters()); // take ownership of lidar + } + cameraThread->setMirroringEnabled(isSourceMirroring()); + cameraThread->setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked()); + cameraThread->setImageDecimation(_ui->spinBox_source_imageDecimation->value()); + cameraThread->setHistogramMethod(_ui->comboBox_source_histogramMethod->currentIndex()); if(_ui->checkbox_source_feature_detection->isChecked()) { - cameraThread.enableFeatureDetection(this->getAllParameters()); + cameraThread->enableFeatureDetection(this->getAllParameters()); } - cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked()); - cameraThread.setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked()); - cameraThread.setScanParameters( + cameraThread->setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked()); + cameraThread->setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked()); + cameraThread->setScanParameters( _ui->checkBox_source_scanFromDepth->isChecked(), _ui->spinBox_source_scanDownsampleStep->value(), _ui->doubleSpinBox_source_scanRangeMin->value(), @@ -7264,23 +7398,23 @@ void PreferencesDialog::testOdometry() _ui->checkBox_source_scanDeskewing->isChecked()); if(_ui->comboBox_imuFilter_strategy->currentIndex()>0 && dynamic_cast(camera) == 0) { - cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters(), _ui->checkBox_imuFilter_baseFrameConversion->isChecked()); + cameraThread->enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters(), _ui->checkBox_imuFilter_baseFrameConversion->isChecked()); } if(isDepthFilteringAvailable()) { if(_ui->groupBox_bilateral->isChecked()) { - cameraThread.enableBilateralFiltering( + cameraThread->enableBilateralFiltering( _ui->doubleSpinBox_bilateral_sigmaS->value(), _ui->doubleSpinBox_bilateral_sigmaR->value()); } if(!_ui->lineEdit_source_distortionModel->text().isEmpty()) { - cameraThread.setDistortionModel(_ui->lineEdit_source_distortionModel->text().toStdString()); + cameraThread->setDistortionModel(_ui->lineEdit_source_distortionModel->text().toStdString()); } } - UEventsManager::createPipe(&cameraThread, &odomThread, "SensorEvent"); + UEventsManager::createPipe(cameraThread, &odomThread, "SensorEvent"); if(imuThread) { UEventsManager::createPipe(imuThread, &odomThread, "IMUEvent"); @@ -7289,7 +7423,7 @@ void PreferencesDialog::testOdometry() UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent"); odomThread.start(); - cameraThread.start(); + cameraThread->start(); if(imuThread) { @@ -7304,8 +7438,9 @@ void PreferencesDialog::testOdometry() imuThread->join(true); delete imuThread; } - cameraThread.join(true); + cameraThread->join(true); odomThread.join(true); + delete cameraThread; } void PreferencesDialog::testCamera() diff --git a/guilib/src/images/ouster.png b/guilib/src/images/ouster.png new file mode 100644 index 0000000000..6f609bd7e8 Binary files /dev/null and b/guilib/src/images/ouster.png differ diff --git a/guilib/src/images/vlp16.png b/guilib/src/images/vlp16.png new file mode 100644 index 0000000000..14ac5840b8 Binary files /dev/null and b/guilib/src/images/vlp16.png differ diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index 05d1036dfa..0986ebad9a 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -162,9 +162,9 @@ p, li { white-space: pre-wrap; } 0 - 0 + -341 596 - 1096 + 1119 @@ -185,107 +185,117 @@ p, li { white-space: pre-wrap; } - - + + - With K4A : + With loam_velodyne : true - - + + - BSD + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - <html><head/><body><p><span style=" font-weight:600;">Third Party Libraries</span></p></body></html> - - - Qt::RichText + With Freenect2 : true - - + + - With Viso2 : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With CPU-TSDF : + Apache-2 true - - + + - BSD + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - Apache-2 + VTK version : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With XVisio SDK : true - - + + - + GPLv3 - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + With MYNTEYE S : true - - + + @@ -297,18 +307,18 @@ p, li { white-space: pre-wrap; } - - + + - GPLv3 + Qt version : true - - + + @@ -320,18 +330,21 @@ p, li { white-space: pre-wrap; } - - + + - With stereo dc1394 : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + @@ -343,18 +356,18 @@ p, li { white-space: pre-wrap; } - - + + - With Zed Open Capture : + With OpenNI2 : true - - + + @@ -366,74 +379,61 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true + <html><head/><body><p><span style=" font-weight:600;">Third Party Libraries</span></p></body></html> - - - - - - With OpenVINS : + + Qt::RichText true - - + + - GPLv3 + BSD true - - + + - With Open3D : + Penn Software License true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + GPLv3 true - - + + - BSD + With Octomap : true - - + + @@ -445,18 +445,18 @@ p, li { white-space: pre-wrap; } - - + + - With RealSense2 : + With Zed SDK : true - - + + BSD @@ -465,38 +465,31 @@ p, li { white-space: pre-wrap; } - - + + - With g2o : + GPLv2 true - - + + - With OpenNI2 : - - - true + - - - - - - BSD + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + @@ -508,8 +501,8 @@ p, li { white-space: pre-wrap; } - - + + BSD @@ -518,10 +511,13 @@ p, li { white-space: pre-wrap; } - - + + - With AliceVision : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true @@ -529,7 +525,7 @@ p, li { white-space: pre-wrap; } - + @@ -541,8 +537,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -554,8 +550,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -567,111 +563,114 @@ p, li { white-space: pre-wrap; } - - + + - Open Source or Commercial + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - MPL2 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With MRPT : + With Python3 : true - - + + - With MYNTEYE S : + BSD true - - + + - With DVO : + BSD true - - + + - With OpenChisel : + GPLv2 true - - + + - With VINS-Fusion : + MIT true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Apache v2 true - - + + - GPLv3 + With MRPT : true - - + + - GPLv3 + PSF true - - + + @@ -683,117 +682,91 @@ p, li { white-space: pre-wrap; } - - - - With cvsba : - - - true - - - - - + + - GPLv2 + With RealSense2 : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With g2o : true - - + + - With Freenect : + With RealSense : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Apache v2 true - - + + - With FastCV : + GPLv3 true - - + + - Apache v2 and/or GPLv2 - - - true + - - - - - - With stereo FlyCapture2 : + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - MIT + LGPL true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With GridMap : true - - + + @@ -805,38 +778,41 @@ p, li { white-space: pre-wrap; } - - + + - Creative Commons [Attribution-NonCommercial-ShareAlike] + Apache v2 true - - + + - With ORB Octree : + BSD true - - + + - OpenCV version : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + @@ -848,8 +824,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -861,28 +837,28 @@ p, li { white-space: pre-wrap; } - - + + - With DepthAI : + With K4A : true - - + + - GPLv2 + With stereo dc1394 : true - - + + @@ -894,8 +870,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -907,8 +883,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -920,8 +896,18 @@ p, li { white-space: pre-wrap; } - - + + + + With Open3D : + + + true + + + + + @@ -933,64 +919,68 @@ p, li { white-space: pre-wrap; } - - + + - BSD + With Freenect : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With K4W2 : true - - + + - Apache v2 + With OpenVINS : true - - + + - + BSD - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + With FOVIS : true - - + + - <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> + MPL2 true - - + + @@ -1002,61 +992,61 @@ p, li { white-space: pre-wrap; } - - + + - With K4W2 : + BSD true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With Zed Open Capture : true - - + + - MIT + With OKVIS : true - - + + - With libpointmatcher : + PCL version : true - - + + - With ORB SLAM 2 : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + BSD @@ -1065,8 +1055,18 @@ p, li { white-space: pre-wrap; } - - + + + + MIT + + + true + + + + + @@ -1078,8 +1078,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -1091,21 +1091,28 @@ p, li { white-space: pre-wrap; } - - + + - + With VINS-Fusion : - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + With FastCV : true - - + + @@ -1117,18 +1124,18 @@ p, li { white-space: pre-wrap; } - - + + - Penn Software License + MIT true - - + + @@ -1140,38 +1147,38 @@ p, li { white-space: pre-wrap; } - - + + - GPLv2 + With ORB SLAM 2 : true - - + + - With FOVIS : + MIT true - - + + - MIT + <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> true - - + + @@ -1183,28 +1190,28 @@ p, li { white-space: pre-wrap; } - - + + - With loam_velodyne : + Creative Commons [Attribution-NonCommercial-ShareAlike] true - - + + - BSD + OpenCV version : true - - + + @@ -1216,51 +1223,58 @@ p, li { white-space: pre-wrap; } - - + + - + GPLv3 - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + With Viso2 : true - - + + - GPLv3 + With GTSAM : true - - + + - PCL version : + GPLv3 true - - + + - Apache v2 + With OpenNI : true - - + + @@ -1272,131 +1286,137 @@ p, li { white-space: pre-wrap; } - - + + - With Freenect2 : + With OpenChisel : true - - + + - With Python3 : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - Apache-2 + With DepthAI : true - - + + - BSD + With Ceres : true - - + + - GPLv3 + With libpointmatcher : true - - + + - BSD + With stereo FlyCapture2 : true - - + + - With RealSense : + Apache-2 true - - + + - Qt version : + GPLv3 true - - + + - Apache v2 and/or GPLv2 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With Ceres : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Open Source or Commercial true - - + + - MIT + GPLv3 true - - + + @@ -1408,8 +1428,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -1421,174 +1441,174 @@ p, li { white-space: pre-wrap; } - - + + - With OKVIS : + With ORB Octree : true - - + + - With Octomap : + With CPU-TSDF : true - - + + - BSD + With cvsba : true - - + + - GPLv3 + With TORO : true - - + + - PSF + BSD true - - + + - With GridMap : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With Zed SDK : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With TORO : + With DVO : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + GPLv2 true - - + + - With CCCoreLib : + With AliceVision : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + BSD true - - + + - VTK version : + Apache v2 and/or GPLv2 true - - + + - BSD + With MSCKF : true - - + + - With XVisio SDK : + Apache v2 and/or GPLv2 true - - + + - LGPL + With SuperPoint Torch : true - - + + - With MSCKF : + With CCCoreLib : true - - + + @@ -1600,18 +1620,18 @@ p, li { white-space: pre-wrap; } - - + + - With GTSAM : + BSD true - - + + @@ -1623,28 +1643,41 @@ p, li { white-space: pre-wrap; } - - + + - With SuperPoint Torch : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With OpenNI : + BSD true - - + + + + With Ouster SDK : + + + true + + + + + @@ -1656,10 +1689,10 @@ p, li { white-space: pre-wrap; } - - + + - Apache v2 + BSD true diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 666e809ae8..f14284faa2 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -356,7 +356,32 @@ LiDAR - + + + :/images/vlp16.png:/images/vlp16.png + + + + Velodyne + + + + :/images/vlp16.png:/images/vlp16.png + + + + + + Ouster + + + + :/images/ouster.png:/images/ouster.png + + + + + @@ -1749,6 +1774,14 @@ Xvisio + + + true + + + Ouster SDK + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index be76a324f7..f013e81034 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - 0 + -3879 713 - 4653 + 4737 @@ -95,7 +95,7 @@ QFrame::Raised - 7 + 5 @@ -369,10 +369,16 @@ - - + + - TOF Camera ICP + e.g., Velodyne, RoboSense and Ouster LiDARs. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse @@ -395,12 +401,46 @@ - LiDAR 3D ICP + LiDAR 3D ICP Legacy (5 cm voxel) - - + + + + LiDAR 3D ICP Indoor (10 cm voxel) + + + + + + + TOF Camera ICP + + + + + + + LiDAR 3D ICP Oudoor (50 cm voxel) + + + + + + + e.g., Velodyne, RoboSense and Ouster LiDARs. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + e.g., Velodyne, RoboSense and Ouster LiDARs. @@ -8465,6 +8505,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki VLP-16 + + + Ouster SDK + + @@ -8761,7 +8806,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 1 + 2 @@ -9033,6 +9078,243 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + Ouster SDK + + + + + + ... + + + + + + + + + + + + + + Path to a *.JSON config file corresponding to PCAP recording. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + IP Address or Hostname. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + This can speedup deskewing. + + + Lidar mode. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + <html><head/><body><p>TIME_FROM_INTERNAL_OSC: Use the internal clock.</p><p>TIME_FROM_SYNC_PULSE_IN: A free running counter synced to the SYNC_PULSE_IN input counts seconds (# of pulses) and nanoseconds since sensor turn on.</p><p>TIME_FROM_PTP_1588: Synchronize with an external PTP master.</p></body></html> + + + + Default + + + + + From Internal OSC + + + + + From Sync Pulse In + + + + + From PTP 1588 + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Path to a *.PCAP or *.OSF file. IP is ignored if PCAP/OSF file is used. A JSON file should be provided if PCAP is used, see below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>If the lidar is not connected to a GPS, the lidar's clock may be not in sync with the host computer's clock. By enabling this, as soon as a packet is received, it will be stamped with host time &quot;now&quot;. Note that it affects only the final scan timsestamp, not the individual relative timestamp for each point. It is recommended to enable this if you combine it with other sensors above and it is not connected to a GPS.</p></body></html> + + + Timestamp mode. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>MODE_512x10: 10 scans of 512 columns per second</p><p>MODE_512x20: 20 scans of 512 columns per second</p><p>MODE_1024x10: 10 scans of 1024 columns per second</p><p>MODE_1024x20: 20 scans of 1024 columns per second</p><p>MODE_2048x10: 10 scans of 2048 columns per second</p><p>MODE_4096x5: 5 scans of 4096 columns per second. Only available on select sensors<br/></p></body></html> + + + + Default + + + + + 512x10 + + + + + 512x20 + + + + + 1024x10 + + + + + 1024x20 + + + + + 2048x10 + + + + + 4096x5 + + + + + + + + + + + + + + + Use Reflectivity instead of Signal for intensity channel. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + ... + + + + + + + + + + + + + + Publish IMU. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index d50724b43b..f95eee8a35 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -22,9 +22,7 @@ ENDIF(OPENCV_NONFREE_FOUND) IF(TARGET rtabmap_gui) ADD_SUBDIRECTORY( CameraRGBD ) - IF(PCL_VERSION VERSION_GREATER_EQUAL "1.8") - ADD_SUBDIRECTORY( LidarViewer ) - ENDIF() + ADD_SUBDIRECTORY( LidarViewer ) ADD_SUBDIRECTORY( DatabaseViewer ) ADD_SUBDIRECTORY( EpipolarGeometry ) ADD_SUBDIRECTORY( OdometryViewer ) diff --git a/tools/LidarViewer/main.cpp b/tools/LidarViewer/main.cpp index fcd62678a3..7759169211 100644 --- a/tools/LidarViewer/main.cpp +++ b/tools/LidarViewer/main.cpp @@ -37,7 +37,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UFile.h" #include "rtabmap/utilite/UDirectory.h" #include "rtabmap/utilite/UConversion.h" -#include #include #include @@ -49,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include using namespace pcl; using namespace pcl::console; @@ -57,8 +57,14 @@ using namespace pcl::visualization; void showUsage() { printf("\nUsage:\n" - "rtabmap-lidar_viewer IP PORT driver\n" - " driver Driver number to use: 0=VLP16 (default IP and port are 192.168.1.201 2368)\n"); + "rtabmap-lidar_viewer DRIVER IP [PORT]\n" + "\n" + "DRIVER: 0=VLP16, 1=Ouster\n" + "PORT: should be set if DRIVER=0\n" + "\n" + "Examples:\n" + " rtabmap-lidar_viewer 0 192.168.1.201 2368\n" + " rtabmap-lidar_viewer 1 192.168.1.2\n\n"); exit(1); } @@ -80,28 +86,45 @@ int main(int argc, char * argv[]) int driver = 0; std::string ip; int port = 2368; - if(argc < 4) + if(argc < 3) { + printf("Not enough arguments.\n"); showUsage(); } else { - ip = argv[1]; - port = uStr2Int(argv[2]); - driver = atoi(argv[3]); - if(driver < 0 || driver > 0) + driver = uStr2Int(argv[1]); + if(driver < 0 || driver > 1) { - UERROR("driver should be 0."); + printf("driver should be 0 or 1.\n"); showUsage(); } + + ip = argv[2]; + if(driver==0) + { + if(argc>=3) + { + port = uStr2Int(argv[3]); + } + else + { + printf("IP and PORT should be set with VLP16 driver\n"); + showUsage(); + } + } } - printf("Using driver %d (ip=%s port=%d)\n", driver, ip.c_str(), port); + printf("Using driver %d (ip=%s%s)\n", driver, ip.c_str(), driver==0?uFormat(" port=%d", port).c_str():""); - rtabmap::LidarVLP16 * lidar = 0; + rtabmap::Lidar * lidar = 0; if(driver == 0) { lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port); } + else if(driver == 1) + { + lidar = new rtabmap::LidarOuster(ip); + } else { UFATAL(""); @@ -109,7 +132,7 @@ int main(int argc, char * argv[]) if(!lidar->init()) { - printf("Lidar init failed! Please select another driver (see \"--help\").\n"); + printf("Lidar init failed!\n"); delete lidar; exit(1); } @@ -121,18 +144,18 @@ int main(int argc, char * argv[]) signal(SIGTERM, &sighandler); signal(SIGINT, &sighandler); - rtabmap::SensorData data = lidar->takeScan(); + rtabmap::SensorData data = lidar->takeData(); while(!data.laserScanRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running) { pcl::PointCloud::Ptr cloud = rtabmap::util3d::laserScanToPointCloudI(data.laserScanRaw(), data.laserScanRaw().localTransform()); - viewer->showCloud(cloud, "cloud"); printf("Scan size: %ld points\n", cloud->size()); + viewer->showCloud(cloud, "cloud"); int c = cv::waitKey(10); // wait 10 ms or for key stroke if(c == 27) break; // if ESC, break and quit - data = lidar->takeScan(); + data = lidar->takeData(); } printf("Closing...\n"); if(viewer) diff --git a/utilite/include/rtabmap/utilite/USemaphore.h b/utilite/include/rtabmap/utilite/USemaphore.h index 14846f4887..3470d651eb 100644 --- a/utilite/include/rtabmap/utilite/USemaphore.h +++ b/utilite/include/rtabmap/utilite/USemaphore.h @@ -60,10 +60,10 @@ class USemaphore */ USemaphore( int initValue = 0 ) { + _available = initValue; #ifdef _WIN32 - S = CreateSemaphore(0,initValue,SEM_VALUE_MAX,0); + S = CreateSemaphoreW(0,initValue,SEM_VALUE_MAX,0); #else - _available = initValue; pthread_mutex_init(&_waitMutex, NULL); pthread_cond_init(&_cond, NULL); #endif @@ -88,12 +88,15 @@ class USemaphore * @return true on success, false on error/timeout */ #ifdef _WIN32 - bool acquire(int n = 1, int ms = 0) const + bool acquire(int n = 1, int ms = 0) { int rt = 0; while(n-- > 0 && rt==0) { rt = WaitForSingleObject((HANDLE)S, ms<=0?INFINITE:ms); + if (rt == 0) { + _available -= 1; + } } return rt == 0; } @@ -136,12 +139,17 @@ class USemaphore * @return false if the semaphore can't be taken without waiting (value <= 0), true otherwise */ #ifdef _WIN32 - int acquireTry() const + bool acquireTry() { - return ((WaitForSingleObject((HANDLE)S,INFINITE)==WAIT_OBJECT_0)?0:EAGAIN); + if(WaitForSingleObject((HANDLE)S, INFINITE) == WAIT_OBJECT_0) + { + _available -= 1; + return true; + } + return false; } #else - int acquireTry(int n) + bool acquireTry(int n) { pthread_mutex_lock(&_waitMutex); if(n > _available) @@ -160,9 +168,11 @@ class USemaphore * signaling waiting threads (which called acquire()). */ #ifdef _WIN32 - int release(int n = 1) const + void release(int n = 1) { - return (ReleaseSemaphore((HANDLE)S,n,0)?0:ERANGE); + if (ReleaseSemaphore((HANDLE)S, n, 0)) { + _available += n; + } } #else void release(int n = 1) @@ -181,7 +191,7 @@ class USemaphore #ifdef _WIN32 int value() const { - LONG V = -1; ReleaseSemaphore((HANDLE)S,0,&V); return V; + return _available; } #else int value() @@ -204,20 +214,21 @@ class USemaphore { CloseHandle(S); S = CreateSemaphore(0,init,SEM_VALUE_MAX,0); + _available = init; } #endif private: void operator=(const USemaphore &){} #ifdef _WIN32 - USemaphore(const USemaphore &S){} + USemaphore(const USemaphore &S) :S(0), _available(0) {} HANDLE S; #else USemaphore(const USemaphore &):_available(0){} pthread_mutex_t _waitMutex; pthread_cond_t _cond; - int _available; #endif + int _available; }; #endif // USEMAPHORE_H