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

Merge branch 'devel' of https://github.com/introlab/rtabmap

Browse Source
This commit is contained in:
matlabbe
2017-09-19 14:40:19 -04:00
parent 68fb5d7252 fd18c0b2e9
commit b737df9c40
61 changed files with 2653 additions and 1063 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
tools/KittiDataset/main.cpp
View File

@@ -60,6 +60,7 @@ void showUsage()
" --scan_step # Scan downsample step (default=10).\n"
" --scan_voxel #.# Scan voxel size (default 0.3 m).\n"
" --scan_k Scan normal K (default 20).\n"
" --scan_radius Scan normal radius (default 0).\n"
" --map_update # Do map update each X odometry frames (default=10, which\n"
" gives 1 Hz map update assuming images are at 10 Hz).\n\n"
"%s\n"
@@ -104,6 +105,7 @@ int main(int argc, char * argv[])
int scanStep = 10;
float scanVoxel = 0.3f;
int scanNormalK = 20;
float scanNormalRadius = 0.0f;
std::string gtPath;
if(argc < 2)
{
@@ -153,6 +155,15 @@ int main(int argc, char * argv[])
showUsage();
}
}
else if(std::strcmp(argv[i], "--scan_radius") == 0)
{
scanNormalRadius = atof(argv[++i]);
if(scanNormalRadius < 0.0f)
{
printf("scanNormalRadius should be >= 0\n");
showUsage();
}
}
else if(std::strcmp(argv[i], "--gt") == 0)
{
gtPath = argv[++i];
@@ -233,10 +244,11 @@ int main(int argc, char * argv[])
if(scan)
{
pathScan = path+"/velodyne";
printf(" Scan: %s\n", pathScan.c_str());
printf(" Scan step: %d\n", scanStep);
printf(" Scan voxel: %fm\n", scanVoxel);
printf(" Scan normal k: %d\n", scanNormalK);
printf(" Scan: %s\n", pathScan.c_str());
printf(" Scan step: %d\n", scanStep);
printf(" Scan voxel: %fm\n", scanVoxel);
printf(" Scan normal k: %d\n", scanNormalK);
printf(" Scan normal radius: %f\n", scanNormalRadius);
}
if(!parameters.empty())
{
@@ -338,6 +350,7 @@ int main(int argc, char * argv[])
scanStep,
scanVoxel,
scanNormalK,
scanNormalRadius,
Transform(-0.27f, 0.0f, 0.08, 0.0f, 0.0f, 0.0f));
}
@@ -387,8 +400,8 @@ int main(int argc, char * argv[])
if(odomInfo.interval>0.0)
speed = odomInfo.transform.x()/odomInfo.interval*3.6;
externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers));
externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features));
externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
bool processData = true;
if(iteration % mapUpdate != 0)
@@ -400,11 +413,11 @@ int main(int argc, char * argv[])
}
if(covariance.empty())
{
covariance = odomInfo.covariance;
covariance = odomInfo.reg.covariance;
}
else
{
covariance += odomInfo.covariance;
covariance += odomInfo.reg.covariance;
}
timer.restart();
@@ -418,7 +431,7 @@ int main(int argc, char * argv[])
++iteration;
printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms",
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
if(processData && rtabmap.getLoopClosureId()>0)
{
printf(" *");

9
tools/OdometryViewer/main.cpp
View File

@@ -191,6 +191,7 @@ int main (int argc, char * argv[])
float maxDepth = 4.0f;
float voxelSize = rtabmap::Parameters::defaultIcpVoxelSize();
int normalsK = 0;
float normalsRadius = 0.0f;
if(regStrategy == 1 || regStrategy == 2)
{
// icp requires scans
@@ -203,8 +204,10 @@ int main (int argc, char * argv[])
rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlane(), pointToPlane);
if(pointToPlane)
{
normalsK = rtabmap::Parameters::defaultIcpPointToPlaneNormalNeighbors();
rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneNormalNeighbors(), normalsK);
normalsK = rtabmap::Parameters::defaultIcpPointToPlaneK();
rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneK(), normalsK);
normalsRadius = rtabmap::Parameters::defaultIcpPointToPlaneRadius();
rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneRadius(), normalsRadius);
}
uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpDownsamplingStep(), "1"));
@@ -310,7 +313,7 @@ int main (int argc, char * argv[])
{
rtabmap::CameraThread cameraThread(camera, parameters);
cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK);
cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK, normalsRadius);
odomThread.start();
cameraThread.start();

10
tools/RgbdDataset/main.cpp
View File

@@ -226,8 +226,8 @@ int main(int argc, char * argv[])
Transform pose = odom.process(data, &odomInfo);
externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers));
externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features));
externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
bool processData = true;
if(detectionRate>0.0f &&
@@ -251,11 +251,11 @@ int main(int argc, char * argv[])
}
if(covariance.empty())
{
covariance = odomInfo.covariance;
covariance = odomInfo.reg.covariance;
}
else
{
covariance += odomInfo.covariance;
covariance += odomInfo.reg.covariance;
}
timer.restart();
@@ -269,7 +269,7 @@ int main(int argc, char * argv[])
++iteration;
printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms",
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
if(processData && rtabmap.getLoopClosureId()>0)
{
printf(" *");