diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h
index 1a0daf0a..559e34a8 100644
--- a/corelib/include/rtabmap/core/util3d.h
+++ b/corelib/include/rtabmap/core/util3d.h
@@ -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());
diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp
index 4aad587e..10f13ea0 100644
--- a/corelib/src/RegistrationIcp.cpp
+++ b/corelib/src/RegistrationIcp.cpp
@@ -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(
diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp
index 5f153c87..e7732e83 100644
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -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);
diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp
index d81b07ad..3e49a169 100644
--- a/guilib/src/DatabaseViewer.cpp
+++ b/guilib/src/DatabaseViewer.cpp
@@ -1705,7 +1705,7 @@ void DatabaseViewer::view3DLaserScans()
 					{
 						scan = util3d::downsample(scan, downsamplingStepSize);
 					}
-					cloud = util3d::laserScanToPointCloud(scan);
+					cloud = util3d::laserScanToPointCloud(scan, data.laserScanInfo().localTransform());
 
 					if(cloud->size())
 					{
@@ -1947,7 +1947,7 @@ void DatabaseViewer::generate3DLaserScans()
 						{
 							scan = util3d::downsample(scan, downsamplingStepSize);
 						}
-						cloud = util3d::laserScanToPointCloud(scan);
+						cloud = util3d::laserScanToPointCloud(scan, data.laserScanInfo().localTransform());
 
 						if(cloud->size())
 						{
@@ -3280,7 +3280,7 @@ void DatabaseViewer::updateConstraintView(
 									data.uncompressDataConst(0, 0, &scan, 0);
 									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)
 										{
 											assembledScans = util3d::transformPointCloud(scanCloud, iter->second);
diff --git a/guilib/src/ExportScansDialog.cpp b/guilib/src/ExportScansDialog.cpp
index 44aec783..66c347b2 100644
--- a/guilib/src/ExportScansDialog.cpp
+++ b/guilib/src/ExportScansDialog.cpp
@@ -402,6 +402,7 @@ std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr> ExportScansDialog::getScan
 							scan = util3d::downsample(scan, _ui->spinBox_decimation->value());
 						}
 					}
+					scan = util3d::transformLaserScan(scan, s.sensorData().laserScanInfo().localTransform());
 				}
 				else
 				{
diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp
index ed112cf8..0570394f 100644
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -1036,7 +1036,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 					}
 
 					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)
 					{
 						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
 					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
@@ -2725,16 +2729,13 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 				if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
 				{
 					//reconvert the voxelized cloud
-					if(scan.channels() == 2)
-					{
-						scan = util3d::laserScan2dFromPointCloud(*cloud);
-					}
-					else
-					{
-						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
 			}
 		}
 		_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));