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