DBReader: set calibrated true when only scans in db. RegIcp: added intensity matching option when complexity is low. OdomF2M: accept first key frame on low complexity if a guess is provided. DbViewer: added gravity visualization in 3D view.

corelib/src/DBReader.cpp

@@ -200,6 +200,18 @@ bool DBReader::init(
 			{
 				_calibrated = true;
 			}
+			else
+			{
+				Signature * s = _dbDriver->loadSignature(*_ids.begin());
+				_dbDriver->loadNodeData(s);
+				if( s->sensorData().imageCompressed().empty() &&
+					s->getWords().empty() &&
+					!s->sensorData().laserScanCompressed().empty())
+				{
+					_calibrated = true; // only scans
+				}
+				delete s;
+			}
 		}
 	}
 	else

corelib/src/RegistrationIcp.cpp

@@ -1209,11 +1209,18 @@ Transform RegistrationIcp::computeTransformationImpl(
 								UWARN("libpointmatcher icp...temporary maxDist=%s (%s=%f, %s=%f)", params["maxDist"].c_str(),  Parameters::kIcpMaxCorrespondenceDistance().c_str(), _maxCorrespondenceDistance, Parameters::kIcpVoxelSize().c_str(), _voxelSize);
 								params["knn"] = uNumber2Str(_libpointmatcherKnn);
 								params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
+								if(_libpointmatcherIntensity)
+								{
+									icpTmp.matcher.reset(new KDTreeMatcherIntensity<float>(params));
+								}
+								else
+								{
 #if POINTMATCHER_VERSION_INT >= 10300
-								icpTmp.matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
+									icpTmp.matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
 #else
-								icpTmp.matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
+									icpTmp.matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
 #endif
+								}
 
 #if POINTMATCHER_VERSION_INT >= 10300
 								icpTmp.errorMinimizer = PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer");

corelib/src/odometry/OdometryF2M.cpp

@@ -1322,6 +1322,16 @@ Transform OdometryF2M::computeTransform(
 								{
 									frameValid = true;
 								}
+								else if(!guess.isNull() && !guess.isIdentity())
+								{
+									UWARN("Scan complexity too low (%f) to init robustly the first "
+											"keyframe. Make sure the lidar is seeing enough "
+											"geometry in all axes for good initialization. "
+											"Accepting as an initial guess (%s) is provided.",
+											complexity,
+											guess.prettyPrint().c_str());
+									frameValid = true;
+								}
 							}
 							else
 							{

guilib/src/DatabaseViewer.cpp

@@ -345,6 +345,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_showMap, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
 	connect(ui_->checkBox_showGrid, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
 	connect(ui_->checkBox_odomFrame_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
+	connect(ui_->checkBox_gravity_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
 
 	ui_->horizontalSlider_neighbors->setTracking(false);
 	ui_->horizontalSlider_loops->setTracking(false);
@@ -4449,14 +4450,7 @@ void DatabaseViewer::update(int value,
 				}
 
 				// 3d view
-				if(cloudViewer_->isVisible())
+				if(cloudViewer_->isVisible())
-				{
-					Transform pose = Transform::getIdentity();
-					if(signatures.size() && ui_->checkBox_odomFrame_3dview->isChecked())
-					{
-						float x, y, z, roll, pitch, yaw;
-						(*signatures.begin())->getPose().getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
-						pose = Transform(0,0,z,roll,pitch,0);
 				{
 					cloudViewer_->removeAllLines();
 					cloudViewer_->removeAllFrustums();
@@ -4467,6 +4461,27 @@ void DatabaseViewer::update(int value,
 					cloudViewer_->removeCloud("ground");
 					cloudViewer_->removeCloud("obstacles");
 					cloudViewer_->removeCloud("empty_cells");
+					cloudViewer_->removeCloud("words");
+					cloudViewer_->removeOctomap();
+
+					Transform pose = Transform::getIdentity();
+					if(signatures.size() && ui_->checkBox_odomFrame_3dview->isChecked())
+					{
+						float x, y, z, roll, pitch, yaw;
+						(*signatures.begin())->getPose().getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+						pose = Transform(0,0,z,roll,pitch,0);
+					}
+
+					if(!gravityLink.empty() && ui_->checkBox_gravity_3dview->isChecked())
+					{
+						Transform gravityT = gravityLink.begin()->second.transform();
+						Eigen::Vector3f gravity(0,0,-1);
+						if(pose.isIdentity())
+						{
+							gravityT = gravityT.inverse();
+						}
+						gravity = (gravityT.rotation()*(pose).rotation().inverse()).toEigen3f()*gravity;
+						cloudViewer_->addOrUpdateLine("gravity", pose, (pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, false);
 					}
 
 					if(ui_->checkBox_showCloud->isChecked() || ui_->checkBox_showMesh->isChecked())
@@ -4811,6 +4826,8 @@ void DatabaseViewer::update(int value,
 								delete octomap;
 							}
 #endif
+						}
+					}
 					cloudViewer_->updateCameraTargetPosition(pose);
 					cloudViewer_->clearTrajectory();
 					cloudViewer_->update();

guilib/src/ui/DatabaseViewer.ui

@@ -61,7 +61,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>414</width>
+           <width>419</width>
            <height>311</height>
           </rect>
          </property>
@@ -304,7 +304,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>413</width>
+           <width>418</width>
            <height>311</height>
           </rect>
          </property>
@@ -1387,7 +1387,7 @@
           <x>0</x>
           <y>0</y>
           <width>318</width>
-          <height>226</height>
+          <height>188</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1542,8 +1542,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>519</width>
+          <width>280</width>
-          <height>831</height>
+          <height>949</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2096,8 +2096,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>312</width>
+          <width>226</width>
-          <height>226</height>
+          <height>160</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2224,8 +2224,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>446</width>
+          <width>185</width>
-          <height>226</height>
+          <height>485</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2540,6 +2540,16 @@
          </property>
         </widget>
        </item>
+       <item>
+        <widget class="QCheckBox" name="checkBox_gravity_3dview">
+         <property name="text">
+          <string>Gravity</string>
+         </property>
+         <property name="checked">
+          <bool>true</bool>
+         </property>
+        </widget>
+       </item>
        <item>
         <spacer name="horizontalSpacer_4">
          <property name="orientation">