0.18: Camera calibration and LaserScan Info refactoring (#324)

* Saving full camera calibration in database, added angle min/max/inc to LaserScan.

* Updated laserscan info save/load in db

* Database: added Tag table, added env_sensors field to Node

* fixed serialization/deserialization of stereo camera model

* fixed multi-calibration db saving

* fixed rebase errors

* Tango: Added saving environmental sensors option

* Memory: Save env sensors

* Tango: fixed env sensor ids

* DBViewer: show env sensors values

* DBViewer: added calibration details on tooltip

* increased package version to 0.18.0

* Fixed LaserScan copies when angleIncrement is valid

* fixed build error without OctoMap dependency

CMakeLists.txt

@@ -20,8 +20,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 17)
+SET(RTABMAP_MINOR_VERSION 18)
-SET(RTABMAP_PATCH_VERSION 7)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

app/android/AndroidManifest.xml.in

@@ -13,6 +13,7 @@
     <uses-permission android:name="android.permission.INTERNET" />
     <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
     <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
+    <uses-permission android:name="android.permission.ACCESS_WIFI_STATE" />
     <uses-feature android:name="android.hardware.location.gps" />
     <uses-feature android:glEsVersion="0x00020000" />
 

app/android/jni/CameraTango.cpp

@@ -438,6 +438,7 @@ void CameraTango::close()
 	fisheyeRectifyMapX_ = cv::Mat();
 	fisheyeRectifyMapY_ = cv::Mat();
 	lastKnownGPS_ = GPS();
+	lastEnvSensors_.clear();
 	originOffset_ = Transform();
 	originUpdate_ = false;
 }
@@ -545,6 +546,11 @@ void CameraTango::setGPS(const GPS & gps)
 	lastKnownGPS_ = gps;
 }
 
+void CameraTango::addEnvSensor(int type, float value)
+{
+	lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
+}
+
 rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
 {
 	UASSERT(tangoPose);
@@ -907,6 +913,12 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			{
 				LOGD("GPS too old (current time=%f, gps time = %f)", rgbStamp, lastKnownGPS_.stamp());
 			}
+
+			if(lastEnvSensors_.size())
+			{
+				data.setEnvSensors(lastEnvSensors_);
+				lastEnvSensors_.clear();
+			}
 		}
 		else
 		{

app/android/jni/CameraTango.h

@@ -92,6 +92,7 @@ public:
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
 	void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 	void setGPS(const GPS & gps);
+	void addEnvSensor(int type, float value);
 
 	void cloudReceived(const cv::Mat & cloud, double timestamp);
 	void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp);
@@ -130,6 +131,7 @@ private:
 	cv::Mat fisheyeRectifyMapX_;
 	cv::Mat fisheyeRectifyMapY_;
 	GPS lastKnownGPS_;
+	EnvSensors lastEnvSensors_;
 	Transform originOffset_;
 	bool originUpdate_;
 };

app/android/jni/RTABMapApp.cpp

@@ -2064,6 +2064,14 @@ void RTABMapApp::setGPS(const rtabmap::GPS & gps)
 	}
 }
 
+void RTABMapApp::addEnvSensor(int type, float value)
+{
+	if(camera_)
+	{
+		camera_->addEnvSensor(type, value);
+	}
+}
+
 void RTABMapApp::resetMapping()
 {
 	LOGW("Reset!");

app/android/jni/RTABMapApp.h

@@ -148,6 +148,7 @@ class RTABMapApp : public UEventsHandler {
   void setBackgroundColor(float gray);
   int setMappingParameter(const std::string & key, const std::string & value);
   void setGPS(const rtabmap::GPS & gps);
+  void addEnvSensor(int type, float value);
 
   void resetMapping();
   void save(const std::string & databasePath);

app/android/jni/jni_interface.cpp

@@ -357,6 +357,15 @@ Java_com_introlab_rtabmap_RTABMapLib_setGPS(
 			bearing));
 }
 
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_addEnvSensor(
+		JNIEnv*, jobject,
+		int type,
+		float value)
+{
+	return app.addEnvSensor(type, value);
+}
+
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_resetMapping(
 		JNIEnv*, jobject)

app/android/res/layout/activity_settings.xml

@@ -209,6 +209,11 @@
 		            android:title="@string/pref_title_gps_saved"
 		            android:summary="@string/pref_summary_gps_saved"
 		            android:defaultValue="@string/pref_default_gps_saved"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_env_sensors_saved"
+		            android:title="@string/pref_title_env_sensors_saved"
+		            android:summary="@string/pref_summary_env_sensors_saved"
+		            android:defaultValue="@string/pref_default_env_sensors_saved"/>
 		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_db_in_memory"
 		            android:title="@string/pref_title_db_in_memory"

app/android/res/values/strings.xml

@@ -36,6 +36,7 @@
     <string name="fps">"FPS (rendering): "</string>
     <string name="distance">"Distance travelled: "</string>
     <string name="gps">"GPS (long,lat,alt,bearing,err): "</string>
+    <string name="env_sensors">"Sensors: "</string>
     <string name="time">"Time: "</string>
     
     <!-- Preference keys: BEGIN -->
@@ -108,6 +109,8 @@
     <string name="pref_default_raw_scan_saved">false</string>
     <string name="pref_key_gps_saved">pref_key_gps_saved</string>
     <string name="pref_default_gps_saved">false</string>
+    <string name="pref_key_env_sensors_saved">pref_key_env_sensors_saved</string>
+    <string name="pref_default_env_sensors_saved">false</string>
     <string name="pref_key_db_in_memory">pref_key_db_in_memory</string>
     <string name="pref_default_db_in_memory">false</string>
     
@@ -349,7 +352,9 @@
     <string name="pref_title_raw_scan_saved">Save Raw Scan</string>
     <string name="pref_summary_raw_scan_saved">Save raw point clouds in database.</string>
     <string name="pref_title_gps_saved">Save GPS</string>
-    <string name="pref_summary_gps_saved">Save GPS in database.</string>
+    <string name="pref_summary_gps_saved">Save GPS to database.</string>
+    <string name="pref_title_env_sensors_saved">Save Environmental Sensors</string>
+    <string name="pref_summary_env_sensors_saved">Save Wifi strength, temperature, air pressure, light intensity and relative humidity to database.</string>
     <string name="pref_title_db_in_memory">Database In Memory</string>
     <string name="pref_summary_db_in_memory">The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened.</string>
 

app/android/src/com/introlab/rtabmap/RTABMapActivity.java

@@ -13,6 +13,8 @@ import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Date;
 import java.util.HashMap;
+import java.util.Timer;
+import java.util.TimerTask;
 
 import android.app.Activity;
 import android.app.ActivityManager;
@@ -45,6 +47,8 @@ import android.location.Location;
 import android.location.LocationListener;
 import android.location.LocationManager;
 import android.net.Uri;
+import android.net.wifi.WifiInfo;
+import android.net.wifi.WifiManager;
 import android.opengl.GLSurfaceView;
 import android.os.Bundle;
 import android.os.Environment;
@@ -192,14 +196,23 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 	private String mMinInliers;
 	private String mMaxOptimizationError;
 	private boolean mGPSSaved = false;
+	private boolean mEnvSensorsSaved = false;
 	
 	private LocationManager mLocationManager;
 	private LocationListener mLocationListener;
 	private Location mLastKnownLocation;
 	private SensorManager mSensorManager;
+	private WifiManager mWifiManager;
+	private Timer mEnvSensorsTimer = new Timer();
 	Sensor mAccelerometer;
 	Sensor mMagnetometer;
+	Sensor mAmbientTemperature;
+	Sensor mAmbientLight;
+	Sensor mAmbientAirPressure;
+	Sensor mAmbientRelativeHumidity;
 	private float mCompassDeg = 0.0f;
+	private float[] mLastEnvSensors = new float[5];
+	private boolean[] mLastEnvSensorsSet = new boolean[5];
 	
 	private float[] mLastAccelerometer = new float[3];
     private float[] mLastMagnetometer = new float[3];
@@ -220,8 +233,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 	
 	private AlertDialog mMemoryWarningDialog = null;
 	
-	private final int STATUS_TEXTS_SIZE = 19;
+	private final int STATUS_TEXTS_SIZE = 20;
-	private final int STATUS_TEXTS_POSE_INDEX = 5;
+	private final int STATUS_TEXTS_POSE_INDEX = 6;
 	private String[] mStatusTexts = new String[STATUS_TEXTS_SIZE];
 	
 	GestureDetector mGesDetect = null;
@@ -517,8 +530,13 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		mSensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
 		mAccelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
 	    mMagnetometer = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
+	    mAmbientTemperature = mSensorManager.getDefaultSensor(Sensor.TYPE_AMBIENT_TEMPERATURE);
+	    mAmbientLight = mSensorManager.getDefaultSensor(Sensor.TYPE_LIGHT);
+	    mAmbientAirPressure = mSensorManager.getDefaultSensor(Sensor.TYPE_PRESSURE);
+	    mAmbientRelativeHumidity = mSensorManager.getDefaultSensor(Sensor.TYPE_RELATIVE_HUMIDITY);
 	    float [] values = {1,0,0,0,0,1,0,-1,0};
 	    mDeviceToCamera.setValues(values);
+	    mWifiManager = (WifiManager) getSystemService(Context.WIFI_SERVICE);
 		
 		setCamera(1);
 
@@ -527,25 +545,52 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 
 	@Override
 	public void onSensorChanged(SensorEvent event) {
-		if (event.sensor == mAccelerometer) {
+		if(event.sensor == mAccelerometer || event.sensor == mMagnetometer)
-            System.arraycopy(event.values, 0, mLastAccelerometer, 0, event.values.length);
+		{
-            mLastAccelerometerSet = true;
+			if (event.sensor == mAccelerometer) {
-        } else if (event.sensor == mMagnetometer) {
+	            System.arraycopy(event.values, 0, mLastAccelerometer, 0, event.values.length);
-            System.arraycopy(event.values, 0, mLastMagnetometer, 0, event.values.length);
+	            mLastAccelerometerSet = true;
-            mLastMagnetometerSet = true;
+	        } else if (event.sensor == mMagnetometer) {
-        }
+	            System.arraycopy(event.values, 0, mLastMagnetometer, 0, event.values.length);
-        if (mLastAccelerometerSet && mLastMagnetometerSet) {
+	            mLastMagnetometerSet = true;
-            SensorManager.getRotationMatrix(mR, null, mLastAccelerometer, mLastMagnetometer);
+	        }
-            mRMat.setValues(mR);
+	        if (mLastAccelerometerSet && mLastMagnetometerSet) {
-            mNewR.setConcat(mRMat, mDeviceToCamera) ;
+	            SensorManager.getRotationMatrix(mR, null, mLastAccelerometer, mLastMagnetometer);
-            mNewR.getValues(mR);
+	            mRMat.setValues(mR);
-            SensorManager.getOrientation(mR, mOrientation);                 
+	            mNewR.setConcat(mRMat, mDeviceToCamera) ;
-            mCompassDeg = mOrientation[0] * 180.0f/(float)Math.PI;
+	            mNewR.getValues(mR);
-            if(mCompassDeg<0.0f)
+	            SensorManager.getOrientation(mR, mOrientation);                 
-            {
+	            mCompassDeg = mOrientation[0] * 180.0f/(float)Math.PI;
-            	mCompassDeg += 360.0f;
+	            if(mCompassDeg<0.0f)
-            }
+	            {
-        }
+	            	mCompassDeg += 360.0f;
+	            }
+	        }
+		}
+		else if(event.sensor == mAmbientTemperature)
+		{
+			mLastEnvSensors[1] = event.values[0];
+			mLastEnvSensorsSet[1] = true;
+			RTABMapLib.addEnvSensor(2, event.values[0]);
+		}
+		else if(event.sensor == mAmbientAirPressure)
+		{
+			mLastEnvSensors[2] = event.values[0];
+			mLastEnvSensorsSet[2] = true;
+			RTABMapLib.addEnvSensor(3, event.values[0]);
+		}
+		else if(event.sensor == mAmbientLight)
+		{
+			mLastEnvSensors[3] = event.values[0];
+			mLastEnvSensorsSet[3] = true;
+			RTABMapLib.addEnvSensor(4, event.values[0]);
+		}
+		else if(event.sensor == mAmbientRelativeHumidity)
+		{
+			mLastEnvSensors[4] = event.values[0];
+			mLastEnvSensorsSet[4] = true;
+			RTABMapLib.addEnvSensor(5, event.values[0]);
+		}
 	}
 
 	@Override
