mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-07 02:27:47 +08:00
LiDAR capture support in standalone library (#1264)
* Working rtabmap_lidar-mapping example (live and pcap) * finalizing merge, added some deprecated * fixed build * Working deskewing for Lidar + Camera/IMU (no camera pose correction yet) and Lidar + Odom Sensor in main UI. * backward compatibility * fixed some not used variable warnings, fixed qt build for lidar mapping example * Refactored CameraMobile, added AREngine background support, fixed LidarVPL16 build error with PCL 1.8 * ARCoreJava: buffer last depth image in case its stamp i higher than pose stamp. CameraMobile: added pose buffer. SensorCaptureThread: to get pose, odomSensor should be explicitly set, but can be same as lidar or camera inputs. * Working external lidar on iOS * util3d::commonFiltering()/adjustNormalsToViewPoint() added organized cloud support. MainWindow: updated odomSensor setup * fixed winsock include order * reverted camera tool * disable imu filtering when odom sensor is used * Updated package version * fixed windows build * fixing more windows build erros
This commit is contained in:
matlabbe
@@ -0,0 +1,378 @@
|
||||
/*
|
||||
Copyright (c) 2010-2022, 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 <rtabmap/core/lidar/LidarVLP16.h>
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <rtabmap/utilite/UThread.h>
|
||||
#include <pcl/pcl_config.h>
|
||||
|
||||
#if PCL_VERSION_COMPARE(<, 1, 9, 0)
|
||||
#define VLP_MAX_NUM_LASERS 16
|
||||
#define VLP_DUAL_MODE 0x39
|
||||
#endif
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
/** @brief Function used to check that hour assigned to timestamp in conversion is
|
||||
* correct. Velodyne only returns time since the top of the hour, so if the computer clock
|
||||
* and the velodyne clock (gps-synchronized) are a little off, there is a chance the wrong
|
||||
* hour may be associated with the timestamp
|
||||
*
|
||||
* Original author: Copyright (C) 2019 Matthew Pitropov, Joshua Whitley
|
||||
* Original license: BSD License 2.0
|
||||
* Original code: https://github.com/ros-drivers/velodyne/blob/master/velodyne_driver/include/velodyne_driver/time_conversion.hpp
|
||||
*
|
||||
* @param stamp timestamp recovered from velodyne
|
||||
* @param nominal_stamp time coming from computer's clock
|
||||
* @return timestamp from velodyne, possibly shifted by 1 hour if the function arguments
|
||||
* disagree by more than a half-hour.
|
||||
*/
|
||||
double resolveHourAmbiguity(const double &stamp, const double &nominal_stamp) {
|
||||
const int HALFHOUR_TO_SEC = 1800;
|
||||
double retval = stamp;
|
||||
if (nominal_stamp > stamp) {
|
||||
if (nominal_stamp - stamp > HALFHOUR_TO_SEC) {
|
||||
retval = retval + 2*HALFHOUR_TO_SEC;
|
||||
}
|
||||
} else if (stamp - nominal_stamp > HALFHOUR_TO_SEC) {
|
||||
retval = retval - 2*HALFHOUR_TO_SEC;
|
||||
}
|
||||
return retval;
|
||||
}
|
||||
|
||||
/*
|
||||
* Original author: Copyright (C) 2019 Matthew Pitropov, Joshua Whitley
|
||||
* Original license: BSD License 2.0
|
||||
* Original code: https://github.com/ros-drivers/velodyne/blob/master/velodyne_driver/include/velodyne_driver/time_conversion.hpp
|
||||
*/
|
||||
double rosTimeFromGpsTimestamp(const uint32_t data) {
|
||||
const int HOUR_TO_SEC = 3600;
|
||||
// time for each packet is a 4 byte uint
|
||||
// It is the number of microseconds from the top of the hour
|
||||
double time_nom = UTimer::now();
|
||||
uint32_t cur_hour = time_nom / HOUR_TO_SEC;
|
||||
double stamp = double(cur_hour * HOUR_TO_SEC) + double(data) / 1000000;
|
||||
stamp = resolveHourAmbiguity(stamp, time_nom);
|
||||
return stamp;
|
||||
}
|
||||
|
||||
LidarVLP16::LidarVLP16(
|
||||
const std::string& pcapFile,
|
||||
bool organized,
|
||||
bool stampLast,
|
||||
float frameRate,
|
||||
Transform localTransform) :
|
||||
Lidar(frameRate, localTransform),
|
||||
pcl::VLPGrabber(pcapFile),
|
||||
timingOffsetsDualMode_(false),
|
||||
startSweepTime_(0),
|
||||
startSweepTimeHost_(0),
|
||||
organized_(organized),
|
||||
useHostTime_(false),
|
||||
stampLast_(stampLast)
|
||||
{
|
||||
UDEBUG("Using PCAP file \"%s\"", pcapFile.c_str());
|
||||
}
|
||||
LidarVLP16::LidarVLP16(
|
||||
const boost::asio::ip::address& ipAddress,
|
||||
const std::uint16_t port,
|
||||
bool organized,
|
||||
bool useHostTime,
|
||||
bool stampLast,
|
||||
float frameRate,
|
||||
Transform localTransform) :
|
||||
Lidar(frameRate, localTransform),
|
||||
pcl::VLPGrabber(ipAddress, port),
|
||||
timingOffsetsDualMode_(false),
|
||||
startSweepTime_(0),
|
||||
startSweepTimeHost_(0),
|
||||
organized_(organized),
|
||||
useHostTime_(useHostTime),
|
||||
stampLast_(stampLast)
|
||||
{
|
||||
UDEBUG("Using network lidar with IP=%s port=%d", ipAddress.to_string().c_str(), port);
|
||||
}
|
||||
|
||||
LidarVLP16::~LidarVLP16()
|
||||
{
|
||||
UDEBUG("Stopping lidar...");
|
||||
stop();
|
||||
scanReady_.release();
|
||||
UDEBUG("Stopped lidar!");
|
||||
}
|
||||
|
||||
void LidarVLP16::setOrganized(bool enable)
|
||||
{
|
||||
organized_ = true;
|
||||
}
|
||||
|
||||
bool LidarVLP16::init(const std::string &, const std::string &)
|
||||
{
|
||||
UDEBUG("Init lidar");
|
||||
if(isRunning())
|
||||
{
|
||||
UDEBUG("Stopping lidar...");
|
||||
stop();
|
||||
uSleep(2000); // make sure all callbacks are finished
|
||||
UDEBUG("Stopped lidar!");
|
||||
}
|
||||
startSweepTime_ = 0.0;
|
||||
startSweepTimeHost_ = 0.0;
|
||||
accumulatedScans_.clear();
|
||||
if(organized_)
|
||||
{
|
||||
accumulatedScans_.resize(16);
|
||||
}
|
||||
else
|
||||
{
|
||||
accumulatedScans_.resize(1);
|
||||
}
|
||||
buildTimings(false);
|
||||
start();
|
||||
UDEBUG("Lidar capture started");
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Build a timing table for each block/firing. Stores in timing_offsets vector
|
||||
*/
|
||||
void LidarVLP16::buildTimings(bool dualMode)
|
||||
{
|
||||
// vlp16
|
||||
// timing table calculation, from velodyne user manual
|
||||
timingOffsets_.resize(12);
|
||||
for (size_t i=0; i < timingOffsets_.size(); ++i){
|
||||
timingOffsets_[i].resize(32);
|
||||
}
|
||||
// constants
|
||||
double full_firing_cycle = 55.296 * 1e-6; // seconds
|
||||
double single_firing = 2.304 * 1e-6; // seconds
|
||||
double dataBlockIndex, dataPointIndex;
|
||||
// compute timing offsets
|
||||
for (size_t x = 0; x < timingOffsets_.size(); ++x){
|
||||
for (size_t y = 0; y < timingOffsets_[x].size(); ++y){
|
||||
if (dualMode){
|
||||
dataBlockIndex = (x - (x % 2)) + (y / 16);
|
||||
}
|
||||
else{
|
||||
dataBlockIndex = (x * 2) + (y / 16);
|
||||
}
|
||||
dataPointIndex = y % 16;
|
||||
//timing_offsets[block][firing]
|
||||
timingOffsets_[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
|
||||
}
|
||||
}
|
||||
timingOffsetsDualMode_ = dualMode;
|
||||
}
|
||||
|
||||
void LidarVLP16::toPointClouds (HDLDataPacket *dataPacket)
|
||||
{
|
||||
if (sizeof(HDLLaserReturn) != 3)
|
||||
return;
|
||||
|
||||
double receivedHostTime = UTimer::now();
|
||||
double packetStamp = rosTimeFromGpsTimestamp(dataPacket->gpsTimestamp);
|
||||
if(startSweepTime_==0)
|
||||
{
|
||||
startSweepTime_ = packetStamp;
|
||||
startSweepTimeHost_ = receivedHostTime;
|
||||
}
|
||||
|
||||
bool dualMode = dataPacket->mode == VLP_DUAL_MODE;
|
||||
if(timingOffsets_.empty() || timingOffsetsDualMode_ != dualMode)
|
||||
{
|
||||
// reset everything
|
||||
timingOffsets_.clear();
|
||||
buildTimings(dualMode);
|
||||
startSweepTime_ = packetStamp;
|
||||
startSweepTimeHost_ = receivedHostTime;
|
||||
for(size_t i=0; i<accumulatedScans_.size(); ++i)
|
||||
{
|
||||
accumulatedScans_[i].clear();
|
||||
}
|
||||
}
|
||||
|
||||
double interpolated_azimuth_delta;
|
||||
std::uint8_t index = 1;
|
||||
if (dualMode)
|
||||
{
|
||||
index = 2;
|
||||
}
|
||||
if (dataPacket->firingData[index].rotationalPosition < dataPacket->firingData[0].rotationalPosition)
|
||||
{
|
||||
interpolated_azimuth_delta = ((dataPacket->firingData[index].rotationalPosition + 36000) - dataPacket->firingData[0].rotationalPosition) / 2.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
interpolated_azimuth_delta = (dataPacket->firingData[index].rotationalPosition - dataPacket->firingData[0].rotationalPosition) / 2.0;
|
||||
}
|
||||
|
||||
for (std::uint8_t i = 0; i < HDL_FIRING_PER_PKT; ++i)
|
||||
{
|
||||
HDLFiringData firing_data = dataPacket->firingData[i];
|
||||
|
||||
for (std::uint8_t j = 0; j < HDL_LASER_PER_FIRING; j++)
|
||||
{
|
||||
double current_azimuth = firing_data.rotationalPosition;
|
||||
if (j >= VLP_MAX_NUM_LASERS)
|
||||
{
|
||||
current_azimuth += interpolated_azimuth_delta;
|
||||
}
|
||||
if (current_azimuth > 36000)
|
||||
{
|
||||
current_azimuth -= 36000;
|
||||
}
|
||||
|
||||
double t = 0;
|
||||
if (timingOffsets_.size())
|
||||
t = timingOffsets_[i][j];
|
||||
|
||||
if (current_azimuth < HDLGrabber::last_azimuth_)
|
||||
{
|
||||
if (!accumulatedScans_[0].empty())
|
||||
{
|
||||
UScopeMutex lock(lastScanMutex_);
|
||||
bool notify = lastScan_.laserScanRaw().empty();
|
||||
if(stampLast_)
|
||||
{
|
||||
double lastStamp = startSweepTime_ + accumulatedScans_[accumulatedScans_.size()-1].back().t;
|
||||
double diff = lastStamp - startSweepTime_;
|
||||
lastScan_.setStamp(useHostTime_?startSweepTimeHost_+diff:lastStamp);
|
||||
for(size_t r=0; r<accumulatedScans_.size(); ++r)
|
||||
{
|
||||
for(size_t k=0; k<accumulatedScans_[r].size(); ++k)
|
||||
{
|
||||
accumulatedScans_[r][k].t -= diff;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
lastScan_.setStamp(useHostTime_?startSweepTimeHost_:startSweepTime_);
|
||||
}
|
||||
if(accumulatedScans_.size() > 1)
|
||||
{
|
||||
cv::Mat organizedScan = cv::Mat(1, accumulatedScans_[0].size(), CV_32FC(5), accumulatedScans_[0].data()).clone();
|
||||
for(size_t k=1; k<accumulatedScans_.size(); ++k)
|
||||
{
|
||||
UASSERT((int)accumulatedScans_[k].size() == organizedScan.cols);
|
||||
organizedScan.push_back(cv::Mat(1, accumulatedScans_[k].size(), CV_32FC(5), accumulatedScans_[k].data()).clone());
|
||||
}
|
||||
lastScan_.setLaserScan(LaserScan(organizedScan, 0, 0, LaserScan::kXYZIT, getLocalTransform()));
|
||||
}
|
||||
else
|
||||
{
|
||||
lastScan_.setLaserScan(LaserScan(cv::Mat(1, accumulatedScans_[0].size(), CV_32FC(5), accumulatedScans_[0].data()).clone(), 0, 0, LaserScan::kXYZIT, getLocalTransform()));
|
||||
}
|
||||
if(notify)
|
||||
{
|
||||
scanReady_.release();
|
||||
}
|
||||
|
||||
startSweepTime_ = packetStamp + t;
|
||||
startSweepTimeHost_ = receivedHostTime + t;
|
||||
}
|
||||
for(size_t k=0; k<accumulatedScans_.size(); ++k)
|
||||
{
|
||||
accumulatedScans_[k].clear();
|
||||
}
|
||||
}
|
||||
|
||||
double timeSinceStartOfThisScan = packetStamp + t - startSweepTime_;
|
||||
|
||||
pcl::PointXYZI xyzi;
|
||||
HDLGrabber::computeXYZI (xyzi, current_azimuth, firing_data.laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]);
|
||||
PointXYZIT xyzit;
|
||||
xyzit.x = xyzi.y;
|
||||
xyzit.y = -xyzi.x;
|
||||
xyzit.z = xyzi.z;
|
||||
xyzit.i = xyzi.intensity;
|
||||
xyzit.t = timeSinceStartOfThisScan;
|
||||
|
||||
if(accumulatedScans_.size()>1)
|
||||
{
|
||||
accumulatedScans_[j % VLP_MAX_NUM_LASERS].push_back(xyzit);
|
||||
}
|
||||
else if (! (std::isnan (xyzit.x) || std::isnan (xyzit.y) || std::isnan (xyzit.z)))
|
||||
{
|
||||
accumulatedScans_[0].push_back (xyzit);
|
||||
}
|
||||
last_azimuth_ = current_azimuth;
|
||||
|
||||
if (dualMode)
|
||||
{
|
||||
pcl::PointXYZI dual_xyzi;
|
||||
HDLGrabber::computeXYZI (dual_xyzi, current_azimuth, dataPacket->firingData[i + 1].laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]);
|
||||
|
||||
if(accumulatedScans_.size()>1)
|
||||
{
|
||||
xyzit.x = dual_xyzi.y;
|
||||
xyzit.y = -dual_xyzi.x;
|
||||
xyzit.z = dual_xyzi.z;
|
||||
xyzit.i = dual_xyzi.intensity;
|
||||
xyzit.t = timeSinceStartOfThisScan;
|
||||
accumulatedScans_[j % VLP_MAX_NUM_LASERS].push_back (xyzit);
|
||||
}
|
||||
else if ((dual_xyzi.x != xyzi.x || dual_xyzi.y != xyzi.y || dual_xyzi.z != xyzi.z)
|
||||
&& ! (std::isnan (dual_xyzi.x) || std::isnan (dual_xyzi.y) || std::isnan (dual_xyzi.z)))
|
||||
{
|
||||
xyzit.x = dual_xyzi.y;
|
||||
xyzit.y = -dual_xyzi.x;
|
||||
xyzit.z = dual_xyzi.z;
|
||||
xyzit.i = dual_xyzi.intensity;
|
||||
xyzit.t = timeSinceStartOfThisScan;
|
||||
accumulatedScans_[0].push_back (xyzit);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (dualMode)
|
||||
{
|
||||
i++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
SensorData LidarVLP16::captureData(SensorCaptureInfo * info)
|
||||
{
|
||||
SensorData data;
|
||||
if(scanReady_.acquire(1, 5000))
|
||||
{
|
||||
UScopeMutex lock(lastScanMutex_);
|
||||
if(!lastScan_.laserScanRaw().empty())
|
||||
{
|
||||
data = lastScan_;
|
||||
lastScan_ = SensorData();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("Did not receive any scans for the past 5 seconds.");
|
||||
}
|
||||
return data;
|
||||
}
|
||||
|
||||
|
||||
} /* namespace rtabmap */
|
||||
Reference in New Issue
Block a user