DBViewer: updated refine constraints menu action with more options, D…

…BDriver: added updateCalibration for convenience

corelib/include/rtabmap/core/DBDriver.h

@@ -96,6 +96,10 @@ class RTABMAP_CORE_EXPORT DBDriver : public UThreadNode
 				const cv::Mat & empty,
 				float cellSize,
 				const cv::Point3f & viewpoint);
+	void updateCalibration(
+		int nodeId,
+		const std::vector<CameraModel> & models,
+		const std::vector<StereoCameraModel> & stereoModels);
 	void updateDepthImage(int nodeId, const cv::Mat & image);
 	void updateLaserScan(int nodeId, const LaserScan & scan);
 
@@ -231,6 +235,11 @@ class RTABMAP_CORE_EXPORT DBDriver : public UThreadNode
 			float cellSize,
 			const cv::Point3f & viewpoint) const = 0;
 
+	virtual void updateCalibrationQuery(
+			int nodeId,
+			const std::vector<CameraModel> & models,
+			const std::vector<StereoCameraModel> & stereoModels) const = 0;
+
 	virtual void updateDepthImageQuery(
 			int nodeId,
 			const cv::Mat & image) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -96,6 +96,11 @@ class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver {
 			float cellSize,
 			const cv::Point3f & viewpoint) const;
 
+	virtual void updateCalibrationQuery(
+			int nodeId,
+			const std::vector<CameraModel> & models,
+			const std::vector<StereoCameraModel> & stereoModels) const;
+
 	virtual void updateDepthImageQuery(
 			int nodeId,
 			const cv::Mat & image) const;
@@ -153,6 +158,7 @@ class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver {
 	std::string queryStepNode() const;
 	std::string queryStepImage() const;
 	std::string queryStepDepth() const;
+	std::string queryStepCalibrationUpdate() const;
 	std::string queryStepDepthUpdate() const;
 	std::string queryStepScanUpdate() const;
 	std::string queryStepSensorData() const;
@@ -165,6 +171,7 @@ class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver {
 	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
 	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
 	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
+	void stepCalibrationUpdate(sqlite3_stmt * ppStmt, int nodeId, const std::vector<CameraModel> & models, const std::vector<StereoCameraModel> & stereoModels) const;
 	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
 	void stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & image) const;
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;

corelib/src/DBDriver.cpp

@@ -502,6 +502,16 @@ void DBDriver::updateOccupancyGrid(
 	_dbSafeAccessMutex.unlock();
 }
 
+void DBDriver::updateCalibration(int nodeId, const std::vector<CameraModel> & models, const std::vector<StereoCameraModel> & stereoModels)
+{
+	_dbSafeAccessMutex.lock();
+	this->updateCalibrationQuery(
+			nodeId,
+			models,
+			stereoModels);
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image)
 {
 	_dbSafeAccessMutex.lock();

corelib/src/DBDriverSqlite3.cpp

@@ -4615,6 +4615,39 @@ void DBDriverSqlite3::updateOccupancyGridQuery(
 	}
 }
 
+void DBDriverSqlite3::updateCalibrationQuery(
+	int nodeId,
+	const std::vector<CameraModel> & models,
+	const std::vector<StereoCameraModel> & stereoModels) const
+{
+	UDEBUG("");
+	if(_ppDb)
+	{
+		std::string type;
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+
+		// Create query
+		std::string query = queryStepCalibrationUpdate();
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// step calibration
+		stepCalibrationUpdate(ppStmt,
+				nodeId,
+				models,
+				stereoModels);
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		UDEBUG("Time=%fs", timer.ticks());
+	}
+}
+
 void DBDriverSqlite3::updateDepthImageQuery(
 		int nodeId,
 		const cv::Mat & image) const
@@ -5771,6 +5804,131 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 }
 