@@ -664,6 +709,9 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		
 		mLocationManager.removeUpdates(mLocationListener);
 		mSensorManager.unregisterListener(this);
+		mLastAccelerometerSet = false;
+		mLastMagnetometerSet= false;
+		mLastEnvSensorsSet[0] = mLastEnvSensorsSet[1]= mLastEnvSensorsSet[2]= mLastEnvSensorsSet[3]= mLastEnvSensorsSet[4]=false;
 
 		RTABMapLib.onPause();
 		
@@ -735,6 +783,29 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 				mSensorManager.registerListener(this, mAccelerometer, SensorManager.SENSOR_DELAY_UI);
 			    mSensorManager.registerListener(this, mMagnetometer, SensorManager.SENSOR_DELAY_UI);
 			}
+			mEnvSensorsSaved = sharedPref.getBoolean(getString(R.string.pref_key_env_sensors_saved), Boolean.parseBoolean(getString(R.string.pref_default_env_sensors_saved)));
+			if(mEnvSensorsSaved)
+			{
+				mSensorManager.registerListener(this, mAmbientTemperature, SensorManager.SENSOR_DELAY_NORMAL);
+				mSensorManager.registerListener(this, mAmbientAirPressure, SensorManager.SENSOR_DELAY_NORMAL);
+				mSensorManager.registerListener(this, mAmbientLight, SensorManager.SENSOR_DELAY_NORMAL);
+				mSensorManager.registerListener(this, mAmbientRelativeHumidity, SensorManager.SENSOR_DELAY_NORMAL);
+				mEnvSensorsTimer.schedule(new TimerTask() {
+
+			        @Override
+			        public void run() {
+			        	WifiInfo wifiInfo = mWifiManager.getConnectionInfo();
+			        	int dbm = 0;
+			        	if(wifiInfo != null && (dbm = wifiInfo.getRssi()) > -127)
+			        	{
+			        		mLastEnvSensors[0] = (float)dbm;
+			        		mLastEnvSensorsSet[0] = true;
+			        		RTABMapLib.addEnvSensor(1, mLastEnvSensors[0]);
+			        	}
+			        }
+
+			    },0,200);
+			}
 			
 			if(!DISABLE_LOG) Log.d(TAG, "set mapping parameters");
 			RTABMapLib.setOnlineBlending(sharedPref.getBoolean(getString(R.string.pref_key_blending), Boolean.parseBoolean(getString(R.string.pref_default_blending))));
@@ -1233,9 +1304,39 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 				statusTexts[3] = getString(R.string.gps)+String.format("[not yet available, %.0fdeg]", mCompassDeg);
 			}
 		}
+		if(mEnvSensorsSaved)
+		{
+			statusTexts[4] = getString(R.string.env_sensors);
+			
+			if(mLastEnvSensorsSet[0])
+			{
+				statusTexts[4] += String.format(" %.0f dbm", mLastEnvSensors[0]);
+				mLastEnvSensorsSet[0] = false;
+			}
+			if(mLastEnvSensorsSet[1])
+			{
+				statusTexts[4] += String.format(" %.1f %cC", mLastEnvSensors[1], '\u00B0');
+				mLastEnvSensorsSet[1] = false;
+			}
+			if(mLastEnvSensorsSet[2])
+			{
+				statusTexts[4] += String.format(" %.1f hPa", mLastEnvSensors[2]);
+				mLastEnvSensorsSet[2] = false;
+			}
+			if(mLastEnvSensorsSet[3])
+			{
+				statusTexts[4] += String.format(" %.0f lx", mLastEnvSensors[3]);
+				mLastEnvSensorsSet[3] = false;
+			}
+			if(mLastEnvSensorsSet[4])
+			{
+				statusTexts[4] += String.format(" %.0f %%", mLastEnvSensors[4]);
+				mLastEnvSensorsSet[4] = false;
+			}
+		}
 		
 		String formattedDate = new SimpleDateFormat("HH:mm:ss.SSS").format(new Date());
-		statusTexts[4] = getString(R.string.time)+formattedDate;
+		statusTexts[5] = getString(R.string.time)+formattedDate;
 		
 		int index = STATUS_TEXTS_POSE_INDEX;
 		statusTexts[index++] = getString(R.string.nodes)+nodes+" (" + nodesDrawn + " shown)";
@@ -1530,6 +1631,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 
     public void stopDisconnectTimer(){
         notouchHandler.removeCallbacks(notouchCallback);
+        Timer timer = new Timer();
+        timer.cancel();
     }
 		
 	private void updateState(State state)

app/android/src/com/introlab/rtabmap/RTABMapLib.java

@@ -99,6 +99,7 @@ public class RTABMapLib
 			double altitude,  
 			double accuracy,
 			double bearing);
+    public static native void addEnvSensor(int type, float value);
 
     public static native void resetMapping();
     public static native void save(String outputDatabasePath);

app/android/src/com/introlab/rtabmap/TextManager.java

@@ -50,7 +50,7 @@ public class TextManager {
 	public static final int RI_TEXT_TEXTURE_SIZE = 512; // 512
 	public static final float RI_TEXT_HEIGHT_BASE = 32.0f;
 	public static final char RI_TEXT_START = ' ';
-	public static final char RI_TEXT_STOP = '~'+1;
+	public static final char RI_TEXT_STOP = '\u00B0'+1;
 	
 	public float getMaxTextHeight() {return mTextHeight;}
 	

corelib/include/rtabmap/core/CameraModel.h

@@ -115,6 +115,9 @@ public:
 
 	bool load(const std::string & directory, const std::string & cameraName);
 	bool save(const std::string & directory) const;
+	std::vector<unsigned char> serialize() const;
+	unsigned int deserialize(const std::vector<unsigned char>& data);
+	unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
 
 	CameraModel scaled(double scale) const;
 	CameraModel roi(const cv::Rect & roi) const;

corelib/include/rtabmap/core/DBDriver.h

@@ -165,8 +165,9 @@ public:
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	bool getLaserScanInfo(int signatureId, LaserScan & info) const;
-	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
+	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
+	void loadTags(int signatureId, std::map<int, TransformStamped> & tags) const;
 	void getWeight(int signatureId, int & weight) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
 	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true) const;
@@ -259,11 +260,12 @@ protected:
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
+	virtual void loadTagsQuery(int signatureId, std::map<int, TransformStamped> & tags) const = 0;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const = 0;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -131,11 +131,12 @@ protected:
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
+	virtual void loadTagsQuery(int signatureId, std::map<int, TransformStamped> & tags) const;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
@@ -151,18 +152,17 @@ private:
 	std::string queryStepSensorData() const;
 	std::string queryStepLinkUpdate() const;
 	std::string queryStepLink() const;
+	std::string queryStepTag() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
 	std::string queryStepOccupancyGridUpdate() const;
 	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
-	void stepImage(
+	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
-			sqlite3_stmt * ppStmt,
-			int id,
-			const cv::Mat & imageBytes) const;
 	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
+	void stepTag(sqlite3_stmt * ppStmt, int nodeId, int tagId, const TransformStamped & pose) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
 	void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
@@ -175,6 +175,7 @@ private:
 
 private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
+	void loadTagsQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
 
 protected:

corelib/include/rtabmap/core/EnvSensor.h

@@ -0,0 +1,85 @@
+/*
+Copyright (c) 2010-2018, 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 CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
+
+namespace rtabmap {
+
+class EnvSensor
+{
+public:
+	enum Type {
+		// built-in types
+		kUndefined = 0,
+		kWifiSignalStrength,      // dBm
+		kAmbientTemperature,      // Celcius
+		kAmbientAirPressure,      // hPa
+		kAmbientLight,            // lx
+		kAmbientRelativeHumidity, // %
+
+		// user types
+		kCustomSensor1 = 100,
+		kCustomSensor2,
+		kCustomSensor3,
+		kCustomSensor4,
+		kCustomSensor5,
+		kCustomSensor6,
+		kCustomSensor7,
+		kCustomSensor8,
+		kCustomSensor9
+	};
+
+public:
+	EnvSensor() :
+		type_(kUndefined),
+		value_(0.0),
+		stamp_(0.0)
+	{}
+	EnvSensor(const Type & type, const double & value,const double & stamp = 0) :
+		type_(type),
+		value_(value),
+		stamp_(stamp)
+	{}
+
+	virtual ~EnvSensor() {}
+
+	const Type & type() const {return type_;}
+	const double & value() const {return value_;}
+	const double & stamp() const {return stamp_;}
+
+private:
+	Type type_;
+	double value_;
+	double stamp_;
+};
+
+typedef std::map<EnvSensor::Type, EnvSensor> EnvSensors;
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_ */

corelib/include/rtabmap/core/GPS.h

@@ -0,0 +1,78 @@
+/*
+Copyright (c) 2010-2018, 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 CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_
+
+#include <rtabmap/core/GeodeticCoords.h>
+
+namespace rtabmap {
+
+class GPS
+{
+public:
+	GPS():
+		stamp_(0.0),
+		longitude_(0.0),
+		latitude_(0.0),
+		altitude_(0.0),
+		error_(0.0),
+		bearing_(0.0)
+	{}
+	GPS(const double & stamp,
+		const double & longitude,
+		const double & latitude,
+		const double & altitude,
+		const double & error,
+		const double & bearing):
+			stamp_(stamp),
+			longitude_(longitude),
+			latitude_(latitude),
+			altitude_(altitude),
+			error_(error),
+			bearing_(bearing)
+	{}
+	const double & stamp() const {return stamp_;}
+	const double & longitude() const {return longitude_;}
+	const double & latitude() const {return latitude_;}
+	const double & altitude() const {return altitude_;}
+	const double & error() const {return error_;}
+	const double & bearing() const {return bearing_;}
+
+	GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
+private:
+	double stamp_;     // in sec
+	double longitude_; // DD
+	double latitude_;  // DD
+	double altitude_;  // m
+	double error_;     // m
+	double bearing_;   // deg (North 0->360 clockwise)
+};
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_ */

corelib/include/rtabmap/core/GeodeticCoords.h

@@ -81,47 +81,6 @@ private:
 	double altitude_;  // m
 };
 
-class GPS
-{
-public:
-	GPS():
-		stamp_(0.0),
-		longitude_(0.0),
-		latitude_(0.0),
-		altitude_(0.0),
-		error_(0.0),
-		bearing_(0.0)
-	{}
-	GPS(const double & stamp,
-		const double & longitude,
-		const double & latitude,
-		const double & altitude,
-		const double & error,
-		const double & bearing):
-			stamp_(stamp),
-			longitude_(longitude),
-			latitude_(latitude),
-			altitude_(altitude),
-			error_(error),
-			bearing_(bearing)
-	{}
-	const double & stamp() const {return stamp_;}
-	const double & longitude() const {return longitude_;}
-	const double & latitude() const {return latitude_;}
-	const double & altitude() const {return altitude_;}
-	const double & error() const {return error_;}
-	const double & bearing() const {return bearing_;}
-
-	GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
-private:
-	double stamp_;     // in sec
-	double longitude_; // DD
-	double latitude_;  // DD
-	double altitude_;  // m
-	double error_;     // m
-	double bearing_;   // deg (North 0->360 clockwise)
-};
-
 }
 
 #endif /* GEODETICCOORDS_H_ */

