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/utilite/ULogger.h>
00029 #include <rtabmap/utilite/UEventsManager.h>
00030 #include <rtabmap/utilite/UFile.h>
00031 #include <rtabmap/utilite/UConversion.h>
00032 #include <rtabmap/utilite/UStl.h>
00033 #include <rtabmap/core/OdometryF2F.h>
00034 #include <rtabmap/core/OdometryMono.h>
00035 #include <rtabmap/core/OdometryThread.h>
00036 #include <rtabmap/gui/OdometryViewer.h>
00037 #include <rtabmap/core/CameraThread.h>
00038 #include <rtabmap/core/CameraRGBD.h>
00039 #include <rtabmap/core/CameraStereo.h>
00040 #include <rtabmap/core/DBReader.h>
00041 #include <rtabmap/core/VWDictionary.h>
00042 #include <QApplication>
00043 #include <QPushButton>
00044 #include <pcl/console/print.h>
00045 #include <rtabmap/core/OdometryF2M.h>
00046
00047 void showUsage()
00048 {
00049 printf("\nUsage:\n"
00050 "odometryViewer [options]\n"
00051 "Options:\n"
00052 " -driver # Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=dc1394, 7=FlyCapture2\n"
00053 " -hz #.# Camera rate (default 0, 0 means as fast as the camera can)\n"
00054 " -db \"input.db\" Use database instead of camera (recorded with rtabmap-dataRecorder)\n"
00055 " -clouds # Maximum clouds shown (default 10, zero means inf)\n"
00056 " -sec #.# Delay (seconds) before reading the database (if set)\n"
00057 "%s\n",
00058 rtabmap::Parameters::showUsage());
00059 exit(1);
00060 }
00061
00062 int main (int argc, char * argv[])
00063 {
00064 ULogger::setType(ULogger::kTypeConsole);
00065 ULogger::setLevel(ULogger::kInfo);
00066
00067
00068 float rate = 0.0;
00069 std::string inputDatabase;
00070 int driver = 0;
00071 int maxClouds = 10;
00072 float sec = 0.0f;
00073
00074 for(int i=1; i<argc; ++i)
00075 {
00076 if(strcmp(argv[i], "-driver") == 0)
00077 {
00078 ++i;
00079 if(i < argc)
00080 {
00081 driver = std::atoi(argv[i]);
00082 if(driver < 0 || driver > 7)
00083 {
00084 showUsage();
00085 }
00086 }
00087 else
00088 {
00089 showUsage();
00090 }
00091 continue;
00092 }
00093 if(strcmp(argv[i], "-hz") == 0)
00094 {
00095 ++i;
00096 if(i < argc)
00097 {
00098 rate = uStr2Float(argv[i]);
00099 if(rate < 0)
00100 {
00101 showUsage();
00102 }
00103 }
00104 else
00105 {
00106 showUsage();
00107 }
00108 continue;
00109 }
00110 if(strcmp(argv[i], "-db") == 0)
00111 {
00112 ++i;
00113 if(i < argc)
00114 {
00115 inputDatabase = argv[i];
00116 if(UFile::getExtension(inputDatabase).compare("db") != 0)
00117 {
00118 printf("Database path (%s) should end with \"db\" \n", inputDatabase.c_str());
00119 showUsage();
00120 }
00121 }
00122 else
00123 {
00124 showUsage();
00125 }
00126 continue;
00127 }
00128 if(strcmp(argv[i], "-clouds") == 0)
00129 {
00130 ++i;
00131 if(i < argc)
00132 {
00133 maxClouds = std::atoi(argv[i]);
00134 if(maxClouds < 0)
00135 {
00136 showUsage();
00137 }
00138 }
00139 else
00140 {
00141 showUsage();
00142 }
00143 continue;
00144 }
00145 if(strcmp(argv[i], "-sec") == 0)
00146 {
00147 ++i;
00148 if(i < argc)
00149 {
00150 sec = uStr2Float(argv[i]);
00151 if(sec < 0.0f)
00152 {
00153 showUsage();
00154 }
00155 }
00156 else
00157 {
00158 showUsage();
00159 }
00160 continue;
00161 }
00162 if(strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "--help") == 0)
00163 {
00164 showUsage();
00165 }
00166 }
00167
00168 if(inputDatabase.size())
00169 {
00170 UINFO("Using database input \"%s\"", inputDatabase.c_str());
00171 }
00172 else
00173 {
00174 UINFO("Using OpenNI camera");
00175 }
00176
00177 UINFO("Camera rate = %f Hz", rate);
00178 UINFO("Maximum clouds shown = %d", maxClouds);
00179 UINFO("Delay = %f s", sec);
00180
00181 rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
00182 for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00183 {
00184 UINFO(" Param \"%s\"=\"%s\"", iter->first.c_str(), iter->second.c_str());
00185 }
00186
00187 bool icp = false;
00188 int regStrategy = rtabmap::Parameters::defaultRegStrategy();
00189 rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kRegStrategy(), regStrategy);
00190 int decimation = 8;
00191 float maxDepth = 4.0f;
00192 float voxelSize = rtabmap::Parameters::defaultIcpVoxelSize();
00193 int normalsK = 0;
00194 float normalsRadius = 0.0f;
00195 if(regStrategy == 1 || regStrategy == 2)
00196 {
00197
00198 icp = true;
00199
00200 rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpDownsamplingStep(), decimation);
00201 rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpVoxelSize(), voxelSize);
00202
00203 bool pointToPlane = rtabmap::Parameters::defaultIcpPointToPlane();
00204 rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlane(), pointToPlane);
00205 if(pointToPlane)
00206 {
00207 normalsK = rtabmap::Parameters::defaultIcpPointToPlaneK();
00208 rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneK(), normalsK);
00209 normalsRadius = rtabmap::Parameters::defaultIcpPointToPlaneRadius();
00210 rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneRadius(), normalsRadius);
00211 }
00212
00213 uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpDownsamplingStep(), "1"));
00214 uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpVoxelSize(), "0"));
00215 }
00216
00217 QApplication app(argc, argv);
00218
00219 rtabmap::Odometry * odom = rtabmap::Odometry::create(parameters);
00220
00221 rtabmap::OdometryThread odomThread(odom);
00222 rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
00223 UEventsManager::addHandler(&odomThread);
00224 UEventsManager::addHandler(&odomViewer);
00225
00226 odomViewer.setWindowTitle("Odometry view");
00227 odomViewer.resize(1280, 480+QPushButton().minimumHeight());
00228
00229 rtabmap::Camera * camera = 0;
00230 rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
00231
00232 if(inputDatabase.size())
00233 {
00234 camera = new rtabmap::DBReader(inputDatabase, rate, true);
00235 }
00236 else if(driver == 0)
00237 {
00238 camera = new rtabmap::CameraOpenni("", rate, t);
00239 }
00240 else if(driver == 1)
00241 {
00242 if(!rtabmap::CameraOpenNI2::available())
00243 {
00244 UERROR("Not built with OpenNI2 support...");
00245 exit(-1);
00246 }
00247 camera = new rtabmap::CameraOpenNI2("", rtabmap::CameraOpenNI2::kTypeColorDepth, rate, t);
00248 }
00249 else if(driver == 2)
00250 {
00251 if(!rtabmap::CameraFreenect::available())
00252 {
00253 UERROR("Not built with Freenect support...");
00254 exit(-1);
00255 }
00256 camera = new rtabmap::CameraFreenect(0, rtabmap::CameraFreenect::kTypeColorDepth, rate, t);
00257 }
00258 else if(driver == 3)
00259 {
00260 if(!rtabmap::CameraOpenNICV::available())
00261 {
00262 UERROR("Not built with OpenNI from OpenCV support...");
00263 exit(-1);
00264 }
00265 camera = new rtabmap::CameraOpenNICV(false, rate, t);
00266 }
00267 else if(driver == 4)
00268 {
00269 if(!rtabmap::CameraOpenNICV::available())
00270 {
00271 UERROR("Not built with OpenNI from OpenCV support...");
00272 exit(-1);
00273 }
00274 camera = new rtabmap::CameraOpenNICV(true, rate, t);
00275 }
00276 else if(driver == 5)
00277 {
00278 if(!rtabmap::CameraFreenect2::available())
00279 {
00280 UERROR("Not built with Freenect2 support...");
00281 exit(-1);
00282 }
00283 camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate, t);
00284 }
00285 else if(driver == 6)
00286 {
00287 if(!rtabmap::CameraStereoDC1394::available())
00288 {
00289 UERROR("Not built with dc1394 support...");
00290 exit(-1);
00291 }
00292 camera = new rtabmap::CameraStereoDC1394(rate, t);
00293 }
00294 else if(driver == 7)
00295 {
00296 if(!rtabmap::CameraStereoFlyCapture2::available())
00297 {
00298 UERROR("Not built with FlyCapture2/Triclops support...");
00299 exit(-1);
00300 }
00301 camera = new rtabmap::CameraStereoFlyCapture2(rate, t);
00302 }
00303 else
00304 {
00305 UFATAL("Camera driver (%d) not found!", driver);
00306 }
00307
00308
00309
00310 if(camera->init())
00311 {
00312 if(camera->isCalibrated())
00313 {
00314 rtabmap::CameraThread cameraThread(camera, parameters);
00315
00316 cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK, normalsRadius);
00317
00318 odomThread.start();
00319 cameraThread.start();
00320
00321 odomViewer.exec();
00322
00323 cameraThread.join(true);
00324 odomThread.join(true);
00325 }
00326 else
00327 {
00328 printf("The camera is not calibrated! You should calibrate the camera first.\n");
00329 delete camera;
00330 }
00331 }
00332 else
00333 {
00334 printf("Failed to initialize the camera! Please select another driver (see \"--help\").\n");
00335 delete camera;
00336 }
00337
00338 return 0;
00339 }