+std::string DBDriverSqlite3::queryStepCalibrationUpdate() const
+{
+	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
+	return "UPDATE Data SET calibration=? WHERE id=?;";
+}
+void DBDriverSqlite3::stepCalibrationUpdate(
+	sqlite3_stmt * ppStmt,
+	int nodeId,
+	const std::vector<CameraModel> & models,
+	const std::vector<StereoCameraModel> & stereoModels) const
+{
+	if(!ppStmt)
+	{
+		UFATAL("");
+	}
+
+	int rc = SQLITE_OK;
+	int index = 1;
+
+	// calibration
+	std::vector<unsigned char> calibrationData;
+	std::vector<float> calibration;
+	// multi-cameras [fx,fy,cx,cy,width,height,local_transform, ... ,fx,fy,cx,cy,width,height,local_transform] (6+12)*float * numCameras
+	// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
+	if(models.size() && models[0].isValidForProjection())
+	{
+		if(uStrNumCmp(_version, "0.18.0") >= 0)
+		{
+			for(unsigned int i=0; i<models.size(); ++i)
+			{
+				UASSERT(models[i].isValidForProjection());
+				std::vector<unsigned char> data = models[i].serialize();
+				UASSERT(!data.empty());
+				unsigned int oldSize = calibrationData.size();
+				calibrationData.resize(calibrationData.size() + data.size());
+				memcpy(calibrationData.data()+oldSize, data.data(), data.size());
+			}
+		}
+		else if(uStrNumCmp(_version, "0.11.2") >= 0)
+		{
+			calibration.resize(models.size() * (6+Transform().size()));
+			for(unsigned int i=0; i<models.size(); ++i)
+			{
+				UASSERT(models[i].isValidForProjection());
+				const Transform & localTransform = models[i].localTransform();
+				calibration[i*(6+localTransform.size())] = models[i].fx();
+				calibration[i*(6+localTransform.size())+1] = models[i].fy();
+				calibration[i*(6+localTransform.size())+2] = models[i].cx();
+				calibration[i*(6+localTransform.size())+3] = models[i].cy();
+				calibration[i*(6+localTransform.size())+4] = models[i].imageWidth();
+				calibration[i*(6+localTransform.size())+5] = models[i].imageHeight();
+				memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
+			}
+		}
+		else
+		{
+			calibration.resize(models.size() * (4+Transform().size()));
+			for(unsigned int i=0; i<models.size(); ++i)
+			{
+				UASSERT(models[i].isValidForProjection());
+				const Transform & localTransform = models[i].localTransform();
+				calibration[i*(4+localTransform.size())] = models[i].fx();
+				calibration[i*(4+localTransform.size())+1] = models[i].fy();
+				calibration[i*(4+localTransform.size())+2] = models[i].cx();
+				calibration[i*(4+localTransform.size())+3] = models[i].cy();
+				memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
+			}
+		}
+	}
+	else if(stereoModels.size() && stereoModels[0].isValidForProjection())
+	{
+		if(uStrNumCmp(_version, "0.18.0") >= 0)
+		{
+			for(unsigned int i=0; i<stereoModels.size(); ++i)
+			{
+				UASSERT(stereoModels[i].isValidForProjection());
+				std::vector<unsigned char> data = stereoModels[i].serialize();
+				UASSERT(!data.empty());
+				unsigned int oldSize = calibrationData.size();
+				calibrationData.resize(calibrationData.size() + data.size());
+				memcpy(calibrationData.data()+oldSize, data.data(), data.size());
+			}
+		}
+		else
+		{
+			UASSERT_MSG(stereoModels.size()==1, uFormat("Database version (%s) is too old for saving multiple stereo cameras", _version.c_str()).c_str());
+			const Transform & localTransform = stereoModels[0].left().localTransform();
+			calibration.resize(7+localTransform.size());
+			calibration[0] = stereoModels[0].left().fx();
+			calibration[1] = stereoModels[0].left().fy();
+			calibration[2] = stereoModels[0].left().cx();
+			calibration[3] = stereoModels[0].left().cy();
+			calibration[4] = stereoModels[0].baseline();
+			calibration[5] = stereoModels[0].left().imageWidth();
+			calibration[6] = stereoModels[0].left().imageHeight();
+			memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
+		}
+	}
+
+	if(calibrationData.size())
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, calibrationData.data(), calibrationData.size(), SQLITE_STATIC);
+	}
+	else if(calibration.size())
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, calibration.data(), calibration.size()*sizeof(float), SQLITE_STATIC);
+	}
+	else
+	{
+		rc = sqlite3_bind_null(ppStmt, index++);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	//id
+	rc = sqlite3_bind_int(ppStmt, index++, nodeId);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	//step
+	rc=sqlite3_step(ppStmt);
+	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	rc = sqlite3_reset(ppStmt);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+}
+
 std::string DBDriverSqlite3::queryStepDepthUpdate() const
 {
 	if(uStrNumCmp(_version, "0.10.0") < 0)

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -62,6 +62,7 @@ class OctoMap;
 class ExportCloudsDialog;
 class EditDepthArea;
 class EditMapArea;
+class LinkRefiningDialog;
 
 class RTABMAP_GUI_EXPORT DatabaseViewer : public QMainWindow
 {
@@ -125,8 +126,7 @@ private Q_SLOTS:
 	void updateAllNeighborCovariances();
 	void updateAllLoopClosureCovariances();
 	void updateAllLandmarkCovariances();
-	void refineAllNeighborLinks();
-	void refineAllLoopClosureLinks();
+	void refineLinks();
 	void resetAllChanges();
 	void sliderAValueChanged(int);
 	void sliderBValueChanged(int);
@@ -193,8 +193,8 @@ private Q_SLOTS:
 	std::multimap<int, rtabmap::Link> updateLinksWithModifications(
 			const std::multimap<int, rtabmap::Link> & edgeConstraints);
 	void updateLoopClosuresSlider(int from = 0, int to = 0);
-	void updateAllCovariances(const QList<Link> & links);
-	void refineAllLinks(const QList<Link> & links);
+	void updateCovariances(const QList<Link> & links);
+	void refineLinks(const QList<Link> & links);
 	void refineConstraint(int from, int to,  bool silent);
 	bool addConstraint(int from, int to, bool silent);
 	void exportPoses(int format);
@@ -238,6 +238,7 @@ private Q_SLOTS:
 	EditDepthArea * editDepthArea_;
 	QDialog * editMapDialog_;
 	EditMapArea * editMapArea_;
+	LinkRefiningDialog * linkRefiningDialog_;
 
 	bool savedMaximized_;
 	bool firstCall_;

guilib/include/rtabmap/gui/LinkRefiningDialog.h

@@ -0,0 +1,79 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_LINKREFININGDIALOG_H_
+#define RTABMAP_LINKREFININGDIALOG_H_
+
+#include "rtabmap/gui/rtabmap_gui_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Link.h>
+
+#include <QDialog>
+
+class Ui_linkRefiningDialog;
+
+namespace rtabmap {
+
+class RTABMAP_GUI_EXPORT LinkRefiningDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	LinkRefiningDialog(QWidget * parent = 0);
+
+	virtual ~LinkRefiningDialog();
+
+	void setMinMax(
+		int nodeIdMin,
+		int nodeIdMax,
+		int mapIdMin,
+		int mapIdMax);
+
+	Link::Type getLinkType() const;
+	void getIntraInterSessions(bool & intra, bool & inter) const;
+	bool isRangeByNodeId() const;
+	bool isRangeByMapId() const;
+	void getRangeNodeId(int & from, int & to) const;
+	void getRangeMapId(int & from, int & to) const;
+
+private Q_SLOTS:
+	void restoreDefaults();
+	void updateIntraInterState();
+	void setRangeToNodeId();
+	void setRangeToMapId();
+
+private:
+	Ui_linkRefiningDialog * ui_;
+	int defaultNodeIdMin_;
+	int defaultNodeIdMax_;
+	int defaultMapIdMin_;
+	int defaultMapIdMax_;
+};
+
+}
+
+#endif /* RTABMAP_LINKREFININGDIALOG_H_ */

guilib/src/CMakeLists.txt

@@ -36,6 +36,7 @@ SET(headers_ui
 	../include/${PROJECT_PREFIX}/gui/EditMapArea.h
 	../include/${PROJECT_PREFIX}/gui/MultiSessionLocWidget.h
 	../include/${PROJECT_PREFIX}/gui/MultiSessionLocSubView.h
+	../include/${PROJECT_PREFIX}/gui/LinkRefiningDialog.h
 )
 
 SET(qrc 
@@ -81,6 +82,7 @@ SET(SRC_FILES
 	./CreateSimpleCalibrationDialog.cpp
 	./ParametersToolBox.cpp
 	./DepthCalibrationDialog.cpp
+	./LinkRefiningDialog.cpp
 	./3rdParty/QMultiComboBox.cpp
 	./opencv/vtkImageMatSource.cpp
 	${qrc}