i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

RegistrationICP: fixed local scan transform issue

Browse Source
This commit is contained in:
matlabbe
2016-08-31 12:43:54 -04:00
parent 48f54cf2f2
commit edee909324
6 changed files with 66 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/util3d.h
+5
View File
@@ -176,6 +176,11 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
float maxDepth = 0, float maxDepth = 0,
float minDepth = 0, float minDepth = 0,
const Transform & localTransform = Transform::getIdentity()); const Transform & localTransform = Transform::getIdentity());
pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
const cv::Mat & depthImages,
const std::vector<CameraModel> & cameraModels,
float maxDepth,
float minDepth);
// return CV_32FC3 // return CV_32FC3
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
corelib/src/RegistrationIcp.cpp
+4 -4
View File
@@ -143,7 +143,7 @@ Transform RegistrationIcp::computeTransformationImpl(
{ {
//special case if we have already normals computed and there is no filtering //special case if we have already normals computed and there is no filtering
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform); pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, toLocalTransform * guess); pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
UDEBUG("Conversion time = %f s", timer.ticks()); UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>()); pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
@@ -183,7 +183,7 @@ Transform RegistrationIcp::computeTransformationImpl(
else else
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform); pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, toLocalTransform * guess); pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform);
UDEBUG("Conversion time = %f s", timer.ticks()); UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud; pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
@@ -226,7 +226,7 @@ Transform RegistrationIcp::computeTransformationImpl(
// update output scans // update output scans
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform)); fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (toLocalTransform * guess).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform)); toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
UDEBUG("Compute normals time = %f s", timer.ticks()); UDEBUG("Compute normals time = %f s", timer.ticks());
@@ -262,7 +262,7 @@ Transform RegistrationIcp::computeTransformationImpl(
{ {
// update output scans // update output scans
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform)); fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (toLocalTransform * guess).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform)); toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
} }
icpT = util3d::icp( icpT = util3d::icp(
corelib/src/util3d.cpp
+41 -8
View File
@@ -959,14 +959,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
} }
pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage( pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
const cv::Mat & depthImage, const cv::Mat & depthImage,
float fx, float fx,
float fy, float fy,
float cx, float cx,
float cy, float cy,
float maxDepth, float maxDepth,
float minDepth, float minDepth,
const Transform & localTransform) const Transform & localTransform)
{ {
UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1); UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
UASSERT(!localTransform.isNull()); UASSERT(!localTransform.isNull());
@@ -994,6 +994,39 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
return scan; return scan;
} }
pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImages(
const cv::Mat & depthImages,
const std::vector<CameraModel> & cameraModels,
float maxDepth,
float minDepth)
{
pcl::PointCloud<pcl::PointXYZ> scan;
UASSERT(int((depthImages.cols/cameraModels.size())*cameraModels.size()) == depthImages.cols);
int subImageWidth = depthImages.cols/cameraModels.size();
for(unsigned int i=0; i<cameraModels.size(); ++i)
{
if(cameraModels[i].isValidForProjection())
{
cv::Mat depth = cv::Mat(depthImages, cv::Rect(subImageWidth*i, 0, subImageWidth, depthImages.rows));
scan += laserScanFromDepthImage(
depth,
cameraModels[i].fx(),
cameraModels[i].fy(),
cameraModels[i].cx(),
cameraModels[i].cy(),
maxDepth,
minDepth,
cameraModels[i].localTransform());
}
else
{
UERROR("Camera model %d is invalid", i);
}
}
return scan;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform) cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
{ {
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3); cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
guilib/src/DatabaseViewer.cpp
+3 -3
View File
@@ -1705,7 +1705,7 @@ void DatabaseViewer::view3DLaserScans()
{ {
scan = util3d::downsample(scan, downsamplingStepSize); scan = util3d::downsample(scan, downsamplingStepSize);
} }
cloud = util3d::laserScanToPointCloud(scan); cloud = util3d::laserScanToPointCloud(scan, data.laserScanInfo().localTransform());
if(cloud->size()) if(cloud->size())
{ {
@@ -1947,7 +1947,7 @@ void DatabaseViewer::generate3DLaserScans()
{ {
scan = util3d::downsample(scan, downsamplingStepSize); scan = util3d::downsample(scan, downsamplingStepSize);
} }
cloud = util3d::laserScanToPointCloud(scan); cloud = util3d::laserScanToPointCloud(scan, data.laserScanInfo().localTransform());
if(cloud->size()) if(cloud->size())
{ {
@@ -3280,7 +3280,7 @@ void DatabaseViewer::updateConstraintView(
data.uncompressDataConst(0, 0, &scan, 0); data.uncompressDataConst(0, 0, &scan, 0);
if(!scan.empty()) if(!scan.empty())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan); pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan, data.laserScanInfo().localTransform());
if(assembledScans->size() == 0) if(assembledScans->size() == 0)
{ {
assembledScans = util3d::transformPointCloud(scanCloud, iter->second); assembledScans = util3d::transformPointCloud(scanCloud, iter->second);
guilib/src/ExportScansDialog.cpp
+1
View File
@@ -402,6 +402,7 @@ std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> ExportScansDialog::getScan
scan = util3d::downsample(scan, _ui->spinBox_decimation->value()); scan = util3d::downsample(scan, _ui->spinBox_decimation->value());
} }
} }
scan = util3d::transformLaserScan(scan, s.sensorData().laserScanInfo().localTransform());
} }
else else
{ {
guilib/src/MainWindow.cpp
+12 -11
View File
@@ -1036,7 +1036,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
} }
pcl::PointCloud<pcl::PointNormal>::Ptr cloud; pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
cloud = util3d::laserScanToPointCloudNormal(scan, odom.data().laserScanInfo().localTransform()*pose); cloud = util3d::laserScanToPointCloudNormal(scan, pose*odom.data().laserScanInfo().localTransform());
if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0) if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
{ {
cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1)); cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
@@ -2700,7 +2700,11 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
//reconvert the voxelized cloud //reconvert the voxelized cloud
scan = util3d::laserScanFromPointCloud(*cloud); scan = util3d::laserScanFromPointCloud(*cloud);
} }
_createdScans.insert(std::make_pair(nodeId, scan)); else
{
scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
}
_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
} }
} }
else else
@@ -2725,16 +2729,13 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0) if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
{ {
//reconvert the voxelized cloud //reconvert the voxelized cloud
if(scan.channels() == 2) scan = util3d::laserScanFromPointCloud(*cloud);
{
scan = util3d::laserScan2dFromPointCloud(*cloud);
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud);
}
} }
_createdScans.insert(std::make_pair(nodeId, scan)); else
{
scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
}
_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
} }
} }
_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0)); _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));