i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ouster SDK integration

Browse Source
This commit is contained in:
matlabbe
2024-08-17 18:23:20 -07:00
parent 20734cade9
commit b44dd537a7
12 changed files with 842 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
examples/LidarMapping/MapBuilder.h
View File

@@ -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;
}

124
examples/LidarMapping/main.cpp
View File

@@ -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 <rtabmap/core/Odometry.h>
#include "rtabmap/core/Rtabmap.h"
@@ -47,11 +48,19 @@ 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] 0 IP PORT\n"
"rtabmap-lidar_mapping [OPTIONS] 1 IP\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);
}
@@ -61,40 +70,106 @@ int main(int argc, char * argv[])
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
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(i<argc-3)
{
if(strcmp("--resolution", argv[i]) == 0)
{
++i;
resolution = uStr2Float(argv[i]);
++i;
}
else
{
break;
}
}
driver = uStr2Int(argv[i++]);
if(driver <0 || driver>1)
{
printf("Not supported driver %d!\n", driver);
showUsage();
}
if(uStrContains(uToLowerCase(argv[i]), "pcap"))
{
pcapFile = argv[i++];
if(driver == 1)
{
if(i<argc) {
jsonFile=argv[i];
}
else {
printf("Missing JSON file\n");
showUsage();
}
}
}
else if(i<argc-1) {
ip = argv[i++];
if(driver == 0 && i<argc) {
port = uStr2Int(argv[i]);
}
else {
printf("Missing port for driver 0\n");
showUsage();
}
}
else {
printf("Only %d argument(s) provided \n", argc-1);
showUsage();
}
}
printf("Using driver=%s\n", driver==0?"VLP16":"Ouster");
// Here is the pipeline that we will use:
// LidarVLP16 -> "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 +187,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"));
@@ -214,13 +287,12 @@ int main(int argc, char * argv[])
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("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
@@ -234,7 +306,5 @@ int main(int argc, char * argv[])
printf("Saving rtabmap_trajectory.txt... failed!\n");
}
rtabmap->close(false);
return 0;
}