corelib/include/rtabmap/core/Graph.h

@@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <list>
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Link.h>
-#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/core/GPS.h>
 
 namespace rtabmap {
 class Memory;

corelib/include/rtabmap/core/LaserScan.h

@@ -54,16 +54,44 @@ public:
 	static bool isScanHasNormals(const Format & format);
 	static bool isScanHasRGB(const Format & format);
 	static bool isScanHasIntensity(const Format & format);
-	static LaserScan backwardCompatibility(const cv::Mat & oldScanFormat, int maxPoints = 0, int maxRange = 0, const Transform & localTransform = Transform::getIdentity());
+	static LaserScan backwardCompatibility(
+			const cv::Mat & oldScanFormat,
+			int maxPoints = 0,
+			int maxRange = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	static LaserScan backwardCompatibility(
+			const cv::Mat & oldScanFormat,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleInc,
+			const Transform & localTransform = Transform::getIdentity());
 
 public:
 	LaserScan();
-	LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format format, const Transform & localTransform = Transform::getIdentity());
+	LaserScan(const cv::Mat & data,
+			int maxPoints,
+			float maxRange,
+			Format format,
+			const Transform & localTransform = Transform::getIdentity());
+	LaserScan(const cv::Mat & data,
+			Format format,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleIncrement,
+			const Transform & localTransform = Transform::getIdentity());
 
 	const cv::Mat & data() const {return data_;}
-	int maxPoints() const {return maxPoints_;}
-	float maxRange() const {return maxRange_;}
 	Format format() const {return format_;}
+	int maxPoints() const {return maxPoints_;}
+	float rangeMin() const {return rangeMin_;}
+	float rangeMax() const {return rangeMax_;}
+	float angleMin() const {return angleMin_;}
+	float angleMax() const {return angleMax_;}
+	float angleIncrement() const {return angleIncrement_;}
 	Transform localTransform() const {return localTransform_;}
 
 	bool isEmpty() const {return data_.empty();}
@@ -74,7 +102,7 @@ public:
 	bool hasRGB() const {return isScanHasRGB(format_);}
 	bool hasIntensity() const {return isScanHasIntensity(format_);}
 	bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
-	LaserScan clone() const {return LaserScan(data_.clone(), maxPoints_, maxRange_, format_, localTransform_.clone());}
+	LaserScan clone() const;
 
 	int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
 	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
@@ -84,9 +112,13 @@ public:
 
 private:
 	cv::Mat data_;
-	int maxPoints_;
-	float maxRange_;
 	Format format_;
+	int maxPoints_;
+	float rangeMin_;
+	float rangeMax_;
+	float angleMin_;
+	float angleMax_;
+	float angleIncrement_;
 	Transform localTransform_;
 };
 

corelib/include/rtabmap/core/Memory.h

@@ -186,6 +186,7 @@ public:
 			Transform & groundTruth,
 			std::vector<float> & velocity,
 			GPS & gps,
+			EnvSensors & sensors,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
 	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;

corelib/include/rtabmap/core/SensorData.h

@@ -33,11 +33,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraModel.h>
 #include <rtabmap/core/StereoCameraModel.h>
 #include <rtabmap/core/Transform.h>
-#include <rtabmap/core/GeodeticCoords.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <rtabmap/core/LaserScan.h>
 #include <rtabmap/core/IMU.h>
+#include <rtabmap/core/GPS.h>
+#include <rtabmap/core/EnvSensor.h>
 
 namespace rtabmap
 {
@@ -235,18 +236,16 @@ public:
 	const Transform & globalPose() const {return globalPose_;}
 	const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
 
-	void setGPS(const GPS & gps)
+	void setGPS(const GPS & gps) {gps_ = gps;}
-	{
-		gps_ = gps;
-	}
 	const GPS & gps() const {return gps_;}
 
-	void setIMU(const IMU & imu)
+	void setIMU(const IMU & imu) {imu_ = imu; }
-	{
-		imu_ = imu;
-	}
 	const IMU & imu() const {return imu_;}
 
+	void setEnvSensors(const EnvSensors & sensors) {_envSensors = sensors;}
+	void addEnvSensor(const EnvSensor & sensor) {_envSensors.insert(std::make_pair(sensor.type(), sensor));}
+	const EnvSensors & envSensors() const {return _envSensors;}
+
 	long getMemoryUsed() const; // Return memory usage in Bytes
 	void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed.clear(); _userDataCompressed=cv::Mat();}
 
@@ -281,6 +280,12 @@ private:
 	float _cellSize;
 	cv::Point3f _viewPoint;
 
+	// environmental sensors
+	EnvSensors _envSensors;
+
+	// tags
+	std::map<int, TransformStamped> _tags;
+
 	// features
 	std::vector<cv::KeyPoint> _keypoints;
 	std::vector<cv::Point3f> _keypoints3D;

corelib/include/rtabmap/core/Signature.h

@@ -90,6 +90,10 @@ public:
 	void removeLink(int idTo);
 	void removeVirtualLinks();
 
+	void setTags(const std::map<int, TransformStamped> & tags) {_tags = tags;}
+	void addTag(int id, const TransformStamped & pose) {_tags.insert(std::make_pair(id, pose));}
+	const std::map<int, TransformStamped> & getTags() const {return _tags;}
+
 	void setSaved(bool saved) {_saved = saved;}
 	void setModified(bool modified) {_modified = modified; _linksModified = modified;}
 
@@ -141,6 +145,7 @@ private:
 	int _mapId;
 	double _stamp;
 	std::map<int, Link> _links; // id, transform
+	std::map<int, TransformStamped> _tags;
 	int _weight;
 	std::string _label;
 	bool _saved; // If it's saved to bd

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -97,6 +97,9 @@ public:
 	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
 	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
 	bool saveStereoTransform(const std::string & directory) const;
+	std::vector<unsigned char> serialize() const;
+	unsigned int deserialize(const std::vector<unsigned char>& data);
+	unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
 
 	double baseline() const {return right_.fx()!=0.0 && left_.fx() != 0.0 ? left_.Tx() / left_.fx() - right_.Tx()/right_.fx():0.0;}
 

corelib/include/rtabmap/core/Transform.h

@@ -155,6 +155,21 @@ private:
 
 RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s);
 
+class TransformStamped
+{
+public:
+	TransformStamped(const Transform & transform, const double & stamp) :
+		transform_(transform),
+		stamp_(stamp)
+	{}
+	const Transform & transform() const {return transform_;}
+	const double & stamp() const {return stamp_;}
+
+private:
+	Transform transform_;
+	double stamp_;
+};
+
 }
 
 #endif /* TRANSFORM_H_ */

corelib/src/CameraModel.cpp

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/Version.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UFile.h>
@@ -449,6 +450,124 @@ bool CameraModel::save(const std::string & directory) const
 	return false;
 }
 
+std::vector<unsigned char> CameraModel::serialize() const
+{
+	const int headerSize = 11;
+	int header[headerSize] = {
+			RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
+			0, //mono                                                            // 3,
+			imageSize_.width, imageSize_.height,                                 // 4,5
+			(int)K_.total(), (int)D_.total(), (int)R_.total(), (int)P_.total(),  // 6,7,8,9
+			localTransform_.isNull()?0:localTransform_.size()};                  // 10
+	UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
+	std::vector<unsigned char> data(
+			sizeof(int)*headerSize +
+			sizeof(double)*(K_.total()+D_.total()+R_.total()+P_.total()) +
+			(localTransform_.isNull()?0:sizeof(float)*localTransform_.size()));
+	memcpy(data.data(), header, sizeof(int)*headerSize);
+	int index = sizeof(int)*headerSize;
+	if(!K_.empty())
+	{
+		memcpy(data.data()+index, K_.data, sizeof(double)*(K_.total()));
+		index+=sizeof(double)*(K_.total());
+	}
+	if(!D_.empty())
+	{
+		memcpy(data.data()+index, D_.data, sizeof(double)*(D_.total()));
+		index+=sizeof(double)*(D_.total());
+	}
+	if(!R_.empty())
+	{
+		memcpy(data.data()+index, R_.data, sizeof(double)*(R_.total()));
+		index+=sizeof(double)*(R_.total());
+	}
+	if(!P_.empty())
+	{
+		memcpy(data.data()+index, P_.data, sizeof(double)*(P_.total()));
+		index+=sizeof(double)*(P_.total());
+	}
+	if(!localTransform_.isNull())
+	{
+		memcpy(data.data()+index, localTransform_.data(), sizeof(float)*(localTransform_.size()));
+		index+=sizeof(float)*(localTransform_.size());
+	}
+	return data;
+}
+
+unsigned int CameraModel::deserialize(const std::vector<unsigned char>& data)
+{
+	return deserialize(data.data(), data.size());
+}
+unsigned int CameraModel::deserialize(const unsigned char * data, unsigned int dataSize)
+{
+	*this = CameraModel();
+	int headerSize = 11;
+	if(dataSize >= sizeof(int)*headerSize)
+	{
+		UASSERT(data != 0);
+		const int * header = (const int *)data;
+		int type = header[3];
+		if(type == 0)
+		{
+			imageSize_.width = header[4];
+			imageSize_.height = header[5];
+			int iK = 6;
+			int iD = 7;
+			int iR = 8;
+			int iP = 9;
+			int iL = 10;
+			UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
+			unsigned int requiredDataSize = sizeof(int)*headerSize +
+					sizeof(double)*(header[iK]+header[iD]+header[iR]+header[iP]) +
+					sizeof(float)*header[iL];
+			UASSERT_MSG(dataSize >= requiredDataSize,
+					uFormat("dataSize=%d != required=%d (header: version %d.%d.%d %dx%d type=%d K=%d D=%d R=%d P=%d L=%d)",
+							dataSize,
+							requiredDataSize,
+							header[0], header[1], header[2], header[4], header[5], header[3],
+							header[iK], header[iD], header[iR],header[iP], header[iL]).c_str());
+			unsigned int index = sizeof(int)*headerSize;
+			if(header[iK] != 0)
+			{
+				UASSERT(header[iK] == 9);
+				K_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
+				index+=sizeof(double)*(K_.total());
+			}
+			if(header[iD] != 0)
+			{
+				D_ = cv::Mat(1, header[iD], CV_64FC1, (void*)(data+index)).clone();
+				index+=sizeof(double)*(D_.total());
+			}
+			if(header[iR] != 0)
+			{
+				UASSERT(header[iR] == 9);
+				R_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
+				index+=sizeof(double)*(R_.total());
+			}
+			if(header[iP] != 0)
+			{
+				UASSERT(header[iP] == 12);
+				P_ = cv::Mat(3, 4, CV_64FC1, (void*)(data+index)).clone();
+				index+=sizeof(double)*(P_.total());
+			}
+			if(header[iL] != 0)
+			{
+				UASSERT(header[iL] == 12);
+				memcpy(localTransform_.data(), data+index, sizeof(float)*localTransform_.size());
+				index+=sizeof(float)*localTransform_.size();
+			}
+			UASSERT(index <= dataSize);
+			return index;
+		}
+		else
+		{
+			UERROR("Serialized calibration is not mono (type=%d), use the appropriate class matching the type to deserialize.", type);
+		}
+	}
+	UERROR("Wrong serialized calibration data format detected (size in bytes=%d)! Cannot deserialize the data.", (int)dataSize);
+	return 0;
+}
+
 CameraModel CameraModel::scaled(double scale) const
 {
 	CameraModel scaledModel = *this;

corelib/src/DBDriver.cpp

@@ -733,7 +733,8 @@ bool DBDriver::getNodeInfo(
 		double & stamp,
 		Transform & groundTruthPose,
 		std::vector<float> & velocity,
-		GPS & gps) const
+		GPS & gps,
+		EnvSensors & sensors) const
 {
 	bool found = false;
 	// look in the trash
@@ -747,6 +748,7 @@ bool DBDriver::getNodeInfo(
 		stamp = _trashSignatures.at(signatureId)->getStamp();
 		groundTruthPose = _trashSignatures.at(signatureId)->getGroundTruthPose();
 		gps = _trashSignatures.at(signatureId)->sensorData().gps();
+		sensors = _trashSignatures.at(signatureId)->sensorData().envSensors();
 		found = true;
 	}
 	_trashesMutex.unlock();
@@ -754,7 +756,7 @@ bool DBDriver::getNodeInfo(
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose, velocity, gps);
+		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose, velocity, gps, sensors);
 		_dbSafeAccessMutex.unlock();
 	}
 	return found;
