DAVID4 SDK  1.8.7
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Groups Pages
SimpleMeshComparison.cpp
1 /// @example SimpleMeshComparison.cpp
2 ///
3 /// Shows usage of david::ShapeFusion and david::Measure class.
4 
5 #include "davidSDK/david.h"
6 
7 #include <math.h>
8 #include <iostream>
9 
10 #ifdef _WIN32
11 #include <conio.h> // required for _getch
12 #else
13 #include <curses.h>
14 #define _getch getch
15 #endif
16 
17 
18 namespace examples {
19 
20 /// Shows usage of david::ShapeFusion and david::Measure class.
21 ///
23 {
24  try
25  {
26  //=============
27  // PREPARATION
28  //=============
29  // INITIALIZE, CONNECT TO SERVER:
30  david::Client david;
31  david.Connect();
32 
33  // IMPORT REFERENCE SCAN:
34  int idRef = david.fusion().ImportMesh("Reference.obj");
35  // Best to use absolute paths like "C:/Scans/Reference.obj".
36  // We assume that the reference part is complete, i.e. contains all surface areas that will be scanned.
37 
38  // PREPARE SCANNING:
39  // Select screen.
40  david.sls().SetScreenID(2);
41 
42  // II) Select camera based on common name: Select any DAVID cam.
43  std::vector<std::string> cameraNames = david.sls().GetAvailableCameraNames();
44  for (size_t i=0; i < cameraNames.size(); ++i)
45  {
46  if (cameraNames[i].find("DAVID-CAM") != std::string::npos)
47  {
48  david.sls().SelectCamera(cameraNames[i]);
49  break;
50  }
51  }
52 
53 
54  while (true) // repeat forever (press ESC to stop)
55  {
56  printf("\nPress ESC to stop, or any other key to start new measurement\n");
57  char ch = _getch(); // read one char from keyboard
58  if (27==ch) break; // breaks if key is ESC
59 
60  //======
61  // SCAN
62  //======
63  // Scan new part:
64  int num = david.sls().Scan();
65  std::cout << "New scan consists of " << num << " 3D points\n";
66 
67  // Add scan to Shapefusion for further analysis:
68  int idScan = david.sls().AddScanToShapeFusion();
69 
70 
71  //===================
72  // ALIGN AND COMPARE
73  //===================
74  // ALIGN SCAN TO REFERENCE:
75  david.fusion().AlignPairCoarse(idScan, idRef, david::CoarseAlignParams(david::GetMotionInfo_Free())); // first, coarse alignment (free)
76  david.fusion().AlignPairFine(idScan, idRef, david::FineAlignParams()); // then fine alignment
77 
78  // COMPUTE DISTANCE VALUES FOR EACH VERTEX:
79  const double MAX_DIST = 2.0; // maximum for measurement
80  std::vector<float> distances; // prepare array
81  david.measure().ComputeSurfaceDistances(distances, idScan, idRef, MAX_DIST);
82  // "distances" now contains one distance value for each vertex of idScan. You can get those vertices in the same order using david.fusion().GetVertexPositions(...)
83 
84 
85  //=====================
86  // SIMPLE DECISION:
87  // PART IS GOOD OR BAD
88  //=====================
89  const float DIST_GOOD = 1.0f; // 1.0 mm --> larger distance is "bad"
90  const float MAX_ALLOWED_RATIO = 0.05f; // 5 percent bad values allowed
91 
92  int numBad=0; // count "bad" vertices
93  for (float dist : distances) // for each distance value
94  {
95  if (fabs(dist)>DIST_GOOD) // if absolute value is larger than DIST_GOOD
96  {
97  numBad++; // count as bad
98  }
99  }
100  float badRatio = (float)numBad / distances.size(); // relative amount of "bad" vertices
101  std::cout << badRatio*100 << " percent of vertices are 'bad'\n";
102 
103  // You could make a more complex analysis here.
104 
105  if (badRatio <= MAX_ALLOWED_RATIO)
106  {
107  std::cout << "==> PART IS GOOD :-)\n";
108  // Here, make your robot put the part into basket A
109  // using david.fusion().GetPose(gripPose, idScan) to compute your grip pose
110  }
111  else
112  {
113  std::cout << "==> PART IS BAD :-(\n";
114  // Here, make your robot put the part into basket B / trash can
115  // using david.fusion().GetPose(gripPose, idScan) to compute your grip pose
116  }
117 
118 
119  // Delete scan because we don't need it any more:
120  david.fusion().DeleteMesh(idScan);
121  }
122  }
123  catch (david::Exception& e)
124  {
125  e.PrintError();
126  }
127 }
128 
129 } // namespace