Viewers can take global configuration parameters (ellipses scale, siz…

…es...). Added 3D display ! Added cpp files for displays. Make optional modules really optional (one consequence is that configure is not supported anymore).
TingruW · Jul 6, 2010 · b000d4d · b000d4d
1 parent bc4a2aa
commit b000d4d
Show file tree

Hide file tree

Showing 17 changed files with 1,110 additions and 512 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -10,11 +10,11 @@ build_jafar_module(rtslam
   REVISION 1 
 
   REQUIRED_MODULES kernel image jmath correl
-  OPTIONAL_MODULES qdisplay
+  OPTIONAL_MODULES qdisplay gdhe
 
   REQUIRED_EXTLIBS boost_thread boost_system boost_filesystem boost_regex
   OPTIONAL_EXTLIBS qt4 viam MTI
 
 #  CXXFLAGS -g -ggdb -O0 -Wall
-  CXXFLAGS -O2 -g -DJFR_NDEBUG -DNDEBUG -DBOOST_UBLAS_NDEBUG -Wall -pthread
+#  CXXFLAGS -O2 -g -DJFR_NDEBUG -DNDEBUG -DBOOST_UBLAS_NDEBUG -Wall -pthread
 )
diff --git a/TODO b/TODO
@@ -8,9 +8,6 @@ Features
 
 - estimation hardware <- IN PROGRESS FOR MTI
 
-- Map management:
-	- Lmk deletion :
-		- non-reparametrized die when out of view in initializer sensor. 
 
 Organization
 ------------------------------------
@@ -32,13 +29,14 @@ Performance
 	7. try to copy the search area in another image for a better use of cache
 	8. try to use cpu vectorialization (compute 4 zncc at once)
 - check algorithm speed <- Good 60Hz. See issues with RT and OS -> reboot and stop the most possible programs and services
-- find a more clever way to manipulate the map used space: ia_indirectArray() is possibly an expensive way to go.
+- find a more clever way to manipulate the map used space: ia_indirectArray() is possibly an expensive way to go, or maybe use eigen2 instead of ublas...
 
 
 
 Precision
 ------------------------------------
-- Implement one point Ransac to avoid outliers !!!
+- Finish implementing 1 point Ransac to avoid outliers
+- Finish implementing RobotCentric Kalman
 - test predicted patch variance: if too low, discard exploration and declare not matched.
 - when a point is detected, do a matching with itself with interpolation to find subpixel position
 - when a point is matched, try to estimate if there is a possible ambiguity with another point in the ellipse 
@@ -48,5 +46,6 @@ Display
 
 - 3D display with GDHE
 - Allow configurations of viewers: font size, dump, ...
+- Allow exhaustive rendering when offline, for videos
 - Catch keyboard and mouse events in the windows, in order to allow to play/pause/next...
 - Implement double color labels for better visibility on light and dark backgrounds
diff --git a/demo_suite/demo_slam.cpp b/demo_suite/demo_slam.cpp
@@ -47,6 +47,7 @@
 //#include "rtslam/hardwareEstimatorMti.hpp"
 
 #include "rtslam/display_qt.hpp"
+#include "rtslam/display_gdhe.hpp"
 
 using namespace jblas;
 using namespace jafar;
@@ -60,10 +61,16 @@ typedef ImagePointObservationMaker<ObservationPinHoleEuclideanPoint, SensorPinHo
 typedef ImagePointObservationMaker<ObservationPinHoleAnchoredHomogeneousPoint, SensorPinHole,
     LandmarkAnchoredHomogeneousPoint, SensorAbstract::PINHOLE, LandmarkAbstract::PNT_AH> PinholeAhpObservationMaker;
 
+int dodisplay = 1;
 int mode = 0;
 std::string dump_path = ".";
 time_t rseed;
 