@@ -790,6 +792,33 @@ void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Typ
 	}
 }
 
+void DBDriver::loadTags(int signatureId, std::map<int, TransformStamped> & tags) const
+{
+	bool found = false;
+	// look in the trash
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		const Signature * s = _trashSignatures.at(signatureId);
+		UASSERT(s != 0);
+		for(std::map<int, TransformStamped>::const_iterator nIter = s->getTags().begin();
+				nIter!=s->getTags().end();
+				++nIter)
+		{
+			tags.insert(*nIter);
+		}
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		this->loadTagsQuery(signatureId, tags);
+		_dbSafeAccessMutex.unlock();
+	}
+}
+
 void DBDriver::getWeight(int signatureId, int & weight) const
 {
 	bool found = false;

corelib/src/DBReader.cpp

@@ -268,7 +268,8 @@ SensorData DBReader::captureImage(CameraInfo * info)
 						Transform localTransform, pose, groundTruth;
 						std::vector<float> velocity;
 						GPS gps;
-						_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
+						EnvSensors sensors;
+						_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors);
 						if(previousStamp && stamp && stamp > previousStamp)
 						{
 							delay = stamp - previousStamp;
@@ -323,7 +324,8 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			Transform groundTruth;
 			std::vector<float> velocity;
 			GPS gps;
-			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
+			EnvSensors sensors;
+			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors);
 
 			cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
 			if(!_odometryIgnored)
@@ -437,6 +439,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			data.setStamp(stamp);
 			data.setGroundTruth(groundTruth);
 			data.setGPS(gps);
+			data.setEnvSensors(sensors);
 			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
 					data.laserScanRaw().isEmpty()?0:1,
 					data.imageRaw().empty()?0:1,

corelib/src/LaserScan.cpp

@@ -81,7 +81,11 @@ bool LaserScan::isScanHasIntensity(const Format & format)
 	return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal;
 }
 
-LaserScan LaserScan::backwardCompatibility(const cv::Mat & oldScanFormat, int maxPoints, int maxRange, const Transform & localTransform)
+LaserScan LaserScan::backwardCompatibility(
+		const cv::Mat & oldScanFormat,
+		int maxPoints,
+		int maxRange,
+		const Transform & localTransform)
 {
 	if(!oldScanFormat.empty())
 	{
@@ -113,19 +117,71 @@ LaserScan LaserScan::backwardCompatibility(const cv::Mat & oldScanFormat, int ma
 	return LaserScan();
 }
 
+LaserScan LaserScan::backwardCompatibility(
+		const cv::Mat & oldScanFormat,
+		float minRange,
+		float maxRange,
+		float angleMin,
+		float angleMax,
+		float angleInc,
+		const Transform & localTransform)
+{
+	if(!oldScanFormat.empty())
+	{
+		if(oldScanFormat.channels() == 2)
+		{
+			return LaserScan(oldScanFormat, kXY, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
+		}
+		else if(oldScanFormat.channels() == 3)
+		{
+			return LaserScan(oldScanFormat, kXYZ, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
+		}
+		else if(oldScanFormat.channels() == 4)
+		{
+			return LaserScan(oldScanFormat, kXYZRGB, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
+		}
+		else if(oldScanFormat.channels() == 5)
+		{
+			return LaserScan(oldScanFormat, kXYNormal, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
+		}
+		else if(oldScanFormat.channels() == 6)
+		{
+			return LaserScan(oldScanFormat, kXYZNormal, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
+		}
+		else if(oldScanFormat.channels() == 7)
+		{
+			return LaserScan(oldScanFormat, kXYZRGBNormal, minRange, maxRange, angleMin, angleMax, angleInc, localTransform);
+		}
+	}
+	return LaserScan();
+}
+
 LaserScan::LaserScan() :
-		maxPoints_(0),
-		maxRange_(0),
 		format_(kUnknown),
+		maxPoints_(0),
+		rangeMin_(0),
+		rangeMax_(0),
+		angleMin_(0),
+		angleMax_(0),
+		angleIncrement_(0),
 		localTransform_(Transform::getIdentity())
 {
 }
 
-LaserScan::LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format format, const Transform & localTransform) :
+LaserScan::LaserScan(
+		const cv::Mat & data,
+		int maxPoints,
+		float maxRange,
+		Format format,
+		const Transform & localTransform) :
 	data_(data),
-	maxPoints_(maxPoints),
-	maxRange_(maxRange),
 	format_(format),
+	maxPoints_(maxPoints),
+	rangeMin_(0),
+	rangeMax_(maxRange),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0),
 	localTransform_(localTransform)
 {
 	UASSERT(data.empty() || data.rows == 1);
@@ -136,7 +192,7 @@ LaserScan::LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format
 	{
 		if(format == kUnknown)
 		{
-			*this = backwardCompatibility(data_, maxPoints_, maxRange_, localTransform_);
+			*this = backwardCompatibility(data_, maxPoints_, rangeMax_, localTransform_);
 		}
 		else // verify that format corresponds to expected number of channels
 		{
@@ -150,4 +206,65 @@ LaserScan::LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format
 	}
 }
 
+LaserScan::LaserScan(
+		const cv::Mat & data,
+		Format format,
+		float minRange,
+		float maxRange,
+		float angleMin,
+		float angleMax,
+		float angleIncrement,
+		const Transform & localTransform) :
+	data_(data),
+	format_(format),
+	rangeMin_(minRange),
+	rangeMax_(maxRange),
+	angleMin_(angleMin),
+	angleMax_(angleMax),
+	angleIncrement_(angleIncrement),
+	localTransform_(localTransform)
+{
+	UASSERT(maxRange>minRange);
+	UASSERT(angleMax>angleMin);
+	UASSERT(angleIncrement != 0.0f);
+	maxPoints_ = std::ceil((angleMax - angleMin) / angleIncrement);
+
+	UASSERT(data.empty() || data.rows == 1);
+	UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6)  || data.type() == CV_32FC(7));
+	UASSERT(!localTransform.isNull());
+
+	if(!data.empty() && !isCompressed())
+	{
+		if(data_.cols > maxPoints_)
+		{
+			UWARN("The number of points (%d) in the scan is over the maximum "
+				  "points (%d) defined by angle settings (min=%f max=%f inc=%f). "
+				  "The scan info may be wrong!",
+				  data_.cols, maxPoints_, angleMin_, angleMax_, angleIncrement_);
+		}
+		if(format == kUnknown)
+		{
+			*this = backwardCompatibility(data_, rangeMin_, rangeMax_, angleMin_, angleMax_, angleIncrement_, localTransform_);
+		}
+		else // verify that format corresponds to expected number of channels
+		{
+			UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%d", format).c_str());
+			UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%d", format).c_str());
+			UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%d", format).c_str());
+			UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%d", format).c_str());
+			UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%d", format).c_str());
+			UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%d", format).c_str());
+		}
+	}
+}
+
+LaserScan LaserScan::clone() const
+{
+	if(angleIncrement_ > 0.0f)
+	{
+		return LaserScan(data_.clone(), format_, rangeMin_, rangeMax_, angleMin_, angleMax_, angleIncrement_, localTransform_.clone());
+	}
+	return LaserScan(data_.clone(), maxPoints_, rangeMax_, format_, localTransform_.clone());
+}
+
 }

corelib/src/Memory.cpp

@@ -2796,11 +2796,11 @@ Transform Memory::computeIcpTransformMulti(
 	{
 		Transform guess = poses.at(fromId).inverse() * poses.at(toId);
 		float guessNorm = guess.getNorm();
-		if(fromScan.maxRange() > 0.0f && toScan.maxRange() > 0.0f &&
+		if(fromScan.rangeMax() > 0.0f && toScan.rangeMax() > 0.0f &&
-			guessNorm > fromScan.maxRange() + toScan.maxRange())
+			guessNorm > fromScan.rangeMax() + toScan.rangeMax())
 		{
 			// stop right known,it is impossible that scans overlay.
-			UINFO("Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.maxRange(), toScan.maxRange());
+			UINFO("Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.rangeMax(), toScan.rangeMax());
 			return t;
 		}
 
@@ -2890,7 +2890,7 @@ Transform Memory::computeIcpTransformMulti(
 		assembledData.setLaserScanRaw(
 				LaserScan(assembledScan,
 					fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
-					fromScan.maxRange(),
+					fromScan.rangeMax(),
 					fromScan.format(),
 					fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
 
@@ -3430,7 +3430,8 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
 	double stamp;
 	std::vector<float> velocity;
 	GPS gps;
-	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
+	EnvSensors sensors;
+	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
 	return pose;
 }
 
@@ -3442,7 +3443,8 @@ Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
 	double stamp;
 	std::vector<float> velocity;
 	GPS gps;
-	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
+	EnvSensors sensors;
+	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
 	return groundTruth;
 }
 
@@ -3456,7 +3458,8 @@ void Memory::getGPS(int id, GPS & gps, Transform & offsetENU, bool lookInDatabas
 	std::string label;
 	double stamp;
 	std::vector<float> velocity;
-	getNodeInfo(id, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
+	EnvSensors sensors;
+	getNodeInfo(id, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
 
 	if(gps.stamp() == 0.0)
 	{
@@ -3502,6 +3505,7 @@ bool Memory::getNodeInfo(int signatureId,
 		Transform & groundTruth,
 		std::vector<float> & velocity,
 		GPS & gps,
+		EnvSensors & sensors,
 		bool lookInDatabase) const
 {
 	const Signature * s = this->getSignature(signatureId);
@@ -3515,11 +3519,12 @@ bool Memory::getNodeInfo(int signatureId,
 		groundTruth = s->getGroundTruthPose();
 		velocity = s->getVelocity();
 		gps = s->sensorData().gps();
+		sensors = s->sensorData().envSensors();
 		return true;
 	}
 	else if(lookInDatabase && _dbDriver)
 	{
-		return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps);
+		return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors);
 	}
 	return false;
 }
@@ -4451,7 +4456,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	LaserScan laserScan = data.laserScanRaw();
 	if(!isIntermediateNode && laserScan.size())
 	{
-		if(laserScan.maxRange() == 0.0f)
+		if(laserScan.rangeMax() == 0.0f)
 		{
 			bool id2d = laserScan.is2d();
 			float maxRange = 0.0f;
@@ -4561,7 +4566,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			data.groundTruth(),
 			stereoCameraModel.isValidForProjection()?
 				SensorData(
-						LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
+						laserScan.angleIncrement() == 0.0f?
+							LaserScan(compressedScan,
+								laserScan.maxPoints(),
+								laserScan.rangeMax(),
+								laserScan.format(),
+								laserScan.localTransform()):
+							LaserScan(compressedScan,
+								laserScan.format(),
+								laserScan.rangeMin(),
+								laserScan.rangeMax(),
+								laserScan.angleMin(),
+								laserScan.angleMax(),
+								laserScan.angleIncrement(),
+								laserScan.localTransform()),
 						compressedImage,
 						compressedDepth,
 						stereoCameraModel,
@@ -4569,7 +4587,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						0,
 						compressedUserData):
 				SensorData(
-						LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
+						laserScan.angleIncrement() == 0.0f?
+							LaserScan(compressedScan,
+								laserScan.maxPoints(),
+								laserScan.rangeMax(),
+								laserScan.format(),
+								laserScan.localTransform()):
+							LaserScan(compressedScan,
+								laserScan.format(),
+								laserScan.rangeMin(),
+								laserScan.rangeMax(),
+								laserScan.angleMin(),
+								laserScan.angleMax(),
+								laserScan.angleIncrement(),
+								laserScan.localTransform()),
 						compressedImage,
 						compressedDepth,
 						cameraModels,
@@ -4619,7 +4650,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			data.groundTruth(),
 			stereoCameraModel.isValidForProjection()?
 				SensorData(
-						LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
+						laserScan.angleIncrement() == 0.0f?
+								LaserScan(compressedScan,
+									laserScan.maxPoints(),
+									laserScan.rangeMax(),
+									laserScan.format(),
+									laserScan.localTransform()):
+								LaserScan(compressedScan,
+									laserScan.format(),
+									laserScan.rangeMin(),
+									laserScan.rangeMax(),
+									laserScan.angleMin(),
+									laserScan.angleMax(),
+									laserScan.angleIncrement(),
+									laserScan.localTransform()),
 						cv::Mat(),
 						cv::Mat(),
 						stereoCameraModel,
@@ -4627,7 +4671,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						0,
 						compressedUserData):
 				SensorData(
-						LaserScan(compressedScan, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform()),
+						laserScan.angleIncrement() == 0.0f?
+								LaserScan(compressedScan,
+									laserScan.maxPoints(),
+									laserScan.rangeMax(),
+									laserScan.format(),
+									laserScan.localTransform()):
+								LaserScan(compressedScan,
+									laserScan.format(),
+									laserScan.rangeMin(),
+									laserScan.rangeMax(),
+									laserScan.angleMin(),
+									laserScan.angleMax(),
+									laserScan.angleIncrement(),
+									laserScan.localTransform()),
 						cv::Mat(),
 						cv::Mat(),
 						cameraModels,
@@ -4648,6 +4705,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 
 	s->sensorData().setGroundTruth(data.groundTruth());
 	s->sensorData().setGPS(data.gps());
+	s->sensorData().setEnvSensors(data.envSensors());
 
 	t = timer.ticks();
 	if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);

corelib/src/OccupancyGrid.cpp

@@ -305,13 +305,13 @@ void OccupancyGrid::createLocalMap(
 		}
 
 		float maxRange = cloudMaxDepth_;
-		if(cloudMaxDepth_>0.0f && node.sensorData().laserScanRaw().maxRange()>0.0f)
+		if(cloudMaxDepth_>0.0f && node.sensorData().laserScanRaw().rangeMax()>0.0f)
 		{
-			maxRange = cloudMaxDepth_ < node.sensorData().laserScanRaw().maxRange()?cloudMaxDepth_:node.sensorData().laserScanRaw().maxRange();
+			maxRange = cloudMaxDepth_ < node.sensorData().laserScanRaw().rangeMax()?cloudMaxDepth_:node.sensorData().laserScanRaw().rangeMax();
 		}
-		else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().maxRange()>0.0f)
+		else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().rangeMax()>0.0f)
 		{
-			maxRange = node.sensorData().laserScanRaw().maxRange();
+			maxRange = node.sensorData().laserScanRaw().rangeMax();
 		}
 		util3d::occupancy2DFromLaserScan(
 				util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(),

corelib/src/RegistrationIcp.cpp

@@ -610,7 +610,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					{
 						// Load point clouds
 						DP data = laserScanToDP(fromScan);
-						DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess * toScan.localTransform()));
+						DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.rangeMax(), toScan.format(), guess * toScan.localTransform()));
 
 						// Compute the transformation to express data in ref
 						PM::TransformationParameters T;
@@ -786,7 +786,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 									LaserScan(
 											util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
 											maxLaserScansFrom,
-											fromScan.maxRange(),
+											fromScan.rangeMax(),
 											LaserScan::kXYNormal,
 											fromScan.localTransform()));
 						}
@@ -796,7 +796,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 									LaserScan(
 											util3d::laserScanFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
 											maxLaserScansFrom,
-											fromScan.maxRange(),
+											fromScan.rangeMax(),
 											LaserScan::kXYZNormal,
 											fromScan.localTransform()));
 						}
@@ -806,7 +806,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 									LaserScan(
 											util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
 											maxLaserScansTo,
-											toScan.maxRange(),
+											toScan.rangeMax(),
 											LaserScan::kXYNormal,
 											toScan.localTransform()));
 						}
@@ -816,7 +816,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 									LaserScan(
 											util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
 											maxLaserScansTo,
-											toScan.maxRange(),
+											toScan.rangeMax(),
 											LaserScan::kXYZNormal,
 											toScan.localTransform()));
 						}
@@ -833,7 +833,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 							{
 								// Load point clouds
 								DP data = laserScanToDP(fromScan);
-								DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess*toScan.localTransform()));
+								DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.rangeMax(), toScan.format(), guess*toScan.localTransform()));
 
 								// Compute the transformation to express data in ref
 								PM::TransformationParameters T;
