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

Tango: Added "Adjust colors" post-processing option. Updated ICP parameters. Added "Mem/LaserScanNormalK" parameter for convenience.

Browse Source
This commit is contained in:
matlabbe
2016-09-07 17:19:21 -04:00
parent ce1acd9d44
commit 4a072b3dfc
18 changed files with 501 additions and 351 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
corelib/src/Memory.cpp
View File

@@ -86,6 +86,7 @@ Memory::Memory(const ParametersMap & parameters) :
_imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
_laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
@@ -407,6 +408,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
Parameters::parse(parameters, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
Parameters::parse(parameters, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
@@ -3495,9 +3497,20 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
maxLaserScanMaxPts /= _laserScanDownsampleStepSize;
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDownsampling_scan(), t*1000.0f);
if(stats) stats->addStatistic(Statistics::kTimingMemScan_downsampling(), t*1000.0f);
UDEBUG("time downsampling scan = %fs", t);
}
if(!laserScan.empty() && _laserScanNormalK > 0 && laserScan.channels() == 3)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
float x,y,z;
data.laserScanInfo().localTransform().getTranslation(x,y,z);
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _laserScanNormalK, Eigen::Vector3f(x,y,z));
laserScan = util3d::laserScanFromPointCloud(*cloud, *normals);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemScan_normals(), t*1000.0f);
UDEBUG("time normals scan = %fs", t);
}
Signature * s;
if(this->isBinDataKept())

190
corelib/src/util3d.cpp
View File

