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

Fixed bug with proximity detection (with combined scans) giving wrong transform sometimes if RGBD/ProximityPathFilteringRadius was used (default true). Fixed util3d::computeNormalsComplexity() when scan has intensity.

Browse Source
This commit is contained in:
matlabbe
2019-02-07 11:19:55 -05:00
parent 6fc884b575
commit 1752b55678
6 changed files with 131 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
corelib/src/LaserScan.cpp
View File

@@ -31,7 +31,48 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
int LaserScan::channels(Format format)
std::string LaserScan::formatName(const Format & format)
{
std::string name;
switch (format) {
case kXY:
name = "XY";
break;
case kXYZ:
name = "XYZ";
break;
case kXYI:
name = "XYI";
break;
case kXYZI:
name = "XYZI";
break;
case kXYZRGB:
name = "XYZRGB";
break;
case kXYNormal:
name = "XYNormal";
break;
case kXYZNormal:
name = "XYZNormal";
break;
case kXYINormal:
name = "XYINormal";
break;
case kXYZINormal:
name = "XYZINormal";
break;
case kXYZRGBNormal:
name = "XYZRGBNormal";
break;
default:
name = "Unknown";
break;
}
return name;
}
int LaserScan::channels(const Format & format)
{
int channels=0;
switch (format) {
@@ -196,12 +237,12 @@ LaserScan::LaserScan(
}
else // verify that format corresponds to expected number of channels
{
UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
}
}
}
@@ -248,12 +289,12 @@ LaserScan::LaserScan(
}
else // verify that format corresponds to expected number of channels
{
UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
}
}
}

21
corelib/src/Rtabmap.cpp
View File

@@ -2321,7 +2321,7 @@ bool Rtabmap::process(
}
// Assemble scans in the path and do ICP only
std::map<int, Transform> filteredPath;
std::map<int, Transform> optimizedLocalPath;
if(_proximityRawPosesUsed)
{
//optimize the path's poses locally
@@ -2333,20 +2333,25 @@ bool Rtabmap::process(
for(std::map<int, Transform>::iterator jter=path.lower_bound(1); jter!=path.end(); ++jter)
{
filteredPath.insert(std::make_pair(jter->first, t * jter->second));
optimizedLocalPath.insert(std::make_pair(jter->first, t * jter->second));
}
}
else
{
filteredPath = path;
optimizedLocalPath = path;
}
if(filteredPath.size() > 2 && _proximityFilteringRadius > 0.0f)
std::map<int, Transform> filteredPath;
if(optimizedLocalPath.size() > 2 && _proximityFilteringRadius > 0.0f)
{
// path filtering
filteredPath = graph::radiusPosesFiltering(filteredPath, _proximityFilteringRadius, 0, true);
filteredPath = graph::radiusPosesFiltering(optimizedLocalPath, _proximityFilteringRadius, 0, true);
// make sure the current pose is still here
filteredPath.insert(*path.find(nearestId));
filteredPath.insert(*optimizedLocalPath.find(nearestId));
}
else
{
filteredPath = optimizedLocalPath;
}
if(filteredPath.size() > 0)
@@ -2371,9 +2376,9 @@ bool Rtabmap::process(
{
std::stringstream stream;
stream << "SCANS:";
for(std::map<int, Transform>::iterator iter=filteredPath.begin(); iter!=filteredPath.end(); ++iter)
for(std::map<int, Transform>::iterator iter=optimizedLocalPath.begin(); iter!=optimizedLocalPath.end(); ++iter)
{
if(iter != filteredPath.begin())
if(iter != optimizedLocalPath.begin())
{
stream << ";";
}

24
corelib/src/util3d_surface.cpp
View File

@@ -2560,36 +2560,28 @@ float computeNormalsComplexity(
bool is2d = scan.is2d();
cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
int oi = 0;
int nOffset = 0;
if(!scan.is2d())
{
nOffset+=1;
}
if(scan.hasIntensity() || scan.hasRGB())
{
nOffset+=1;
}
int nOffset = scan.getNormalsOffset();
for (int i = 0; i < scan.size(); ++i)
{
const float * ptrScan = scan.data().ptr<float>(0, i);
if(is2d)
{
if(uIsFinite(ptrScan[nOffset+2]) && uIsFinite(ptrScan[nOffset+3]))
if(uIsFinite(ptrScan[nOffset]) && uIsFinite(ptrScan[nOffset+1]))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = ptrScan[2];
ptr[1] = ptrScan[3];
ptr[0] = ptrScan[nOffset];
ptr[1] = ptrScan[nOffset+1];
}
}
else
{
if(uIsFinite(ptrScan[nOffset+2]) && uIsFinite(ptrScan[nOffset+3]) && uIsFinite(ptrScan[nOffset+4]))
if(uIsFinite(ptrScan[nOffset]) && uIsFinite(ptrScan[nOffset+1]) && uIsFinite(ptrScan[nOffset+2]))
{
float * ptr = data_normals.ptr<float>(oi++, 0);
ptr[0] = ptrScan[3];
ptr[1] = ptrScan[4];
ptr[2] = ptrScan[5];
ptr[0] = ptrScan[nOffset];
ptr[1] = ptrScan[nOffset+1];
ptr[2] = ptrScan[nOffset+2];
}
}
}

20
corelib/src/util3d_transforms.cpp
View File

@@ -83,7 +83,25 @@ LaserScan transformLaserScan(const LaserScan & laserScan, const Transform & tran
}
}
}
return LaserScan(output, laserScan.maxPoints(), laserScan.rangeMax(), laserScan.format(), laserScan.localTransform());
if(laserScan.angleIncrement() > 0.0f)
{
return LaserScan(output,
laserScan.format(),
laserScan.rangeMin(),
laserScan.rangeMax(),
laserScan.angleMin(),
laserScan.angleMax(),
laserScan.angleIncrement(),
laserScan.localTransform());
}
else
{
return LaserScan(output,
laserScan.maxPoints(),
laserScan.rangeMax(),
laserScan.format(),
laserScan.localTransform());
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(