@@ -905,7 +905,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 									LaserScan(
 											util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
 											maxLaserScansFrom,
-											fromScan.maxRange(),
+											fromScan.rangeMax(),
 											LaserScan::kXY,
 											fromScan.localTransform()));
 						}
@@ -915,7 +915,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 									LaserScan(
 											util3d::laserScanFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
 											maxLaserScansFrom,
-											fromScan.maxRange(),
+											fromScan.rangeMax(),
 											LaserScan::kXYZ,
 											fromScan.localTransform()));
 						}
@@ -925,7 +925,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 									LaserScan(
 											util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
 											maxLaserScansTo,
-											toScan.maxRange(),
+											toScan.rangeMax(),
 											LaserScan::kXY,
 											toScan.localTransform()));
 						}
@@ -935,7 +935,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 									LaserScan(
 											util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
 											maxLaserScansTo,
-											toScan.maxRange(),
+											toScan.rangeMax(),
 											LaserScan::kXYZ,
 											toScan.localTransform()));
 						}
@@ -948,7 +948,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					{
 						// Load point clouds
 						DP data = laserScanToDP(fromScan);
-						DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess*toScan.localTransform()));
+						DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.rangeMax(), toScan.format(), guess*toScan.localTransform()));
 
 						// Compute the transformation to express data in ref
 						PM::TransformationParameters T;

corelib/src/Rtabmap.cpp

@@ -824,7 +824,8 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
 				double stamp = 0.0;
 				std::vector<float> v;
 				GPS gps;
-				_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, gps, true);
+				EnvSensors sensors;
+				_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, gps, sensors, true);
 				stamps.insert(std::make_pair(iter->first, stamp));
 			}
 		}
