daily pastebin goal
22%
SHARE
TWEET

main.cpp

lamiastella May 23rd, 2017 28 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. //NOMINMAX would help to avoid the following error
  2. //1>C:\Program Files\PCL 1.8.0\include\pcl-1.8\pcl/io/pcd_io.h(281):
  3. //error C2589: '(': illegal token on right side of '::'
  4. #define NOMINMAX
  5.  
  6. #define _WINSOCKAPI_  
  7. #include <limits>
  8. // C++ Libraries
  9. #include <stdio.h>
  10. #include <iostream>
  11. #include <string>
  12. #include <time.h>
  13.  
  14. // OpenCV Libraries
  15. #include <opencv/cxcore.h>
  16. #include "opencv2/highgui/highgui.hpp"
  17.  
  18. #include "SR300Camera.h"
  19. #include "RealSense/SenseManager.h"
  20. #include "RealSense/SampleReader.h"
  21. #include "util_render.h"
  22.  
  23. // OpenARK Libraries
  24. #include "Webcam.h"
  25. #include "Visualizer.h"
  26. #include "Hand.h"
  27. #include "Plane.h"
  28. #include "Calibration.h"
  29. #include "Util.h"
  30. #include "UDPSender.h"
  31. #include "Object3D.h"
  32. #include "StreamingAverager.h"
  33.  
  34. int main() {
  35.     clock_t starttime = clock();
  36.     DepthCamera* sr300 = new SR300Camera();
  37.     //RGBCamera *cam = new Webcam(1);
  38.     int frame = 0;
  39.     //Calibration::XYZToUnity(*pmd, 4, 4, 3);
  40.     cv::FileStorage fs;
  41.     fs.open("RT_Transform.txt", cv::FileStorage::READ);
  42.  
  43.     cv::Mat r, t;
  44.     fs["R"] >> r;
  45.     fs["T"] >> t;
  46.  
  47.     fs.release();
  48.  
  49.     UDPSender u = UDPSender();
  50.     cv::namedWindow("Results", CV_WINDOW_NORMAL);
  51.  
  52.     StreamingAverager handAverager = StreamingAverager(4, 0.1);
  53.     StreamingAverager paleeteAverager = StreamingAverager(6, 0.05);
  54.  
  55.     while (true)
  56.     {
  57.         sr300->update();
  58.  
  59.         /**
  60.         std::string filename = "..//OpenARK_Datasets//TwoHandDataSet1//img" + std::to_string(frame) + ".yml";
  61.         if (!pmd->readImage(filename))
  62.         break;
  63.         **/
  64.  
  65.         // Loading image from sensor
  66.  
  67.         sr300->removeNoise();
  68.         cv::imshow("XYZ Map", Visualizer::visualizeXYZMap(sr300->getXYZMap()));
  69.         if (sr300->badInput) {
  70.             continue;
  71.         }
  72.  
  73.         // Classifying objects in the scene
  74.  
  75.         sr300->computeClusters(0.02, 500);
  76.         std::vector<cv::Mat> clusters = sr300->getClusters();
  77.         std::vector<Object3D> objects;
  78.  
  79.         int handObjectIndex = -1, planeObjectIndex = -1;
  80.         for (int i = 0; i < clusters.size(); i++) {
  81.             Object3D obj = Object3D(clusters[i].clone());
  82.             if (obj.hasHand) {
  83.                 handObjectIndex = i;
  84.             }
  85.  
  86.             if (obj.hasPlane) {
  87.                 planeObjectIndex = i;
  88.             }
  89.             objects.push_back(obj);
  90.  
  91.         }
  92.  
  93.         // Interprate the relationship between the objects
  94.  
  95.         bool clicked = false, paletteFound = false;
  96.         Object3D handObject, planeObject;
  97.         cv::Point paletteCenter(-1. - 1);
  98.         cv::Mat mask = cv::Mat::zeros(sr300->getXYZMap().rows, sr300->getXYZMap().cols, CV_8UC1);
  99.         if (planeObjectIndex != -1 && handObjectIndex != -1) {
  100.             planeObject = objects[planeObjectIndex];
  101.             handObject = objects[handObjectIndex];
  102.  
  103.             clicked = handObject.getHand().touchObject(planeObject.getPlane().getPlaneEquation(), planeObject.getPlane().R_SQUARED_DISTANCE_THRESHOLD * 5);
  104.             cv::Mat scene = Visualizer::visualizePlaneRegression(sr300->getXYZMap(), planeObject.getPlane().getPlaneEquation(), planeObject.getPlane().R_SQUARED_DISTANCE_THRESHOLD, clicked);
  105.             //scene = Visualizer::visualizeHand(scene, handObject.getHand().pointer_finger_ij, handObject.getHand().shape_centroid_ij);
  106.             if (planeObject.leftEdgeConnected) {
  107.                 Visualizer::visualizePlanePoints(mask, planeObject.getPlane().getPlaneIndicies());
  108.                 cv::Moments m = cv::moments(mask, false);
  109.                 paletteCenter = cv::Point(m.m10 / m.m00, m.m01 / m.m00);
  110.                 cv::circle(scene, paletteCenter, 2, cv::Scalar(0, 0, 255), 2);
  111.                 paletteFound = true;
  112.             }
  113.             cv::imshow("Results", scene);
  114.         }
  115.         else if (handObjectIndex != -1) {
  116.             handObject = objects[handObjectIndex];
  117.             //cv::imshow("Results", Visualizer::visualizeHand(pmd->getXYZMap(), handObject.getHand().pointer_finger_ij, handObject.getHand().shape_centroid_ij));
  118.         }
  119.         else if (planeObjectIndex != -1) {
  120.             planeObject = objects[planeObjectIndex];
  121.             cv::Mat scene = Visualizer::visualizePlaneRegression(sr300->getXYZMap(), planeObject.getPlane().getPlaneEquation(), planeObject.getPlane().R_SQUARED_DISTANCE_THRESHOLD, clicked);
  122.             if (planeObject.leftEdgeConnected) {
  123.                 Visualizer::visualizePlanePoints(mask, planeObject.getPlane().getPlaneIndicies());
  124.                 cv::Moments m = cv::moments(mask, false);
  125.                 paletteCenter = cv::Point(m.m10 / m.m00, m.m01 / m.m00);
  126.                 cv::circle(scene, paletteCenter, 2, cv::Scalar(0, 0, 255), 2);
  127.                 paletteFound = true;
  128.             }
  129.             cv::imshow("Results", scene);
  130.         }
  131.  
  132.         // Organize the data and send to game engine
  133.  
  134.         std::string handX = "-", handY = "-", handZ = "-";
  135.         std::string paletteX = "-", paletteY = "-", paletteZ = "-";
  136.         std::string clickStatus = "2";
  137.         std::string num_fingers = "0";
  138.         if (handObjectIndex != -1) {
  139.             cv::Vec3f handPos = handAverager.addDataPoint(objects[handObjectIndex].getHand().fingers_xyz[0]);
  140.             //float hand_pt[3] = { objects[handObjectIndex].getHand().pointer_finger_xyz[0], objects[handObjectIndex].getHand().pointer_finger_xyz[1], objects[handObjectIndex].getHand().pointer_finger_xyz[2]};
  141.             float hand_pt[3] = { handPos[0], handPos[1], handPos[2] };
  142.             cv::Mat hand_mat = cv::Mat(3, 1, CV_32FC1, &hand_pt);
  143.             hand_mat = r*hand_mat + t;
  144.             handX = std::to_string(hand_mat.at<float>(0, 0));
  145.             handY = std::to_string(hand_mat.at<float>(1, 0));
  146.             handZ = std::to_string(hand_mat.at<float>(2, 0));
  147.             num_fingers = std::to_string(objects[handObjectIndex].getHand().fingers_xyz.size());
  148.         }
  149.         else {
  150.             handAverager.addEmptyPoint();
  151.         }
  152.         if (paletteFound) {
  153.             cv::Vec3f pt = paleeteAverager.addDataPoint(sr300->getXYZMap().at<cv::Vec3f>(paletteCenter.y, paletteCenter.x));
  154.             float palette_pt[3] = { pt[0], pt[1], pt[2] };
  155.             cv::Mat palette_mat = cv::Mat(3, 1, CV_32FC1, &palette_pt);
  156.             palette_mat = r*palette_mat + t;
  157.             paletteX = std::to_string(palette_mat.at<float>(0, 0));
  158.             paletteY = std::to_string(palette_mat.at<float>(1, 0));
  159.             paletteZ = std::to_string(palette_mat.at<float>(2, 0));
  160.         }
  161.         else {
  162.             paleeteAverager.addEmptyPoint();
  163.         }
  164.         if (clicked) {
  165.             clickStatus = "1";
  166.         }
  167.  
  168.         std::string tempS = "";
  169.         tempS = handX + "%" + handY + "%" + handZ + "%" + paletteX + "%" + paletteY + "%" + paletteZ + "%" + clickStatus + "%" + num_fingers;
  170.         u.send(tempS);
  171.  
  172.  
  173.  
  174.         /**** Start: Write Frames to File ****/
  175.         //std::string filename = "img" + std::to_string(frame) + ".yml";
  176.         //pmd->writeImage(filename);
  177.         //std::cout << filename << std::endl;
  178.         /**** End: Write Frames to File ****/
  179.  
  180.  
  181.  
  182.         /**** Start: Loop Break Condition ****/
  183.         int c = cvWaitKey(1);
  184.         if (c == 'q' || c == 'Q' || c == 27) {
  185.             break;
  186.         }
  187.         /**** End: Loop Break Condition ****/
  188.         frame++;
  189.     }
  190.  
  191.     sr300->destroyInstance();
  192.     return 0;
  193. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top