Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <rtabmap/core/Rtabmap.h>
00029 #include <rtabmap/core/CameraStereo.h>
00030 #include <rtabmap/utilite/UThread.h>
00031 #include "MapBuilder.h"
00032 #include <pcl/visualization/cloud_viewer.h>
00033 #include <rtabmap/core/OdometryF2M.h>
00034 #include <QApplication>
00035 #include <stdio.h>
00036
00037 using namespace rtabmap;
00038
00039 void showUsage()
00040 {
00041 printf("\nUsage:\n"
00042 "rtabmap-noEventsExample camera_rate odom_update map_update calibration_dir calibration_name path_left_images path_right_images\n"
00043 "Description:\n"
00044 " camera_rate Rate (Hz) of the camera.\n"
00045 " odom_update Do odometry update each X camera frames.\n"
00046 " map_update Do map update each X odometry frames.\n"
00047 "\n"
00048 "Example:\n"
00049 " (with images from \"https://github.com/introlab/rtabmap/wiki/Stereo-mapping#process-a-directory-of-stereo-images\") \n"
00050 " $ rtabmap-noEventsExample 20 2 10 stereo_20hz stereo_20Hz stereo_20hz/left stereo_20hz/right\n"
00051 " Camera rate = 20 Hz\n"
00052 " Odometry update rate = 10 Hz\n"
00053 " Map update rate = 1 Hz\n");
00054 exit(1);
00055 }
00056
00057 int main(int argc, char * argv[])
00058 {
00059 ULogger::setType(ULogger::kTypeConsole);
00060 ULogger::setLevel(ULogger::kError);
00061
00062 if(argc < 8)
00063 {
00064 showUsage();
00065 }
00066
00067 int argIndex = 1;
00068 int cameraRate = atoi(argv[argIndex++]);
00069 if(cameraRate <= 0)
00070 {
00071 printf("camera_rate should be > 0\n");
00072 showUsage();
00073 }
00074 int odomUpdate = atoi(argv[argIndex++]);
00075 if(odomUpdate <= 0)
00076 {
00077 printf("odom_update should be > 0\n");
00078 showUsage();
00079 }
00080 int mapUpdate = atoi(argv[argIndex++]);
00081 if(mapUpdate <= 0)
00082 {
00083 printf("map_update should be > 0\n");
00084 showUsage();
00085 }
00086
00087 printf("Camera rate = %d Hz\n", cameraRate);
00088 printf("Odometry update rate = %d Hz\n", cameraRate/odomUpdate);
00089 printf("Map update rate = %d Hz\n", (cameraRate/odomUpdate)/mapUpdate);
00090
00091 std::string calibrationDir = argv[argIndex++];
00092 std::string calibrationName = argv[argIndex++];
00093 std::string pathLeftImages = argv[argIndex++];
00094 std::string pathRightImages = argv[argIndex++];
00095
00096 Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
00097 CameraStereoImages camera(
00098 pathLeftImages,
00099 pathRightImages,
00100 false,
00101 (float)cameraRate,
00102 opticalRotation);
00103
00104 if(camera.init(calibrationDir, calibrationName))
00105 {
00106 OdometryF2M odom;
00107 Rtabmap rtabmap;
00108 rtabmap.init();
00109
00110 QApplication app(argc, argv);
00111 MapBuilder mapBuilder;
00112 mapBuilder.show();
00113 QApplication::processEvents();
00114
00115 SensorData data = camera.takeImage();
00116 int cameraIteration = 0;
00117 int odometryIteration = 0;
00118 printf("Press \"Space\" in the window to pause\n");
00119 while(data.isValid() && mapBuilder.isVisible())
00120 {
00121 if(cameraIteration++ % odomUpdate == 0)
00122 {
00123 OdometryInfo info;
00124 Transform pose = odom.process(data, &info);
00125
00126 if(odometryIteration++ % mapUpdate == 0)
00127 {
00128 if(rtabmap.process(data, pose))
00129 {
00130 mapBuilder.processStatistics(rtabmap.getStatistics());
00131 if(rtabmap.getLoopClosureId() > 0)
00132 {
00133 printf("Loop closure detected!\n");
00134 }
00135 }
00136 }
00137
00138 mapBuilder.processOdometry(data, pose, info);
00139 }
00140
00141 QApplication::processEvents();
00142
00143 while(mapBuilder.isPaused() && mapBuilder.isVisible())
00144 {
00145 uSleep(100);
00146 QApplication::processEvents();
00147 }
00148
00149 data = camera.takeImage();
00150 }
00151
00152 if(mapBuilder.isVisible())
00153 {
00154 printf("Processed all frames\n");
00155 app.exec();
00156 }
00157 }
00158 else
00159 {
00160 UERROR("Camera init failed!");
00161 }
00162
00163 return 0;
00164 }