@@ -2928,7 +2929,8 @@ bool Rtabmap::process(
 			Transform groundTruth;
 			std::vector<float> velocity;
 			GPS gps;
-			_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, false);
+			EnvSensors sensors;
+			_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, false);
 			signatures.insert(std::make_pair(iter->first,
 					Signature(iter->first,
 							mapId,
@@ -2942,6 +2944,7 @@ bool Rtabmap::process(
 				signatures.at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
 			}
 			signatures.at(iter->first).sensorData().setGPS(gps);
+			signatures.at(iter->first).sensorData().setEnvSensors(sensors);
 			if(_computeRMSE && !groundTruth.isNull())
 			{
 				groundTruths.insert(std::make_pair(iter->first, groundTruth));
@@ -3801,7 +3804,8 @@ void Rtabmap::get3DMap(
 			Transform groundTruth;
 			std::vector<float> velocity;
 			GPS gps;
-			_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, true);
+			EnvSensors sensors;
+			_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, true);
 			SensorData data = _memory->getNodeData(*iter);
 			data.setId(*iter);
 			std::multimap<int, cv::KeyPoint> words;
@@ -3825,6 +3829,7 @@ void Rtabmap::get3DMap(
 				signatures.at(*iter).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
 			}
 			signatures.at(*iter).sensorData().setGPS(gps);
+			signatures.at(*iter).sensorData().setEnvSensors(sensors);
 		}
 	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
@@ -3879,7 +3884,8 @@ void Rtabmap::getGraph(
 				Transform groundTruth;
 				std::vector<float> velocity;
 				GPS gps;
-				_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, global);
+				EnvSensors sensors;
+				_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, global);
 				signatures->insert(std::make_pair(iter->first,
 						Signature(iter->first,
 							mapId,
@@ -3908,6 +3914,7 @@ void Rtabmap::getGraph(
 					signatures->at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
 				}
 				signatures->at(iter->first).sensorData().setGPS(gps);
+				signatures->at(iter->first).sensorData().setEnvSensors(sensors);
 			}
 		}
 	}

corelib/src/SensorData.cpp

@@ -632,7 +632,14 @@ void SensorData::uncompressData(
 		_laserScanRaw = *laserScanRaw;
 		if(_laserScanCompressed.format() == LaserScan::kUnknown)
 		{
-			_laserScanCompressed = LaserScan(_laserScanCompressed.data(), _laserScanCompressed.maxPoints(), _laserScanCompressed.maxRange(), _laserScanRaw.format(), _laserScanCompressed.localTransform());
+			if(_laserScanCompressed.angleIncrement() > 0.0f)
+			{
+				_laserScanCompressed = LaserScan(_laserScanCompressed.data(), _laserScanRaw.format(), _laserScanCompressed.rangeMin(), _laserScanCompressed.rangeMax(), _laserScanCompressed.angleMin(), _laserScanCompressed.angleMax(), _laserScanCompressed.angleIncrement(), _laserScanCompressed.localTransform());
+			}
+			else
+			{
+				_laserScanCompressed = LaserScan(_laserScanCompressed.data(), _laserScanCompressed.maxPoints(), _laserScanCompressed.rangeMax(), _laserScanRaw.format(), _laserScanCompressed.localTransform());
+			}
 		}
 	}
 	if(userDataRaw && !userDataRaw->empty() && _userDataRaw.empty())
@@ -780,8 +787,14 @@ void SensorData::uncompressDataConst(
 		}
 		if(laserScanRaw && laserScanRaw->isEmpty())
 		{
-			*laserScanRaw = LaserScan(ctLaserScan.getUncompressedData(), _laserScanCompressed.maxPoints(), _laserScanCompressed.maxRange(), _laserScanCompressed.format(), _laserScanCompressed.localTransform());
+			if(_laserScanCompressed.angleIncrement() > 0.0f)
-
+			{
+				*laserScanRaw = LaserScan(ctLaserScan.getUncompressedData(), _laserScanCompressed.format(), _laserScanCompressed.rangeMin(), _laserScanCompressed.rangeMax(), _laserScanCompressed.angleMin(), _laserScanCompressed.angleMax(), _laserScanCompressed.angleIncrement(), _laserScanCompressed.localTransform());
+			}
+			else
+			{
+				*laserScanRaw = LaserScan(ctLaserScan.getUncompressedData(), _laserScanCompressed.maxPoints(), _laserScanCompressed.rangeMax(), _laserScanCompressed.format(), _laserScanCompressed.localTransform());
+			}
 			if(laserScanRaw->isEmpty())
 			{
 				if(_laserScanCompressed.isEmpty())

corelib/src/StereoCameraModel.cpp

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <rtabmap/core/StereoCameraModel.h>
+#include <rtabmap/core/Version.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UFile.h>
@@ -358,6 +359,141 @@ bool StereoCameraModel::saveStereoTransform(const std::string & directory) const
 	return false;
 }
 
+std::vector<unsigned char> StereoCameraModel::serialize() const
+{
+	std::vector<unsigned char> leftData = left_.serialize();
+	std::vector<unsigned char> rightData = right_.serialize();
+
+	const int headerSize = 10;
+	int header[headerSize] = {
+			RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
+			1, //stereo                                                          // 3
+			(int)R_.total(), (int)T_.total(), (int)E_.total(), (int)F_.total(),  // 4,5,6,7
+			(int)leftData.size(), (int)rightData.size()};                        // 8,9
+	UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9]);
+	std::vector<unsigned char> data(
+			sizeof(int)*headerSize +
+			sizeof(double)*(R_.total()+T_.total()+E_.total()+F_.total()) +
+			leftData.size() + rightData.size());
+	memcpy(data.data(), header, sizeof(int)*headerSize);
+	int index = sizeof(int)*headerSize;
+	if(!R_.empty())
+	{
+		memcpy(data.data()+index, R_.data, sizeof(double)*(R_.total()));
+		index+=sizeof(double)*(R_.total());
+	}
+	if(!T_.empty())
+	{
+		memcpy(data.data()+index, T_.data, sizeof(double)*(T_.total()));
+		index+=sizeof(double)*(T_.total());
+	}
+	if(!E_.empty())
+	{
+		memcpy(data.data()+index, E_.data, sizeof(double)*(E_.total()));
+		index+=sizeof(double)*(E_.total());
+	}
+	if(!F_.empty())
+	{
+		memcpy(data.data()+index, F_.data, sizeof(double)*(F_.total()));
+		index+=sizeof(double)*(F_.total());
+	}
+	if(leftData.size())
+	{
+		memcpy(data.data()+index, leftData.data(), leftData.size());
+		index+=leftData.size();
+	}
+	if(rightData.size())
+	{
+		memcpy(data.data()+index, rightData.data(), rightData.size());
+		index+=rightData.size();
+	}
+	return data;
+}
+
+unsigned int StereoCameraModel::deserialize(const std::vector<unsigned char>& data)
+{
+	return deserialize(data.data(), data.size());
+}
+unsigned int StereoCameraModel::deserialize(const unsigned char * data, unsigned int dataSize)
+{
+	*this = StereoCameraModel();
+	int headerSize = 10;
+	if(dataSize >= sizeof(int)*headerSize)
+	{
+		int iR = 4;
+		int iT = 5;
+		int iE = 6;
+		int iF = 7;
+		int iLeft = 8;
+		int iRight = 9;
+		const int * header = (const int *)data;
+		UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9]);
+		int type = header[3];
+		if(type==1)
+		{
+			unsigned int requiredDataSize = sizeof(int)*headerSize +
+					sizeof(double)*(header[iR]+header[iT]+header[iE]+header[iF]) +
+					header[iLeft] + header[iRight];
+			UASSERT_MSG(dataSize >= requiredDataSize,
+					uFormat("dataSize=%d != required=%d (header: version %d.%d.%d type=%d R=%d T=%d E=%d F=%d Left=%d Right=%d)",
+							dataSize,
+							requiredDataSize,
+							header[0], header[1], header[2], header[3],
+							header[iR], header[iT], header[iE],header[iF], header[iLeft], header[iRight]).c_str());
+
+			unsigned int index = sizeof(int)*headerSize;
+
+			if(header[iR] != 0)
+			{
+				UASSERT(header[iR] == 9);
+				R_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
+				index+=sizeof(double)*(R_.total());
+			}
+
+			if(header[iT] != 0)
+			{
+				UASSERT(header[iT] == 3);
+				T_ = cv::Mat(3, 1, CV_64FC1, (void*)(data+index)).clone();
+				index+=sizeof(double)*(T_.total());
+			}
+
+			if(header[iE] != 0)
+			{
+				UASSERT(header[iE] == 9);
+				E_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
+				index+=sizeof(double)*(E_.total());
+			}
+
+			if(header[iF] != 0)
+			{
+				UASSERT(header[iF] == 9);
+				F_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
+				index+=sizeof(double)*(F_.total());
+			}
+
+			if(header[iLeft] != 0)
+			{
+				index += left_.deserialize((data+index), header[iLeft]);
+			}
+
+			if(header[iRight] != 0)
+			{
+				index += right_.deserialize((data+index), header[iRight]);
+			}
+
+			UASSERT(index <= dataSize);
+
+			return index;
+		}
+		else
+		{
+			UERROR("Serialized calibration is not stereo (type=%d), use the appropriate class matching the type to deserialize.", type);
+		}
+	}
+	UERROR("Wrong serialized calibration data format detected (size in bytes=%d)! Cannot deserialize the data.", (int)dataSize);
+	return 0;
+}
+
 void StereoCameraModel::scale(double scale)
 {
 	left_ = left_.scaled(scale);

corelib/src/resources/DatabaseSchema.sql.in

@@ -23,7 +23,7 @@ CREATE TABLE Node (
 	velocity BLOB,            -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
 	label TEXT,
 	gps BLOB,                 -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
-	
+	env_sensors BLOB,         -- Variable 3xdouble: (sensorId1, value, stamp, sensorId2, value, stamp, ...)
 	time_enter DATE,
 	PRIMARY KEY (id)
 );
@@ -87,6 +87,15 @@ CREATE TABLE Feature (
 	FOREIGN KEY (node_id) REFERENCES Node(id)
 );
 
+--
+CREATE TABLE Tag (
+	node_id INTEGER NOT NULL,
+	tag_id INTEGER NOT NULL,
+	stamp FLOAT NOT NULL,
+	transform BLOB NOT NULL,     -- 3x4 float, /base_link -> /tag_frame
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
 CREATE TABLE Info (
 	STM_size INTEGER,
 	last_sign_added INTEGER,

corelib/src/util3d_filtering.cpp

@@ -130,7 +130,27 @@ LaserScan commonFiltering(
 			}
 			int previousSize = scan.size();
 			int scanMaxPtsTmp = scan.maxPoints();
-			scan = LaserScan(cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)), scanMaxPtsTmp/downsamplingStep, rangeMax>0.0f&&rangeMax<scan.maxRange()?rangeMax:scan.maxRange(), scan.format(), scan.localTransform());
+			if(scan.angleIncrement() > 0.0f)
+			{
+				scan = LaserScan(
+						cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)),
+						scan.format(),
+						rangeMin>0.0f&&rangeMin>scan.rangeMin()?rangeMin:scan.rangeMin(),
+						rangeMax>0.0f&&rangeMax<scan.rangeMax()?rangeMax:scan.rangeMax(),
+						scan.angleMin(),
+						scan.angleMax(),
+						scan.angleIncrement() * (float)downsamplingStep,
+						scan.localTransform());
+			}
+			else
+			{
+				scan = LaserScan(
+						cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)),
+						scanMaxPtsTmp/downsamplingStep,
+						rangeMax>0.0f&&rangeMax<scan.rangeMax()?rangeMax:scan.rangeMax(),
+						scan.format(),
+						scan.localTransform());
+			}
 			UDEBUG("Downsampling scan (step=%d): %d -> %d (scanMaxPts=%d->%d)", downsamplingStep, previousSize, scan.size(), scanMaxPtsTmp, scan.maxPoints());
 		}
 
@@ -154,16 +174,16 @@ LaserScan commonFiltering(
 					if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
 					{
 						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, normalK, normalRadius);
-						scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZRGBNormal, scan.localTransform());
+						scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYZRGBNormal, scan.localTransform());
 						UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
 					}
 					else
 					{
 						if(scan.hasNormals())
 						{
-							UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
+							UWARN("Voxel filter is applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
 						}
-						scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZRGB, scan.localTransform());
+						scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYZRGB, scan.localTransform());
 					}
 				}
 			}
@@ -186,12 +206,19 @@ LaserScan commonFiltering(
 						if(scan.is2d())
 						{
 							normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
-							scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYINormal, scan.localTransform());
+							if(voxelSize == 0.0f && scan.angleIncrement() > 0.0f)
+							{
+								scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYINormal, scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
+							}
+							else
+							{
+								scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYINormal, scan.localTransform());
+							}
 						}
 						else
 						{
 							normals = util3d::computeNormals(cloud, normalK, normalRadius);
-							scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZINormal, scan.localTransform());
+							scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYZINormal, scan.localTransform());
 						}
 						UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
 					}
@@ -203,11 +230,11 @@ LaserScan commonFiltering(
 						}
 						if(scan.is2d())
 						{
-							scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYI, scan.localTransform());
+							scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYI, scan.localTransform());
 						}
 						else
 						{
-							scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZI, scan.localTransform());
+							scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYZI, scan.localTransform());
 						}
 					}
 				}
@@ -231,12 +258,19 @@ LaserScan commonFiltering(
 						if(scan.is2d())
 						{
 							normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
-							scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYNormal, scan.localTransform());
+							if(voxelSize == 0.0f && scan.angleIncrement() > 0.0f)
+							{
+								scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYNormal, scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
+							}
+							else
+							{
+								scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYNormal, scan.localTransform());
+							}
 						}
 						else
 						{
 							normals = util3d::computeNormals(cloud, normalK, normalRadius);
-							scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.maxRange(), LaserScan::kXYZNormal, scan.localTransform());
+							scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYZNormal, scan.localTransform());
 						}
 						UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
 					}
@@ -248,11 +282,11 @@ LaserScan commonFiltering(
 						}
 						if(scan.is2d())
 						{
-							scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXY, scan.localTransform());
+							scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXY, scan.localTransform());
 						}
 						else
 						{
-							scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.maxRange(), LaserScan::kXYZ, scan.localTransform());
+							scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYZ, scan.localTransform());
 						}
 					}
 				}
@@ -307,7 +341,11 @@ LaserScan rangeFiltering(
 				cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
 				++oi;
 			}
-			return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0, oi)), scan.maxPoints(), scan.maxRange(), scan.format(), scan.localTransform());
+			if(scan.angleIncrement() > 0.0f)
+			{
+				return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0, oi)), scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
+			}
+			return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0, oi)), scan.maxPoints(), scan.rangeMax(), scan.format(), scan.localTransform());
 		}
 	}
 
@@ -334,7 +372,11 @@ LaserScan downsample(
 			cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
 			++oi;
 		}
-		return LaserScan(output, scan.maxPoints()/step, scan.maxRange(), scan.format(), scan.localTransform());
+		if(scan.angleIncrement() > 0.0f)
+		{
+			return LaserScan(output, scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement()*step, scan.localTransform());
+		}
+		return LaserScan(output, scan.maxPoints()/step, scan.rangeMax(), scan.format(), scan.localTransform());
 	}
 }
 template<typename PointT>

corelib/src/util3d_surface.cpp

@@ -2148,7 +2148,7 @@ LaserScan computeNormals(
 		{
 			UASSERT(!laserScan.is2d());
 			pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
-			return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
+			return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
 		}
 	}
 	else if(laserScan.hasIntensity())
@@ -2159,12 +2159,20 @@ LaserScan computeNormals(
 			if(laserScan.is2d())
 			{
 				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
-				return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
+				if(laserScan.angleIncrement() > 0.0f)
+				{
+					return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYINormal, laserScan.rangeMin(), laserScan.rangeMax(), laserScan.angleMin(), laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform());
+				}
+				else
+				{
+					return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYINormal, laserScan.localTransform());
+				}
+
 			}
 			else
 			{
 				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
-				return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
+				return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYZINormal, laserScan.localTransform());
 			}
 		}
 	}
