i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 18:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

LiDAR capture support in standalone library (#1264)

Browse Source 
* 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
2024-04-14 19:06:04 -07:00
committed by GitHub
parent 6a6913c939
commit 700704bec9
131 changed files with 10585 additions and 7476 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Odometry.cpp
+71
View File
@@ -140,6 +140,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_alignWithGround(Parameters::defaultOdomAlignWithGround()),
_publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
_deskewing(Parameters::defaultOdomDeskewing()),
_pose(Transform::getIdentity()),
_resetCurrentCount(0),
previousStamp_(0),
@@ -169,6 +170,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
Parameters::parse(parameters, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
Parameters::parse(parameters, Parameters::kOdomDeskewing(), _deskewing);
if(_imageDecimation == 0)
{
@@ -620,6 +622,75 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
UTimer time;
// Deskewing lidar
if( _deskewing &&
!data.laserScanRaw().empty() &&
data.laserScanRaw().hasTime() &&
dt > 0 &&
!guess.isNull())
{
UDEBUG("Deskewing begin");
// Recompute velocity
float vx,vy,vz, vroll,vpitch,vyaw;
guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
// transform to velocity
vx /= dt;
vy /= dt;
vz /= dt;
vroll /= dt;
vpitch /= dt;
vyaw /= dt;
if(!imus_.empty())
{
float scanTime =
data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
// replace orientation velocity based on IMU (if available)
Transform imuFirstScan = Transform::getTransform(imus_,
data.stamp() +
data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()]);
Transform imuLastScan = Transform::getTransform(imus_,
data.stamp() +
data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]);
if(!imuFirstScan.isNull() && !imuLastScan.isNull())
{
Transform orientation = imuFirstScan.inverse() * imuLastScan;
orientation.getEulerAngles(vroll, vpitch, vyaw);
if(_force3DoF)
{
vroll=0;
vpitch=0;
vyaw /= scanTime;
}
else
{
vroll /= scanTime;
vpitch /= scanTime;
vyaw /= scanTime;
}
}
}
Transform velocity(vx,vy,vz,vroll,vpitch,vyaw);
LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
if(!scanDeskewed.isEmpty())
{
data.setLaserScan(scanDeskewed);
}
info->timeDeskewing = time.ticks();
UDEBUG("Deskewing end");
}
if(data.laserScanRaw().isOrganized())
{
// Laser scans should be dense passing this point
data.setLaserScan(data.laserScanRaw().densify());
}
Transform t;
if(_imageDecimation > 1 && !data.imageRaw().empty())
{