i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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-24 12:15:32 -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

5
corelib/include/rtabmap/core/util3d.h
View File

@@ -176,6 +176,11 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
float maxDepth = 0,
float minDepth = 0,
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
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());

8
corelib/src/RegistrationIcp.cpp
View File

@@ -143,7 +143,7 @@ Transform RegistrationIcp::computeTransformationImpl(
{
//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 toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, toLocalTransform * guess);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
@@ -183,7 +183,7 @@ Transform RegistrationIcp::computeTransformationImpl(
else
{
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());
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
@@ -226,7 +226,7 @@ Transform RegistrationIcp::computeTransformationImpl(
// update output scans
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());
@@ -262,7 +262,7 @@ Transform RegistrationIcp::computeTransformationImpl(
{
// update output scans
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(

49
corelib/src/util3d.cpp
View File

@@ -959,14 +959,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
}
pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
const cv::Mat & depthImage,
float fx,
float fy,
float cx,
float cy,
float maxDepth,
float minDepth,
const Transform & localTransform)
const cv::Mat & depthImage,
float fx,
float fy,
float cx,
float cy,
float maxDepth,
float minDepth,
const Transform & localTransform)
{
UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
UASSERT(!localTransform.isNull());
@@ -994,6 +994,39 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
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 laserScan(1, (int)cloud.size(), CV_32FC3);