@@ -1047,18 +1047,19 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
Eigen::Affine3f transform3f = transform.toEigen3f();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
laserScan.at<cv::Vec3f>(i)[0] = pt.x;
laserScan.at<cv::Vec3f>(i)[1] = pt.y;
laserScan.at<cv::Vec3f>(i)[2] = pt.z;
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
}
else
{
laserScan.at<cv::Vec3f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec3f>(i)[1] = cloud.at(i).y;
laserScan.at<cv::Vec3f>(i)[2] = cloud.at(i).z;
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
}
}
@@ -1071,24 +1072,63 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud,
bool nullTransform = transform.isNull() || transform.isIdentity();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
laserScan.at<cv::Vec6f>(i)[0] = pt.x;
laserScan.at<cv::Vec6f>(i)[1] = pt.y;
laserScan.at<cv::Vec6f>(i)[2] = pt.z;
laserScan.at<cv::Vec6f>(i)[3] = pt.normal_x;
laserScan.at<cv::Vec6f>(i)[4] = pt.normal_y;
laserScan.at<cv::Vec6f>(i)[5] = pt.normal_z;
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
ptr[3] = pt.normal_x;
ptr[4] = pt.normal_y;
ptr[5] = pt.normal_z;
}
else
{
laserScan.at<cv::Vec6f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec6f>(i)[1] = cloud.at(i).y;
laserScan.at<cv::Vec6f>(i)[2] = cloud.at(i).z;
laserScan.at<cv::Vec6f>(i)[3] = cloud.at(i).normal_x;
laserScan.at<cv::Vec6f>(i)[4] = cloud.at(i).normal_y;
laserScan.at<cv::Vec6f>(i)[5] = cloud.at(i).normal_z;
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
ptr[3] = cloud.at(i).normal_x;
ptr[4] = cloud.at(i).normal_y;
ptr[5] = cloud.at(i).normal_z;
}
}
return laserScan;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
{
UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
bool nullTransform = transform.isNull() || transform.isIdentity();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointNormal pt;
pt.x = cloud.at(i).x;
pt.y = cloud.at(i).y;
pt.z = cloud.at(i).z;
pt.normal_x = normals.at(i).normal_x;
pt.normal_y = normals.at(i).normal_y;
pt.normal_z = normals.at(i).normal_z;
pt = util3d::transformPoint(pt, transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
ptr[3] = pt.normal_x;
ptr[4] = pt.normal_y;
ptr[5] = pt.normal_z;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
ptr[3] = normals.at(i).normal_x;
ptr[4] = normals.at(i).normal_y;
ptr[5] = normals.at(i).normal_z;
}
}
return laserScan;
@@ -1101,20 +1141,22 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
Eigen::Affine3f transform3f = transform.toEigen3f();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f);
laserScan.at<cv::Vec4f>(i)[0] = pt.x;
laserScan.at<cv::Vec4f>(i)[1] = pt.y;
laserScan.at<cv::Vec4f>(i)[2] = pt.z;
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.z;
}
else
{
laserScan.at<cv::Vec4f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec4f>(i)[1] = cloud.at(i).y;
laserScan.at<cv::Vec4f>(i)[2] = cloud.at(i).z;
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = cloud.at(i).z;
}
laserScan.at<cv::Vec4i>(i)[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
int * ptrInt = (int*)ptr;
ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
}
return laserScan;
}
@@ -1126,16 +1168,17 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
Eigen::Affine3f transform3f = transform.toEigen3f();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
laserScan.at<cv::Vec2f>(i)[0] = pt.x;
laserScan.at<cv::Vec2f>(i)[1] = pt.y;
ptr[0] = pt.x;
ptr[1] = pt.y;
}
else
{
laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
}
}
@@ -1203,28 +1246,12 @@ pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
pcl::PointXYZ output;
if(laserScan.type() == CV_32FC2)
const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3)
{
output.x = laserScan.at<cv::Vec2f>(index)[0];
output.y = laserScan.at<cv::Vec2f>(index)[1];
}
else if(laserScan.type() == CV_32FC3)
{
output.x = laserScan.at<cv::Vec3f>(index)[0];
output.y = laserScan.at<cv::Vec3f>(index)[1];
output.z = laserScan.at<cv::Vec3f>(index)[2];
}
else if(laserScan.type() == CV_32FC(4))
{
output.x = laserScan.at<cv::Vec4f>(index)[0];
output.y = laserScan.at<cv::Vec4f>(index)[1];
output.z = laserScan.at<cv::Vec4f>(index)[2];
}
else
{
output.x = laserScan.at<cv::Vec6f>(index)[0];
output.y = laserScan.at<cv::Vec6f>(index)[1];
output.z = laserScan.at<cv::Vec6f>(index)[2];
output.z = ptr[2];
}
return output;
}
@@ -1234,31 +1261,18 @@ pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
pcl::PointNormal output;
if(laserScan.type() == CV_32FC2)
const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3)
{
output.x = laserScan.at<cv::Vec2f>(index)[0];
output.y = laserScan.at<cv::Vec2f>(index)[1];
output.z = ptr[2];
}
else if(laserScan.type() == CV_32FC3)
if(laserScan.channels() == 6)
{
output.x = laserScan.at<cv::Vec3f>(index)[0];
output.y = laserScan.at<cv::Vec3f>(index)[1];
output.z = laserScan.at<cv::Vec3f>(index)[2];
}
else if(laserScan.type() == CV_32FC(4))
{
output.x = laserScan.at<cv::Vec4f>(index)[0];
output.y = laserScan.at<cv::Vec4f>(index)[1];
output.z = laserScan.at<cv::Vec4f>(index)[2];
}
else
{
output.x = laserScan.at<cv::Vec6f>(index)[0];
output.y = laserScan.at<cv::Vec6f>(index)[1];
output.z = laserScan.at<cv::Vec6f>(index)[2];
output.normal_x = laserScan.at<cv::Vec6f>(index)[3];
output.normal_y = laserScan.at<cv::Vec6f>(index)[4];
output.normal_z = laserScan.at<cv::Vec6f>(index)[5];
output.normal_x = ptr[3];
output.normal_y = ptr[4];
output.normal_z = ptr[5];
}
return output;
}
@@ -1268,31 +1282,19 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index)
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
pcl::PointXYZRGB output;
if(laserScan.type() == CV_32FC2)
const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.type() >= 3)
{
output.x = laserScan.at<cv::Vec2f>(index)[0];
output.y = laserScan.at<cv::Vec2f>(index)[1];
output.z = ptr[2];
}
else if(laserScan.type() == CV_32FC3)
if(laserScan.channels() == 4)
{
output.x = laserScan.at<cv::Vec3f>(index)[0];
output.y = laserScan.at<cv::Vec3f>(index)[1];
output.z = laserScan.at<cv::Vec3f>(index)[2];
}
else if(laserScan.type() == CV_32FC(4))
{
output.x = laserScan.at<cv::Vec4f>(index)[0];
output.y = laserScan.at<cv::Vec4f>(index)[1];
output.z = laserScan.at<cv::Vec4f>(index)[2];
output.b = (unsigned char)(laserScan.at<cv::Vec4i>(index)[3] & 0xFF);
output.g = (unsigned char)((laserScan.at<cv::Vec4i>(index)[3] >> 8) & 0xFF);
output.r = (unsigned char)((laserScan.at<cv::Vec4i>(index)[3] >> 16) & 0xFF);
}
else
{
output.x = laserScan.at<cv::Vec6f>(index)[0];
output.y = laserScan.at<cv::Vec6f>(index)[1];
output.z = laserScan.at<cv::Vec6f>(index)[2];
int * ptrInt = (int*)ptr;
output.b = (unsigned char)(ptrInt[3] & 0xFF);
output.g = (unsigned char)((ptrInt[3] >> 8) & 0xFF);
output.r = (unsigned char)((ptrInt[3] >> 16) & 0xFF);
}
return output;
}