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/CameraRGBD.h"
00029 #include "rtabmap/core/CameraStereo.h"
00030 #include "rtabmap/core/util2d.h"
00031 #include "rtabmap/core/util3d.h"
00032 #include "rtabmap/core/util3d_transforms.h"
00033 #include "rtabmap/utilite/ULogger.h"
00034 #include "rtabmap/utilite/UMath.h"
00035 #include "rtabmap/utilite/UFile.h"
00036 #include "rtabmap/utilite/UDirectory.h"
00037 #include "rtabmap/utilite/UConversion.h"
00038 #include <opencv2/highgui/highgui.hpp>
00039 #include <opencv2/imgproc/imgproc.hpp>
00040 #include <pcl/visualization/cloud_viewer.h>
00041 #include <stdio.h>
00042 #include <signal.h>
00043
00044 void showUsage()
00045 {
00046 printf("\nUsage:\n"
00047 "rtabmap-rgbd_camera [options] driver\n"
00048 " driver Driver number to use: 0=OpenNI-PCL (Kinect)\n"
00049 " 1=OpenNI2 (Kinect and Xtion PRO Live)\n"
00050 " 2=Freenect (Kinect)\n"
00051 " 3=OpenNI-CV (Kinect)\n"
00052 " 4=OpenNI-CV-ASUS (Xtion PRO Live)\n"
00053 " 5=Freenect2 (Kinect v2)\n"
00054 " 6=DC1394 (Bumblebee2)\n"
00055 " 7=FlyCapture2 (Bumblebee2)\n"
00056 " 8=ZED stereo\n"
00057 " Options:\n"
00058 " -rate #.# Input rate Hz (default 0=inf)\n"
00059 " -save_stereo \"path\" Save stereo images in a folder or a video file (side by side *.avi).\n"
00060 " -fourcc \"XXXX\" Four characters FourCC code (default is \"MJPG\") used\n"
00061 " when saving stereo images to a video file.\n"
00062 " See http://www.fourcc.org/codecs.php for more codes.\n");
00063 exit(1);
00064 }
00065
00066
00067 bool running = true;
00068 void sighandler(int sig)
00069 {
00070 printf("\nSignal %d caught...\n", sig);
00071 running = false;
00072 }
00073
00074 int main(int argc, char * argv[])
00075 {
00076 ULogger::setType(ULogger::kTypeConsole);
00077 ULogger::setLevel(ULogger::kInfo);
00078
00079
00080
00081 int driver = 0;
00082 std::string stereoSavePath;
00083 float rate = 0.0f;
00084 std::string fourcc = "MJPG";
00085 if(argc < 2)
00086 {
00087 showUsage();
00088 }
00089 else
00090 {
00091 for(int i=1; i<argc; ++i)
00092 {
00093 if(strcmp(argv[i], "-rate") == 0)
00094 {
00095 ++i;
00096 if(i < argc)
00097 {
00098 rate = uStr2Float(argv[i]);
00099 if(rate < 0.0f)
00100 {
00101 showUsage();
00102 }
00103 }
00104 else
00105 {
00106 showUsage();
00107 }
00108 continue;
00109 }
00110 if(strcmp(argv[i], "-save_stereo") == 0)
00111 {
00112 ++i;
00113 if(i < argc)
00114 {
00115 stereoSavePath = argv[i];
00116 }
00117 else
00118 {
00119 showUsage();
00120 }
00121 continue;
00122 }
00123 if(strcmp(argv[i], "-fourcc") == 0)
00124 {
00125 ++i;
00126 if(i < argc)
00127 {
00128 fourcc = argv[i];
00129 if(fourcc.size() != 4)
00130 {
00131 UERROR("fourcc should be 4 characters.");
00132 showUsage();
00133 }
00134 }
00135 else
00136 {
00137 showUsage();
00138 }
00139 continue;
00140 }
00141 if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0)
00142 {
00143 showUsage();
00144 }
00145 else if(i< argc-1)
00146 {
00147 printf("Unrecognized option \"%s\"", argv[i]);
00148 showUsage();
00149 }
00150
00151
00152 driver = atoi(argv[i]);
00153 if(driver < 0 || driver > 8)
00154 {
00155 UERROR("driver should be between 0 and 8.");
00156 showUsage();
00157 }
00158 }
00159 }
00160 UINFO("Using driver %d", driver);
00161
00162 rtabmap::Camera * camera = 0;
00163 if(driver < 6)
00164 {
00165 if(!stereoSavePath.empty())
00166 {
00167 UWARN("-save_stereo option cannot be used with RGB-D drivers.");
00168 stereoSavePath.clear();
00169 }
00170
00171 if(driver == 0)
00172 {
00173 camera = new rtabmap::CameraOpenni();
00174 }
00175 else if(driver == 1)
00176 {
00177 if(!rtabmap::CameraOpenNI2::available())
00178 {
00179 UERROR("Not built with OpenNI2 support...");
00180 exit(-1);
00181 }
00182 camera = new rtabmap::CameraOpenNI2();
00183 }
00184 else if(driver == 2)
00185 {
00186 if(!rtabmap::CameraFreenect::available())
00187 {
00188 UERROR("Not built with Freenect support...");
00189 exit(-1);
00190 }
00191 camera = new rtabmap::CameraFreenect();
00192 }
00193 else if(driver == 3)
00194 {
00195 if(!rtabmap::CameraOpenNICV::available())
00196 {
00197 UERROR("Not built with OpenNI from OpenCV support...");
00198 exit(-1);
00199 }
00200 camera = new rtabmap::CameraOpenNICV(false);
00201 }
00202 else if(driver == 4)
00203 {
00204 if(!rtabmap::CameraOpenNICV::available())
00205 {
00206 UERROR("Not built with OpenNI from OpenCV support...");
00207 exit(-1);
00208 }
00209 camera = new rtabmap::CameraOpenNICV(true);
00210 }
00211 else if(driver == 5)
00212 {
00213 if(!rtabmap::CameraFreenect2::available())
00214 {
00215 UERROR("Not built with Freenect2 support...");
00216 exit(-1);
00217 }
00218 camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD);
00219 }
00220 }
00221 else if(driver == 6)
00222 {
00223 if(!rtabmap::CameraStereoDC1394::available())
00224 {
00225 UERROR("Not built with DC1394 support...");
00226 exit(-1);
00227 }
00228 camera = new rtabmap::CameraStereoDC1394();
00229 }
00230 else if(driver == 7)
00231 {
00232 if(!rtabmap::CameraStereoFlyCapture2::available())
00233 {
00234 UERROR("Not built with FlyCapture2/Triclops support...");
00235 exit(-1);
00236 }
00237 camera = new rtabmap::CameraStereoFlyCapture2();
00238 }
00239 else if(driver == 8)
00240 {
00241 if(!rtabmap::CameraStereoZed::available())
00242 {
00243 UERROR("Not built with ZED sdk support...");
00244 exit(-1);
00245 }
00246 camera = new rtabmap::CameraStereoZed(0);
00247 }
00248 else
00249 {
00250 UFATAL("");
00251 }
00252
00253 if(!camera->init())
00254 {
00255 printf("Camera init failed! Please select another driver (see \"--help\").\n");
00256 delete camera;
00257 exit(1);
00258 }
00259
00260 rtabmap::SensorData data = camera->takeImage();
00261 if(data.imageRaw().cols != data.depthOrRightRaw().cols || data.imageRaw().rows != data.depthOrRightRaw().rows)
00262 {
00263 UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.",
00264 data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
00265 }
00266 pcl::visualization::CloudViewer * viewer = 0;
00267 if(!data.stereoCameraModel().isValidForProjection() && (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValidForProjection()))
00268 {
00269 UWARN("Camera not calibrated! The registered cloud cannot be shown.");
00270 }
00271 else
00272 {
00273 viewer = new pcl::visualization::CloudViewer("cloud");
00274 }
00275 rtabmap::Transform t(1, 0, 0, 0,
00276 0, -1, 0, 0,
00277 0, 0, -1, 0);
00278
00279 cv::VideoWriter videoWriter;
00280 UDirectory dir;
00281 if(!stereoSavePath.empty() &&
00282 !data.imageRaw().empty() &&
00283 !data.rightRaw().empty())
00284 {
00285 if(UFile::getExtension(stereoSavePath).compare("avi") == 0)
00286 {
00287 if(data.imageRaw().size() == data.rightRaw().size())
00288 {
00289 if(rate <= 0)
00290 {
00291 UERROR("You should set the input rate when saving stereo images to a video file.");
00292 showUsage();
00293 }
00294 cv::Size targetSize = data.imageRaw().size();
00295 targetSize.width *= 2;
00296 UASSERT(fourcc.size() == 4);
00297 videoWriter.open(
00298 stereoSavePath,
00299 CV_FOURCC(fourcc.at(0), fourcc.at(1), fourcc.at(2), fourcc.at(3)),
00300 rate,
00301 targetSize,
00302 data.imageRaw().channels() == 3);
00303 }
00304 else
00305 {
00306 UERROR("Images not the same size, cannot save stereo images to the video file.");
00307 }
00308 }
00309 else if(UDirectory::exists(stereoSavePath))
00310 {
00311 UDirectory::makeDir(stereoSavePath+"/"+"left");
00312 UDirectory::makeDir(stereoSavePath+"/"+"right");
00313 }
00314 else
00315 {
00316 UERROR("Directory \"%s\" doesn't exist.", stereoSavePath.c_str());
00317 stereoSavePath.clear();
00318 }
00319 }
00320
00321
00322 signal(SIGABRT, &sighandler);
00323 signal(SIGTERM, &sighandler);
00324 signal(SIGINT, &sighandler);
00325
00326 int id=1;
00327 while(!data.imageRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running)
00328 {
00329 cv::Mat rgb = data.imageRaw();
00330 if(!data.depthRaw().empty() && (data.depthRaw().type() == CV_16UC1 || data.depthRaw().type() == CV_32FC1))
00331 {
00332
00333 cv::Mat depth = data.depthRaw();
00334 if(depth.type() == CV_32FC1)
00335 {
00336 depth = rtabmap::util2d::cvtDepthFromFloat(depth);
00337 }
00338
00339 if(rgb.cols == depth.cols && rgb.rows == depth.rows &&
00340 data.cameraModels().size() &&
00341 data.cameraModels()[0].isValidForProjection())
00342 {
00343 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(
00344 rgb, depth,
00345 data.cameraModels()[0]);
00346 cloud = rtabmap::util3d::transformPointCloud(cloud, t);
00347 if(viewer)
00348 viewer->showCloud(cloud, "cloud");
00349 }
00350 else if(!depth.empty() &&
00351 data.cameraModels().size() &&
00352 data.cameraModels()[0].isValidForProjection())
00353 {
00354 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(
00355 depth,
00356 data.cameraModels()[0]);
00357 cloud = rtabmap::util3d::transformPointCloud(cloud, t);
00358 viewer->showCloud(cloud, "cloud");
00359 }
00360
00361 cv::Mat tmp;
00362 unsigned short min=0, max = 2048;
00363 uMinMax((unsigned short*)depth.data, depth.rows*depth.cols, min, max);
00364 depth.convertTo(tmp, CV_8UC1, 255.0/max);
00365
00366 cv::imshow("Video", rgb);
00367 cv::imshow("Depth", tmp);
00368 }
00369 else if(!data.rightRaw().empty())
00370 {
00371
00372 cv::Mat right = data.rightRaw();
00373 cv::imshow("Left", rgb);
00374 cv::imshow("Right", right);
00375
00376 if(rgb.cols == right.cols && rgb.rows == right.rows && data.stereoCameraModel().isValidForProjection())
00377 {
00378 if(right.channels() == 3)
00379 {
00380 cv::cvtColor(right, right, CV_BGR2GRAY);
00381 }
00382 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(
00383 rgb, right,
00384 data.stereoCameraModel());
00385 cloud = rtabmap::util3d::transformPointCloud(cloud, t);
00386 if(viewer)
00387 viewer->showCloud(cloud, "cloud");
00388 }
00389 }
00390
00391 int c = cv::waitKey(10);
00392 if(c == 27)
00393 break;
00394
00395 if(videoWriter.isOpened())
00396 {
00397 cv::Mat left = data.imageRaw();
00398 cv::Mat right = data.rightRaw();
00399 if(left.size() == right.size())
00400 {
00401 cv::Size targetSize = left.size();
00402 targetSize.width *= 2;
00403 cv::Mat targetImage(targetSize, left.type());
00404 if(right.type() != left.type())
00405 {
00406 cv::Mat tmp;
00407 cv::cvtColor(right, tmp, left.channels()==3?CV_GRAY2BGR:CV_BGR2GRAY);
00408 right = tmp;
00409 }
00410 UASSERT(left.type() == right.type());
00411
00412 cv::Mat roiA(targetImage, cv::Rect( 0, 0, left.size().width, left.size().height ));
00413 left.copyTo(roiA);
00414 cv::Mat roiB( targetImage, cvRect( left.size().width, 0, left.size().width, left.size().height ) );
00415 right.copyTo(roiB);
00416
00417 videoWriter.write(targetImage);
00418 printf("Saved frame %d to \"%s\"\n", id, stereoSavePath.c_str());
00419 }
00420 else
00421 {
00422 UERROR("Left and right images are not the same size!?");
00423 }
00424 }
00425 else if(!stereoSavePath.empty())
00426 {
00427 cv::imwrite(stereoSavePath+"/"+"left/"+uNumber2Str(id) + ".jpg", data.imageRaw());
00428 cv::imwrite(stereoSavePath+"/"+"right/"+uNumber2Str(id) + ".jpg", data.rightRaw());
00429 printf("Saved frames %d to \"%s/left\" and \"%s/right\" directories\n", id, stereoSavePath.c_str(), stereoSavePath.c_str());
00430 }
00431 ++id;
00432 data = camera->takeImage();
00433 }
00434 printf("Closing...\n");
00435 if(viewer)
00436 {
00437 delete viewer;
00438 }
00439 cv::destroyWindow("Video");
00440 cv::destroyWindow("Depth");
00441 delete camera;
00442 return 0;
00443 }