@@ -2176,12 +2184,19 @@ LaserScan computeNormals(
 			if(laserScan.is2d())
 			{
 				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
-				return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
+				if(laserScan.angleIncrement() > 0.0f)
+				{
+					return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYNormal, laserScan.rangeMin(), laserScan.rangeMax(), laserScan.angleMin(), laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform());
+				}
+				else
+				{
+					return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYNormal, laserScan.localTransform());
+				}
 			}
 			else
 			{
 				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
-				return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.maxRange(), LaserScan::kXYZRGBNormal, laserScan.localTransform());
+				return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), LaserScan::kXYZNormal, laserScan.localTransform());
 			}
 		}
 	}
@@ -2854,7 +2869,14 @@ LaserScan adjustNormalsToViewPoint(
 				}
 			}
 		}
-		return LaserScan(output, scan.maxPoints(), scan.maxRange(), scan.format(), scan.localTransform());
+		if(scan.angleIncrement() > 0.0f)
+		{
+			return LaserScan(output, scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
+		}
+		else
+		{
+			return LaserScan(output, scan.maxPoints(), scan.rangeMax(), scan.format(), scan.localTransform());
+		}
 	}
 	return scan;
 }

corelib/src/util3d_transforms.cpp

@@ -83,7 +83,7 @@ LaserScan transformLaserScan(const LaserScan & laserScan, const Transform & tran
 			}
 		}
 	}
-	return LaserScan(output, laserScan.maxPoints(), laserScan.maxRange(), laserScan.format(), laserScan.localTransform());
+	return LaserScan(output, laserScan.maxPoints(), laserScan.rangeMax(), laserScan.format(), laserScan.localTransform());
 }
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -157,8 +157,10 @@ private:
 				QLabel * labelMapId,
 				QLabel * labelPose,
 				QLabel * labelVelocity,
-				QLabel * labeCalib,
+				QLabel * labelCalib,
+				QLabel * labelScan,
 				QLabel * labelGps,
+				QLabel * labelSensors,
 				bool updateConstraintView);
 	void updateStereo(const SensorData * data);
 	void updateWordsMatching();

guilib/include/rtabmap/gui/GraphViewer.h

@@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QMap>
 #include <QtCore/QSettings>
 #include <rtabmap/core/Link.h>
-#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/core/GPS.h>
 #include <opencv2/opencv.hpp>
 #include <map>
 #include <vector>

guilib/src/DatabaseViewer.cpp

@@ -1178,6 +1178,7 @@ void DatabaseViewer::exportDatabase()
 			std::map<int, double> stamps;
 			std::map<int, Transform> groundTruths;
 			std::map<int, GPS> gpsValues;
+			std::map<int, EnvSensors> sensorsValues;
 			for(int i=0; i<ids_.size(); i+=1+framesIgnored)
 			{
 				Transform odomPose, groundTruth;
@@ -1187,7 +1188,8 @@ void DatabaseViewer::exportDatabase()
 				double stamp = 0;
 				std::vector<float> velocity;
 				GPS gps;
-				if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
+				EnvSensors sensors;
+				if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
 				{
 					if(frameRate == 0 ||
 					   previousStamp == 0 ||
@@ -1211,6 +1213,10 @@ void DatabaseViewer::exportDatabase()
 							{
 								gpsValues.insert(std::make_pair(ids_[i], gps));
 							}
+							if(sensors.size())
+							{
+								sensorsValues.insert(std::make_pair(ids_[i], sensors));
+							}
 						}
 					}
 					if(sessionExported >= 0 && mapId > sessionExported)
@@ -1289,6 +1295,10 @@ void DatabaseViewer::exportDatabase()
 					{
 						sensorData.setGPS(gpsValues.at(id));
 					}
+					if(sensorsValues.find(id)!=sensorsValues.end())
+					{
+						sensorData.setEnvSensors(sensorsValues.at(id));
+					}
 
 					recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
 
@@ -1527,7 +1537,8 @@ void DatabaseViewer::updateIds()
 		int mapId;
 		std::vector<float> v;
 		GPS gps;
-		dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps);
+		EnvSensors sensors;
+		dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps, sensors);
 		mapIds_.insert(std::make_pair(ids_[i], mapId));
 		weights_.insert(std::make_pair(ids_[i], w));
 		if(wmStates.find(ids_[i]) != wmStates.end())
@@ -2059,7 +2070,8 @@ void DatabaseViewer::exportPoses(int format)
 				int mapId;
 				std::vector<float> v;
 				GPS gps;
-				dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps);
+				EnvSensors sensors;
+				dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
 				values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
 			}
 
@@ -2265,7 +2277,8 @@ void DatabaseViewer::exportPoses(int format)
 				int mapId;
 				std::vector<float> v;
 				GPS gps;
-				if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps))
+				EnvSensors sensors;
+				if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors))
 				{
 					stamps.insert(std::make_pair(iter->first, stamp));
 				}
@@ -2886,7 +2899,8 @@ void DatabaseViewer::regenerateLocalMaps()
 		QString msg;
 		std::vector<float> velocity;
 		GPS gps;
-		if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
+		EnvSensors sensors;
+		if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
 		{
 			Signature s = data;
 			s.setPose(odomPose);
@@ -3009,7 +3023,8 @@ void DatabaseViewer::regenerateCurrentLocalMaps()
 		QString msg;
 		std::vector<float> velocity;
 		GPS gps;
-		if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
+		EnvSensors sensors;
+		if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
 		{
 			Signature s = data;
 			s.setPose(odomPose);
@@ -3444,7 +3459,9 @@ void DatabaseViewer::sliderAValueChanged(int value)
 			ui_->label_poseA,
 			ui_->label_velA,
 			ui_->label_calibA,
+			ui_->label_scanA,
 			ui_->label_gpsA,
+			ui_->label_sensorsA,
 			true);
 }
 
@@ -3463,7 +3480,9 @@ void DatabaseViewer::sliderBValueChanged(int value)
 			ui_->label_poseB,
 			ui_->label_velB,
 			ui_->label_calibB,
+			ui_->label_scanB,
 			ui_->label_gpsB,
+			ui_->label_sensorsB,
 			true);
 }
 
@@ -3480,7 +3499,9 @@ void DatabaseViewer::update(int value,
 						QLabel * labelPose,
 						QLabel * labelVelocity,
 						QLabel * labelCalib,
+						QLabel * labelScan,
 						QLabel * labelGps,
+						QLabel * labelSensors,
 						bool updateConstraintView)
 {
 	UTimer timer;
@@ -3494,7 +3515,9 @@ void DatabaseViewer::update(int value,
 	labelVelocity->clear();
 	stamp->clear();
 	labelCalib->clear();
+	labelScan ->clear();
 	labelGps->clear();
+	labelSensors->clear();
 	QRectF rect;
 	if(value >= 0 && value < ids_.size())
 	{
@@ -3545,7 +3568,8 @@ void DatabaseViewer::update(int value,
 				double s;
 				std::vector<float> v;
 				GPS gps;
-				dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g, v, gps);
+				EnvSensors sensors;
+				dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g, v, gps, sensors);
 
 				weight->setNum(w);
 				label->setText(l.c_str());
@@ -3566,8 +3590,56 @@ void DatabaseViewer::update(int value,
 					labelGps->setText(QString("stamp=%1 longitude=%2 latitude=%3 altitude=%4m error=%5m bearing=%6deg").arg(QString::number(gps.stamp(), 'f')).arg(gps.longitude()).arg(gps.latitude()).arg(gps.altitude()).arg(gps.error()).arg(gps.bearing()));
 					labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps.stamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
 				}
+				if(sensors.size())
+				{
+					QString sensorsStr;
+					QString tooltipStr;
+					for(EnvSensors::iterator iter=sensors.begin(); iter!=sensors.end(); ++iter)
+					{
+						if(iter != sensors.begin())
+						{
+							sensorsStr += " | ";
+							tooltipStr += " | ";
+						}
+
+						if(iter->first == EnvSensor::kWifiSignalStrength)
+						{
+							sensorsStr += uFormat("%.1f dbm", iter->second.value()).c_str();
+							tooltipStr += "Wifi signal strength";
+						}
+						else if(iter->first == EnvSensor::kAmbientTemperature)
+						{
+							sensorsStr += uFormat("%.1f \u00B0C", iter->second.value()).c_str();
+							tooltipStr += "Ambient Temperature";
+						}
+						else if(iter->first == EnvSensor::kAmbientAirPressure)
+						{
+							sensorsStr += uFormat("%.1f hPa", iter->second.value()).c_str();
+							tooltipStr += "Ambient Air Pressure";
+						}
+						else if(iter->first == EnvSensor::kAmbientLight)
+						{
+							sensorsStr += uFormat("%.0f lx", iter->second.value()).c_str();
+							tooltipStr += "Ambient Light";
+						}
+						else if(iter->first == EnvSensor::kAmbientRelativeHumidity)
+						{
+							sensorsStr += uFormat("%.0f %%", iter->second.value()).c_str();
+							tooltipStr += "Ambient Relative Humidity";
+						}
+						else
+						{
+							sensorsStr += uFormat("%.2f", iter->second.value()).c_str();
+							tooltipStr += QString("Type %1").arg((int)iter->first);
+						}
+
+					}
+					labelSensors->setText(sensorsStr);
+					labelSensors->setToolTip(tooltipStr);
+				}
 				if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())
 				{
+					std::stringstream calibrationDetails;
 					if(data.cameraModels().size())
 					{
 						if(!data.depthRaw().empty() && data.depthRaw().cols!=data.imageRaw().cols && data.imageRaw().cols)
@@ -3596,6 +3668,17 @@ void DatabaseViewer::update(int value,
 									.arg(data.cameraModels()[0].cy())
 									.arg(data.cameraModels()[0].localTransform().prettyPrint().c_str()));
 						}
+
+						for(unsigned int i=0; i<data.cameraModels().size();++i)
+						{
+							if(i!=0) calibrationDetails << std::endl;
+							calibrationDetails << "Id: " << i << " Size=" << data.cameraModels()[i].imageWidth() << "x" << data.cameraModels()[i].imageWidth() << std::endl;
+							if( data.cameraModels()[i].K_raw().total()) calibrationDetails << "K=" << data.cameraModels()[i].K_raw() << std::endl;
+							if( data.cameraModels()[i].D_raw().total()) calibrationDetails << "D=" << data.cameraModels()[i].D_raw() << std::endl;
+							if( data.cameraModels()[i].R().total()) calibrationDetails << "R=" << data.cameraModels()[i].R() << std::endl;
+							if( data.cameraModels()[i].P().total()) calibrationDetails << "P=" << data.cameraModels()[i].P() << std::endl;
+						}
+
 					}
 					else
 					{
@@ -3609,7 +3692,25 @@ void DatabaseViewer::update(int value,
 									.arg(data.stereoCameraModel().left().cy())
 									.arg(data.stereoCameraModel().baseline())
 									.arg(data.stereoCameraModel().localTransform().prettyPrint().c_str()));
+
+						calibrationDetails << "Left:" << " Size=" << data.stereoCameraModel().left().imageWidth() << "x" << data.stereoCameraModel().left().imageWidth() << std::endl;
+						if( data.stereoCameraModel().left().K_raw().total()) calibrationDetails << "K=" << data.stereoCameraModel().left().K_raw() << std::endl;
+						if( data.stereoCameraModel().left().D_raw().total()) calibrationDetails << "D=" << data.stereoCameraModel().left().D_raw() << std::endl;
+						if( data.stereoCameraModel().left().R().total()) calibrationDetails << "R=" << data.stereoCameraModel().left().R() << std::endl;
+						if( data.stereoCameraModel().left().P().total()) calibrationDetails << "P=" << data.stereoCameraModel().left().P() << std::endl;
+						calibrationDetails << std::endl;
+						calibrationDetails << "Right:" << " Size=" << data.stereoCameraModel().right().imageWidth() << "x" << data.stereoCameraModel().right().imageWidth() << std::endl;
+						if( data.stereoCameraModel().right().K_raw().total()) calibrationDetails << "K=" << data.stereoCameraModel().right().K_raw() << std::endl;
+						if( data.stereoCameraModel().right().D_raw().total()) calibrationDetails << "D=" << data.stereoCameraModel().right().D_raw() << std::endl;
+						if( data.stereoCameraModel().right().R().total()) calibrationDetails << "R=" << data.stereoCameraModel().right().R() << std::endl;
+						if( data.stereoCameraModel().right().P().total()) calibrationDetails << "P=" << data.stereoCameraModel().right().P() << std::endl;
+						calibrationDetails << std::endl;
+						if( data.stereoCameraModel().R().total()) calibrationDetails << "R=" << data.stereoCameraModel().R() << std::endl;
+						if( data.stereoCameraModel().T().total()) calibrationDetails << "T=" << data.stereoCameraModel().T() << std::endl;
+						if( data.stereoCameraModel().F().total()) calibrationDetails << "F=" << data.stereoCameraModel().F() << std::endl;
+						if( data.stereoCameraModel().E().total()) calibrationDetails << "E=" << data.stereoCameraModel().E() << std::endl;
 					}
+					labelCalib->setToolTip(calibrationDetails.str().c_str());
 
 				}
 				else
