cloudFromDepth(): fixed roi ratios not used if one value was null. DbViewer: Added occupancy grid clouds to 3d views, add action to regenerate occupancy grids only for the current selected ids.

corelib/src/util3d.cpp

@@ -717,19 +717,23 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 				cv::Mat depth = cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows));
 				CameraModel model = sensorData.cameraModels()[i];
 				if( roiRatios.size() == 4 &&
-					roiRatios[0] != 0.0f &&
-					roiRatios[1] != 0.0f &&
-					roiRatios[2] != 0.0f &&
-					roiRatios[3] != 0.0f)
-				{
-					if(	int((roiRatios[0]+roiRatios[1])*double(depth.cols))%decimation==0 &&
-						int((roiRatios[2]+roiRatios[3])*double(depth.rows))%decimation==0 &&
-						(model.imageWidth() == 0 ||
-						 model.imageHeight() == 0 ||
-						 (int((roiRatios[0]+roiRatios[1])*double(model.imageWidth()))%decimation==0 &&
-						  int((roiRatios[2]+roiRatios[3])*double(model.imageHeight()))%decimation==0)))
+					(roiRatios[0] > 0.0f ||
+					roiRatios[1] > 0.0f ||
+					roiRatios[2] > 0.0f ||
+					roiRatios[3] > 0.0f))
 				{
 					cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
+					cv::Rect roiRgb;
+					if(model.imageWidth() && model.imageHeight())
+					{
+						roiRgb = util2d::computeRoi(model.imageSize(), roiRatios);
+					}
+					if(	roiDepth.width%decimation==0 &&
+						roiDepth.height%decimation==0 &&
+						(roiRgb.width != 0 ||
+						   (roiRgb.width%decimation==0 &&
+							roiRgb.height%decimation==0)))
+					{
 						depth = cv::Mat(depth, roiDepth);
 						if(model.imageWidth() != 0 && model.imageHeight() != 0)
 						{
@@ -743,10 +747,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 					else
 					{
 						UWARN("Cannot apply ROI ratios because resulting "
-							  "dimension (%dx%d) cannot be divided exactly "
+							  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
 							  "by decimation parameter (%d). Ignoring ROI ratios...",
-							  int((roiRatios[0]+roiRatios[1])*double(depth.cols)),
-							  int((roiRatios[2]+roiRatios[3])*double(depth.rows)),
+							  roiDepth.width,
+							  roiDepth.height,
+							  roiRgb.width,
+							  roiRgb.height,
 							  decimation);
 					}
 				}
@@ -867,18 +873,18 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 				cv::Mat rgb(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows));
 				CameraModel model = sensorData.cameraModels()[i];
 				if( roiRatios.size() == 4 &&
-					roiRatios[0] != 0.0f &&
-					roiRatios[1] != 0.0f &&
-					roiRatios[2] != 0.0f &&
-					roiRatios[3] != 0.0f)
-				{
-					if(	int((roiRatios[0]+roiRatios[1])*double(depth.cols))%decimation==0 &&
-						int((roiRatios[2]+roiRatios[3])*double(depth.rows))%decimation==0 &&
-						int((roiRatios[0]+roiRatios[1])*double(rgb.cols))%decimation==0 &&
-						int((roiRatios[2]+roiRatios[3])*double(rgb.rows))%decimation==0)
+					(roiRatios[0] > 0.0f ||
+					roiRatios[1] > 0.0f ||
+					roiRatios[2] > 0.0f ||
+					roiRatios[3] > 0.0f))
 				{
 					cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
 					cv::Rect roiRgb = util2d::computeRoi(rgb, roiRatios);
+					if(	roiDepth.width%decimation==0 &&
+						roiDepth.height%decimation==0 &&
+						roiRgb.width%decimation==0 &&
+						roiRgb.height%decimation==0)
+					{
 						depth = cv::Mat(depth, roiDepth);
 						rgb = cv::Mat(rgb, roiRgb);
 						model = model.roi(roiRgb);
@@ -888,10 +894,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 						UWARN("Cannot apply ROI ratios because resulting "
 							  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
 							  "by decimation parameter (%d). Ignoring ROI ratios...",
-							  int((roiRatios[0]+roiRatios[1])*double(depth.cols)),
-							  int((roiRatios[2]+roiRatios[3])*double(depth.rows)),
-							  int((roiRatios[0]+roiRatios[1])*double(rgb.cols)),
-							  int((roiRatios[2]+roiRatios[3])*double(rgb.rows)),
+							  roiDepth.width,
+							  roiDepth.height,
+							  roiRgb.width,
+							  roiRgb.height,
 							  decimation);
 					}
 				}

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -84,6 +84,7 @@ private slots:
 	void generateTOROGraph();
 	void generateG2OGraph();
 	void regenerateLocalMaps();
+	void regenerateCurrentLocalMaps();
 	void view3DMap();
 	void view3DLaserScans();
 	void generate3DMap();

guilib/src/DatabaseViewer.cpp

@@ -185,6 +185,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->actionVisual_Refine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineVisuallyAllNeighborLinks()));
 	connect(ui_->actionVisual_Refine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineVisuallyAllLoopClosureLinks()));
 	connect(ui_->actionRegenerate_local_grid_maps, SIGNAL(triggered()), this, SLOT(regenerateLocalMaps()));