+const int slam_priority = -20; // needs to be started as root to be < 0
+const int display_priority = 10;
+const int display_period = 100; // ms
+
+
 void demo_slam01_main(world_ptr_t *world) {
 	std::cout << rand() << "," << rand() << "," << rand() << "," << rand() << "," << rand() << std::endl;
 	// time
@@ -157,7 +164,8 @@ void demo_slam01_main(world_ptr_t *world) {
 	senPtr11->setId();
 	senPtr11->linkToParentRobot(robPtr1);
 	senPtr11->state.clear();
-	senPtr11->pose.x(quaternion::originFrame());
+	senPtr11->setPose(0,0,0,-90,0,-90);
+	//senPtr11->pose.x(quaternion::originFrame());
 	senPtr11->params.setImgSize(IMG_WIDTH, IMG_HEIGHT);
 	senPtr11->params.setIntrinsicCalibration(intrinsic, distortion, distortion.size());
 	senPtr11->params.setMiscellaneous(1.0, 0.1);
@@ -280,6 +288,7 @@ void demo_slam01_main(world_ptr_t *world) {
 
 		worldPtr->display_mutex.unlock();
 		if (mode == 2 && had_data) getchar(); // wait for key in replay mode
+//std::cout << "one frame " << (*world)->t << " : " << mode << " " << had_data << std::endl;
 	} // temporal loop
 
 	std::cout << "\nFINISHED ! Press a key to terminate." << std::endl;
@@ -288,28 +297,74 @@ void demo_slam01_main(world_ptr_t *world) {
 
 
 void demo_slam01_display(world_ptr_t *world) {
-	//(*world)->display_mutex.lock();
 	static unsigned prev_t = 0;
-	qdisplay::qtMutexLock<kernel::FifoMutex>((*world)->display_mutex);
-	if ((*world)->t != prev_t)
+	kernel::Timer timer(display_period*1000);
+	while(true)
 	{
-		prev_t = (*world)->t;
-		display::ViewerQt *viewerQt = static_cast<display::ViewerQt*> ((*world)->getDisplayViewer(display::ViewerQt::id()));
-		if (viewerQt == NULL) {
-			viewerQt = new display::ViewerQt();
-			(*world)->addDisplayViewer(viewerQt, display::ViewerQt::id());
+		if (dodisplay & 1)
+		{
+			#ifdef HAVE_MODULE_QDISPLAY
+			qdisplay::qtMutexLock<kernel::FifoMutex>((*world)->display_mutex);
+			#endif
+		}
+		else
+		{
+			(*world)->display_mutex.lock();
+		}
+
+		if ((*world)->t != prev_t)
+		{
+			prev_t = (*world)->t;
+			#ifdef HAVE_MODULE_QDISPLAY
+			display::ViewerQt *viewerQt = NULL;
+			if (dodisplay & 1)
+			{
+				viewerQt = PTR_CAST<display::ViewerQt*> ((*world)->getDisplayViewer(display::ViewerQt::id()));
+				if (viewerQt == NULL) {
+					viewerQt = new display::ViewerQt(8, 3.0, false, "/mnt/ram/rtslam");
+					(*world)->addDisplayViewer(viewerQt, display::ViewerQt::id());
+				}
+			}
+			#endif
+			#ifdef HAVE_MODULE_GDHE
+			display::ViewerGdhe *viewerGdhe = NULL;
+			if (dodisplay & 2)
+			{
+				viewerGdhe = PTR_CAST<display::ViewerGdhe*> ((*world)->getDisplayViewer(display::ViewerGdhe::id()));
+				if (viewerGdhe == NULL) {
+					viewerGdhe = new display::ViewerGdhe("camera", 3.0, "localhost");
+					(*world)->addDisplayViewer(viewerGdhe, display::ViewerGdhe::id());
+				}
+			}
+			#endif
+
+			#ifdef HAVE_MODULE_QDISPLAY
+			if (dodisplay & 1) viewerQt->bufferize(*world);
+			#endif
+			#ifdef HAVE_MODULE_GDHE
+			if (dodisplay & 2) viewerGdhe->bufferize(*world);
+			#endif
+
+			(*world)->display_mutex.unlock();
+
+			#ifdef HAVE_MODULE_QDISPLAY
+			if (dodisplay & 1) viewerQt->render();
+			#endif
+			#ifdef HAVE_MODULE_GDHE
+			if (dodisplay & 2) viewerGdhe->render();
+			#endif
+
+		} else
+		{
+			(*world)->display_mutex.unlock();
 		}
-		viewerQt->bufferize(*world);
-		(*world)->display_mutex.unlock();
 
-		viewerQt->render();
-	} else
-	{
-		(*world)->display_mutex.unlock();
+		if (dodisplay & 1) break;
+		              else timer.wait();
 	}
 }
 
-	void demo_slam01(bool display) {
+	void demo_slam01() {
 		world_ptr_t worldPtr(new WorldAbstract());
 
 		rseed = time(NULL);
@@ -328,14 +383,25 @@ void demo_slam01_display(world_ptr_t *world) {
 
 
 		// to start with qt display
-		const int slam_priority = -20; // needs to be started as root to be < 0
-		const int display_priority = 10;
-		const int display_period = 100; // ms
-		if (display)
+		if (dodisplay & 1) // at least 2d
 		{
+			#ifdef HAVE_MODULE_QDISPLAY
 			qdisplay::QtAppStart((qdisplay::FUNC)&demo_slam01_display,display_priority,(qdisplay::FUNC)&demo_slam01_main,slam_priority,display_period,&worldPtr);
-		}
-		else
+			#else
+			std::cout << "Please install qdisplay module if you want 2D display" << std::endl;
+			#endif
+		} else
+		if (dodisplay & 2) // only 3d
+		{
+			#ifdef HAVE_MODULE_GDHE
+			kernel::setCurrentThreadPriority(display_priority);
+			boost::thread *thread_disp = new boost::thread(boost::bind(demo_slam01_display,&worldPtr));
+			kernel::setCurrentThreadPriority(slam_priority);
+			demo_slam01_main(&worldPtr);
+			#else
+			std::cout << "Please install gdhe module if you want 3D display" << std::endl;
+			#endif
+		} else // none
 		{
 			kernel::setCurrentThreadPriority(slam_priority);
 			demo_slam01_main(&worldPtr);
@@ -360,15 +426,14 @@ void demo_slam01_display(world_ptr_t *world) {
 		 */
 		int main(int argc, const char* argv[])
 		{
-			bool display = 1;
 			if (argc == 4)
 			{
-				display = atoi(argv[1]);
+				dodisplay = atoi(argv[1]);
 				mode = atoi(argv[2]);
 				dump_path = argv[3];
 			}
 			else if (argc != 0)
 			std::cout << "Usage: demo_slam <display-enabled=1> <image-mode=0> <dump-path=.>" << std::endl;
 
-			demo_slam01(display);
+			demo_slam01();
 		}