@@ -3617,6 +3718,23 @@ void DatabaseViewer::update(int value,
 					labelCalib->setText("NA");
 				}
 
+				if(data.laserScanRaw().size())
+				{
+					labelScan->setText(tr("Format=%1 Points=%2 [max=%3] Range=[%4->%5 m] Angle=[%6->%7 rad inc=%8] Has [Color=%9 2D=%10 Normals=%11 Intensity=%12]")
+							.arg(data.laserScanRaw().format())
+							.arg(data.laserScanRaw().size())
+							.arg(data.laserScanRaw().maxPoints())
+							.arg(data.laserScanRaw().rangeMin())
+							.arg(data.laserScanRaw().rangeMax())
+							.arg(data.laserScanRaw().angleMin())
+							.arg(data.laserScanRaw().angleMax())
+							.arg(data.laserScanRaw().angleIncrement())
+							.arg(data.laserScanRaw().hasRGB()?1:0)
+							.arg(data.laserScanRaw().is2d()?1:0)
+							.arg(data.laserScanRaw().hasNormals()?1:0)
+							.arg(data.laserScanRaw().hasIntensity()?1:0));
+				}
+
 				//stereo
 				if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
 				{
@@ -4576,7 +4694,9 @@ void DatabaseViewer::updateConstraintView(
 					ui_->label_poseA,
 					ui_->label_velA,
 					ui_->label_calibA,
+					ui_->label_scanA,
 					ui_->label_gpsA,
+					ui_->label_sensorsA,
 					false); // don't update constraints view!
 		this->update(idToIndex_.value(link.to()),
 					ui_->label_indexB,
@@ -4591,7 +4711,9 @@ void DatabaseViewer::updateConstraintView(
 					ui_->label_poseB,
 					ui_->label_velB,
 					ui_->label_calibB,
+					ui_->label_scanB,
 					ui_->label_gpsB,
+					ui_->label_sensorsB,
 					false); // don't update constraints view!
 	}
 
@@ -4633,7 +4755,8 @@ void DatabaseViewer::updateConstraintView(
 			Transform p,g;
 			std::vector<float> v;
 			GPS gps;
-			dbDriver_->getNodeInfo(link.from(), p, m, w, l, s, g, v, gps);
+			EnvSensors sensors;
+			dbDriver_->getNodeInfo(link.from(), p, m, w, l, s, g, v, gps, sensors);
 			if(!p.isNull())
 			{
 				// keep just the z and roll/pitch rotation
@@ -6054,7 +6177,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 		assembledData.setLaserScanRaw(LaserScan(
 				assembledScan,
 				fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
-				fromScan.maxRange(),
+				fromScan.rangeMax(),
 				fromScan.format(),
 				fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
 

guilib/src/ExportCloudsDialog.cpp

@@ -2575,7 +2575,8 @@ bool ExportCloudsDialog::getExportedClouds(
 										std::string l;
 										double s;
 										GPS gps;
-										_dbDriver->getNodeInfo(jter->first, p, m, w, l, s, gt, velocity, gps);
+										EnvSensors sensors;
+										_dbDriver->getNodeInfo(jter->first, p, m, w, l, s, gt, velocity, gps, sensors);
 									}
 								}
 								cv::Size imageSize = img.size();
@@ -2608,7 +2609,8 @@ bool ExportCloudsDialog::getExportedClouds(
 										std::string l;
 										double s;
 										GPS gps;
-										_dbDriver->getNodeInfo(jter->first, p, m, w, l, s, gt, velocity, gps);
+										EnvSensors sensors;
+										_dbDriver->getNodeInfo(jter->first, p, m, w, l, s, gt, velocity, gps, sensors);
 									}
 								}
 							}

guilib/src/MainWindow.cpp

@@ -2004,7 +2004,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 					LaserScan(
 							cv::Mat(),
 							signature.sensorData().laserScanRaw().maxPoints(),
-							signature.sensorData().laserScanRaw().maxRange(),
+							signature.sensorData().laserScanRaw().rangeMax(),
 							signature.sensorData().laserScanRaw().format(),
 							signature.sensorData().laserScanRaw().localTransform()));
 			s.sensorData().clearOccupancyGridRaw();
@@ -3359,7 +3359,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 			added = _cloudViewer->addCloud(scanName, cloudRGBWithNormals, pose, color);
 			if(added && nodeId > 0)
 			{
-				scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGBWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZRGBNormal, scan.localTransform());
+				scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGBWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZRGBNormal, scan.localTransform());
 			}
 		}
 		else if(cloudIWithNormals.get())
@@ -3369,11 +3369,11 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 			{
 				if(scan.is2d())
 				{
-					scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYINormal, scan.localTransform());
+					scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYINormal, scan.localTransform());
 				}
 				else
 				{
-					scan = LaserScan(util3d::laserScanFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZINormal, scan.localTransform());
+					scan = LaserScan(util3d::laserScanFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZINormal, scan.localTransform());
 				}
 			}
 		}
@@ -3384,11 +3384,11 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 			{
 				if(scan.is2d())
 				{
-					scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYNormal, scan.localTransform());
+					scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYNormal, scan.localTransform());
 				}
 				else
 				{
-					scan = LaserScan(util3d::laserScanFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZNormal, scan.localTransform());
+					scan = LaserScan(util3d::laserScanFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZNormal, scan.localTransform());
 				}
 			}
 		}
@@ -3397,7 +3397,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 			added = _cloudViewer->addCloud(scanName, cloudRGB, pose, color);
 			if(added && nodeId > 0)
 			{
-				scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGB, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZRGB, scan.localTransform());
+				scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGB, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZRGB, scan.localTransform());
 			}
 		}
 		else if(cloudI.get())
@@ -3407,11 +3407,11 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 			{
 				if(scan.is2d())
 				{
-					scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYI, scan.localTransform());
+					scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYI, scan.localTransform());
 				}
 				else
 				{
-					scan = LaserScan(util3d::laserScanFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZI, scan.localTransform());
+					scan = LaserScan(util3d::laserScanFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZI, scan.localTransform());
 				}
 			}
 		}
@@ -3423,11 +3423,11 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 			{
 				if(scan.is2d())
 				{
-					scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXY, scan.localTransform());
+					scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXY, scan.localTransform());
 				}
 				else
 				{
-					scan = LaserScan(util3d::laserScanFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZ, scan.localTransform());
+					scan = LaserScan(util3d::laserScanFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.rangeMax(), LaserScan::kXYZ, scan.localTransform());
 				}
 			}
 		}

guilib/src/ui/DatabaseViewer.ui

@@ -61,8 +61,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>301</width>
+           <width>287</width>
-           <height>242</height>
+           <height>288</height>
           </rect>
          </property>
          <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -202,14 +202,14 @@
             </property>
            </widget>
           </item>
-          <item row="9" column="0">
+          <item row="10" column="0">
            <widget class="QLabel" name="label_childrenA_16">
             <property name="text">
              <string>GPS</string>
             </property>
            </widget>
           </item>
-          <item row="9" column="1">
+          <item row="10" column="1">
            <widget class="QLabel" name="label_gpsA">
             <property name="text">
              <string/>
@@ -236,6 +236,40 @@
             </property>
            </widget>
           </item>
+          <item row="11" column="0">
+           <widget class="QLabel" name="label_childrenA_20">
+            <property name="text">
+             <string>Sensors</string>
+            </property>
+           </widget>
+          </item>
+          <item row="11" column="1">
+           <widget class="QLabel" name="label_sensorsA">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="0">
+           <widget class="QLabel" name="label_childrenA_22">
+            <property name="text">
+             <string>Scan</string>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="1">
+           <widget class="QLabel" name="label_scanA">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
          </layout>
         </widget>
        </widget>
@@ -253,8 +287,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>301</width>
+           <width>287</width>
-           <height>242</height>
+           <height>288</height>
           </rect>
          </property>
          <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -302,14 +336,14 @@
             </property>
            </widget>
           </item>
-          <item row="9" column="0">
+          <item row="10" column="0">
            <widget class="QLabel" name="label_childrenA_17">
             <property name="text">
              <string>GPS</string>
             </property>
            </widget>
           </item>
-          <item row="9" column="1">
+          <item row="10" column="1">
            <widget class="QLabel" name="label_gpsB">
             <property name="text">
              <string/>
@@ -428,6 +462,40 @@
             </property>
            </widget>
           </item>
+          <item row="11" column="0">
+           <widget class="QLabel" name="label_childrenA_21">
+            <property name="text">
+             <string>Sensors</string>
+            </property>
+           </widget>
+          </item>
+          <item row="11" column="1">
+           <widget class="QLabel" name="label_sensorsB">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="0">
+           <widget class="QLabel" name="label_childrenA_23">
+            <property name="text">
+             <string>Scan</string>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="1">
+           <widget class="QLabel" name="label_scanB">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="textInteractionFlags">
+             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+            </property>
+           </widget>
+          </item>
          </layout>
         </widget>
        </widget>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.17.7</version>
+  <version>0.18.0</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

tools/EurocDataset/main.cpp

@@ -607,7 +607,8 @@ int main(int argc, char * argv[])
 				double s;
 				std::vector<float> v;
 				GPS gps;
-				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, true);
+				EnvSensors sensors;
+				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, true);
 				if(!gtPose.isNull())
 				{
 					groundTruth.insert(std::make_pair(iter->first, gtPose));

tools/KittiDataset/main.cpp

@@ -589,7 +589,8 @@ int main(int argc, char * argv[])
 				double s;
 				std::vector<float> v;
 				GPS gps;
-				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, true);
+				EnvSensors sensors;
+				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, true);
 				if(!gtPose.isNull())
 				{
 					groundTruth.insert(std::make_pair(iter->first, gtPose));

tools/Report/main.cpp

@@ -202,7 +202,8 @@ int main(int argc, char * argv[])
 						std::string l;
 						double s;
 						std::vector<float> v;
-						if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps))
+						EnvSensors sensors;
+						if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps, sensors))
 						{
 							odomPoses.insert(std::make_pair(*iter, p));
 							if(!gt.isNull())

tools/RgbdDataset/main.cpp

@@ -395,7 +395,8 @@ int main(int argc, char * argv[])
 				double s;
 				std::vector<float> v;
 				GPS gps;
-				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, true);
+				EnvSensors sensors;
+				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, true);
 				if(!gtPose.isNull())
 				{
 					groundTruth.insert(std::make_pair(iter->first, gtPose));