+	connect(ui_->actionRegenerate_local_grid_maps_selected, SIGNAL(triggered()), this, SLOT(regenerateCurrentLocalMaps()));
 	connect(ui_->actionReset_all_changes, SIGNAL(triggered()), this, SLOT(resetAllChanges()));
 
 	//ICP buttons
@@ -1595,6 +1596,60 @@ void DatabaseViewer::regenerateLocalMaps()
 	updateGrid();
 }
 
+void DatabaseViewer::regenerateCurrentLocalMaps()
+{
+	UTimer time;
+	OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
+
+	if(ids_.size() == 0)
+	{
+		UWARN("ids_ is empty!");
+		return;
+	}
+
+	QSet<int> idsSet;
+	idsSet.insert(ids_.at(ui_->horizontalSlider_A->value()));
+	idsSet.insert(ids_.at(ui_->horizontalSlider_B->value()));
+	QList<int> ids = idsSet.toList();
+
+	rtabmap::ProgressDialog progressDialog(this);
+	progressDialog.setMaximumSteps(ids.size());
+	progressDialog.show();
+
+	for(int i =0; i<ids.size(); ++i)
+	{
+		generatedLocalMaps_.erase(ids.at(i));
+		generatedLocalMapsInfo_.erase(ids.at(i));
+
+		SensorData data;
+		dbDriver_->getNodeData(ids.at(i), data);
+		data.uncompressData();
+
+		int mapId, weight;
+		Transform odomPose, groundTruth;
+		std::string label;
+		double stamp;
+		QString msg;
+		if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth))
+		{
+			Signature s = data;
+			s.setPose(odomPose);
+			cv::Mat ground, obstacles;
+			cv::Point3f viewpoint;
+			grid.createLocalMap(s, ground, obstacles, viewpoint);
+			uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(ground, obstacles)));
+			uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
+			msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids.size()).arg(time.ticks());
+		}
+
+		progressDialog.appendText(msg);
+		progressDialog.incrementStep();
+		QApplication::processEvents();
+	}
+	progressDialog.setValue(progressDialog.maximumSteps());
+	updateGrid();
+}
+
 void DatabaseViewer::view3DMap()
 {
 	if(!ids_.size() || !dbDriver_)
@@ -2431,6 +2486,8 @@ void DatabaseViewer::update(int value,
 					view3D->removeCloud("0");
 					view3D->removeCloud("1");
 					view3D->removeCloud("map");
+					view3D->removeCloud("ground");
+					view3D->removeCloud("obstacles");
 					if(!data.depthOrRightRaw().empty())
 					{
 						if(!data.imageRaw().empty())
@@ -2505,7 +2562,7 @@ void DatabaseViewer::update(int value,
 					pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanInfo().localTransform());
 					if(scan->size())
 					{
-						view3D->addCloud("1", scan, pose);
+						view3D->addCloud("1", scan, pose, Qt::yellow);
 					}
 
 					//add occupancy grid
@@ -2535,6 +2592,18 @@ void DatabaseViewer::update(int value,
 							cv::Mat map8U = util3d::convertMap2Image8U(map8S);
 							view3D->addOccupancyGridMap(map8U, ui_->doubleSpinBox_gridCellSize->value(), xMin, yMin, 1);
 						}
+
+						// occupancy cloud
+						view3D->addCloud("ground",
+								util3d::laserScanToPointCloud(localMaps.begin()->second.first),
+								Transform::getIdentity(),
+								Qt::green);
+						view3D->addCloud("obstacles",
+								util3d::laserScanToPointCloud(localMaps.begin()->second.second),
+								Transform::getIdentity(),
+								Qt::red);
+						view3D->setCloudPointSize("ground", 5);
+						view3D->setCloudPointSize("obstacles", 5);
 					}
 
 					view3D->update();

guilib/src/ui/DatabaseViewer.ui

@@ -518,6 +518,7 @@
     <addaction name="actionRefine_all_loop_closure_links"/>
     <addaction name="separator"/>
     <addaction name="actionRegenerate_local_grid_maps"/>
+    <addaction name="actionRegenerate_local_grid_maps_selected"/>
     <addaction name="separator"/>
     <addaction name="actionReset_all_changes"/>
     <addaction name="separator"/>
@@ -1374,8 +1375,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>201</width>
-          <height>117</height>
+          <width>289</width>
+          <height>182</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1474,8 +1475,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>175</width>
-          <height>191</height>
+          <width>289</width>
+          <height>182</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1870,6 +1871,11 @@
     <string>Regenerate local grid maps...</string>
    </property>
   </action>
+  <action name="actionRegenerate_local_grid_maps_selected">
+   <property name="text">
+    <string>Regenerate local grid maps (selected)...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>