main.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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, // assume that images are already rectified
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 }


rtabmap
Author(s): Mathieu Labbe
autogenerated on Sat Jul 23 2016 11:44:16