i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 17: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/util3d.cpp
+230 -13
View File
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UTimer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
@@ -2238,7 +2239,7 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
{
pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
}
else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT)
{
pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
}
@@ -2268,8 +2269,17 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
if(laserScan.isOrganized())
{
output->width = laserScan.data().cols;
output->height = laserScan.data().rows;
output->is_dense = false;
}
else
{
output->is_dense = true;
}
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.size(); ++i)
@@ -2286,8 +2296,17 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & lase
pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform)
{
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
if(laserScan.isOrganized())
{
output->width = laserScan.data().cols;
output->height = laserScan.data().rows;
output->is_dense = false;
}
else
{
output->is_dense = true;
}
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull();
for(int i=0; i<laserScan.size(); ++i)
{
@@ -2303,8 +2322,17 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const LaserSc
pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
if(laserScan.isOrganized())
{
output->width = laserScan.data().cols;
output->height = laserScan.data().rows;
output->is_dense = false;
}
else
{
output->is_dense = true;
}
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.size(); ++i)
@@ -2321,8 +2349,17 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const LaserScan
pcl::PointCloud<pcl::PointXYZI>::Ptr laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform, float intensity)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr output(new pcl::PointCloud<pcl::PointXYZI>);
if(laserScan.isOrganized())
{
output->width = laserScan.data().cols;
output->height = laserScan.data().rows;
output->is_dense = false;
}
else
{
output->is_dense = true;
}
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.size(); ++i)
@@ -2339,8 +2376,17 @@ pcl::PointCloud<pcl::PointXYZI>::Ptr laserScanToPointCloudI(const LaserScan & la
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
if(laserScan.isOrganized())
{
output->width = laserScan.data().cols;
output->height = laserScan.data().rows;
output->is_dense = false;
}
else
{
output->is_dense = true;
}
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
for(int i=0; i<laserScan.size(); ++i)
{
@@ -2356,8 +2402,17 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(cons
pcl::PointCloud<pcl::PointXYZINormal>::Ptr laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform, float intensity)
{
pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>);
if(laserScan.isOrganized())
{
output->width = laserScan.data().cols;
output->height = laserScan.data().rows;
output->is_dense = false;
}
else
{
output->is_dense = true;
}
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
for(int i=0; i<laserScan.size(); ++i)
{
@@ -2374,7 +2429,8 @@ pcl::PointXYZ laserScanToPoint(const LaserScan & laserScan, int index)
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZ output;
const float * ptr = laserScan.data().ptr<float>(0, index);
int row = index / laserScan.data().cols;
const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
@@ -2388,7 +2444,8 @@ pcl::PointNormal laserScanToPointNormal(const LaserScan & laserScan, int index)
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointNormal output;
const float * ptr = laserScan.data().ptr<float>(0, index);
int row = index / laserScan.data().cols;
const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
@@ -2409,7 +2466,8 @@ pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, uns
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZRGB output;
const float * ptr = laserScan.data().ptr<float>(0, index);
int row = index / laserScan.data().cols;
const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
@@ -2448,7 +2506,8 @@ pcl::PointXYZI laserScanToPointI(const LaserScan & laserScan, int index, float i
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZI output;
const float * ptr = laserScan.data().ptr<float>(0, index);
int row = index / laserScan.data().cols;
const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
@@ -2473,7 +2532,8 @@ pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, in
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZRGBNormal output;
const float * ptr = laserScan.data().ptr<float>(0, index);
int row = index / laserScan.data().cols;
const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
@@ -2520,7 +2580,8 @@ pcl::PointXYZINormal laserScanToPointINormal(const LaserScan & laserScan, int in
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZINormal output;
const float * ptr = laserScan.data().ptr<float>(0, index);
int row = index / laserScan.data().cols;
const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
@@ -3489,6 +3550,162 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
return util3d::transformPointCloud(cloud, transform);
}
LaserScan deskew(
const LaserScan & input,
double inputStamp,
const rtabmap::Transform & velocity)
{
if(velocity.isNull())
{
UERROR("velocity should be valid!");
return LaserScan();
}
if(input.format() != LaserScan::kXYZIT)
{
UERROR("input scan doesn't have \"time\" channel! Only format \"%s\" supported yet.", LaserScan::formatName(LaserScan::kXYZIT).c_str());
return LaserScan();
}
if(input.empty())
{
UERROR("input scan is empty!");
return LaserScan();
}
int offsetTime = input.getTimeOffset();
// Get latest timestamp
double firstStamp;
double lastStamp;
firstStamp = inputStamp + input.data().ptr<float>(0, 0)[offsetTime];
lastStamp = inputStamp + input.data().ptr<float>(0, input.size()-1)[offsetTime];
if(lastStamp <= firstStamp)
{
UERROR("First and last stamps in the scan are the same!");
return LaserScan();
}
rtabmap::Transform firstPose;
rtabmap::Transform lastPose;
float vx,vy,vz, vroll,vpitch,vyaw;
velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
// 1- The pose of base frame in odom frame at first stamp
// 2- The pose of base frame in odom frame at last stamp
double dt1 = firstStamp - inputStamp;
double dt2 = lastStamp - inputStamp;
firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
lastPose = rtabmap::Transform(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
if(firstPose.isNull())
{
UERROR("Could not get transform between stamps %f and %f!",
firstStamp,
inputStamp);
return LaserScan();
}
if(lastPose.isNull())
{
UERROR("Could not get transform between stamps %f and %f!",
lastStamp,
inputStamp);
return LaserScan();
}
double stamp;
UTimer processingTime;
double scanTime = lastStamp - firstStamp;
cv::Mat output(1, input.size(), CV_32FC4); // XYZI - Dense
int offsetIntensity = input.getIntensityOffset();
bool isLocalTransformIdentity = input.localTransform().isIdentity();
Transform localTransformInv = input.localTransform().inverse();
bool timeOnColumns = input.data().cols > input.data().rows;
int oi = 0;
if(timeOnColumns)
{
// t1 t2 ...
// ring1 ring1 ...
// ring2 ring2 ...
// ring3 ring4 ...
// ring4 ring3 ...
for(int u=0; u<input.data().cols; ++u)
{
const float * inputPtr = input.data().ptr<float>(0, u);
stamp = inputStamp + inputPtr[offsetTime];
rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);
for(int v=0; v<input.data().rows; ++v)
{
inputPtr = input.data().ptr<float>(v, u);
pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]);
if(pcl::isFinite(pt))
{
if(!isLocalTransformIdentity)
{
pt = rtabmap::util3d::transformPoint(pt, input.localTransform());
}
pt = rtabmap::util3d::transformPoint(pt, transform);
if(!isLocalTransformIdentity)
{
pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
}
float * dataPtr = output.ptr<float>(0, oi++);
dataPtr[0] = pt.x;
dataPtr[1] = pt.y;
dataPtr[2] = pt.z;
dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
}
}
}
}
else // time on rows
{
// t1 ring1 ring2 ring3 ring4
// t2 ring1 ring2 ring3 ring4
// t3 ring1 ring2 ring3 ring4
// t4 ring1 ring2 ring3 ring4
// ... ... ... ... ...
for(int v=0; v<input.data().rows; ++v)
{
const float * inputPtr = input.data().ptr<float>(v, 0);
stamp = inputStamp + inputPtr[offsetTime];
rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);
for(int u=0; u<input.data().cols; ++u)
{
inputPtr = input.data().ptr<float>(v, u);
pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]);
if(pcl::isFinite(pt))
{
if(!isLocalTransformIdentity)
{
pt = rtabmap::util3d::transformPoint(pt, input.localTransform());
}
pt = rtabmap::util3d::transformPoint(pt, transform);
if(!isLocalTransformIdentity)
{
pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
}
float * dataPtr = output.ptr<float>(0, oi++);
dataPtr[0] = pt.x;
dataPtr[1] = pt.y;
dataPtr[2] = pt.z;
dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
}
}
}
}
output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi));
UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
return LaserScan(output, input.maxPoints(), input.rangeMax(), LaserScan::kXYZI, input.localTransform());
}
}
}