VisualContactDetection.cpp
Go to the documentation of this file.
1 /*
2 * This file is part of ArmarX.
3 *
4 * ArmarX is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * @package VisionX::Component
17 * @author Kai Welke <kai dot welke at kit dot edu>
18 * @copyright 2011 Humanoids Group, HIS, KIT
19 * @license http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #include "VisualContactDetection.h"
24 
25 #include "../../../vision_algorithms/HandLocalizationWithFingertips/HandLocalisation.h"
26 #include "../../../vision_algorithms/HandLocalizationWithFingertips/Visualization/HandModelVisualizer.h"
27 #include "Helpers/helpers.h"
28 
29 // Eigen
30 #include <Eigen/Core>
31 
32 // IVT
33 #include <Calibration/StereoCalibration.h>
34 #include <Image/ByteImage.h>
35 #include <Image/ImageProcessor.h>
36 #include <Image/IplImageAdaptor.h>
37 #include <Threading/Threading.h>
38 
39 // OpenMP
40 #include <omp.h>
41 
42 // Core
43 #include <ArmarXCore/core/exceptions/Exception.h>
44 
45 #include <RobotAPI/interface/components/ViewSelectionInterface.h>
46 
47 // VisionX
48 #include <sys/time.h>
49 
50 #include <opencv2/video/tracking.hpp>
51 
52 #include <VisionX/tools/ImageUtil.h>
53 #include <VisionX/tools/TypeMapping.h>
54 
55 namespace visionx
56 {
57 
58  void
59  VisualContactDetection::onInitImageProcessor()
60  {
61  timeOfLastExecution = IceUtil::Time::now();
62  active = false;
63 
64  // set desired image provider
65  providerName = getProperty<std::string>("ImageProviderAdapterName").getValue();
66  usingImageProvider(providerName);
67 
68  robotStateProxyName = getProperty<std::string>("RobotStateProxyName").getValue();
69  usingProxy(robotStateProxyName);
70 
71  usingProxy(getProperty<std::string>("ObjectMemoryObserverName").getValue());
72 
73  handFrameName = getProperty<std::string>("HandFrameName").getValue();
74  cameraFrameName = getProperty<std::string>("CameraFrameName").getValue();
75 
76  minNumPixelsInClusterForCollisionDetection =
77  getProperty<int>("MinNumPixelsInClusterForCollisionDetection").getValue();
78  minNumPixelsInClusterForCollisionDetection /= clusteringSampleStep * clusteringSampleStep *
79  imageResizeFactorForOpticalFlowCalculation *
80  imageResizeFactorForOpticalFlowCalculation;
81 
82  useHandLocalization = getProperty<bool>("UseHandLocalization").getValue();
83 
84  pVisOptFlowRaw = new float[3 * DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT];
85 
86  offeringTopic("VisualContactDetection");
87  }
88 
89  void
90  VisualContactDetection::onConnectImageProcessor()
91  {
92  // connect to image provider
93  ARMARX_INFO << getName() << " connecting to " << providerName;
94  visionx::ImageProviderInfo imageProviderInfo = getImageProvider(providerName);
95  imageProviderPrx = getProxy<ImageProviderInterfacePrx>(providerName);
96 
97  cameraImages = new CByteImage*[2];
98  cameraImages[0] = tools::createByteImage(imageProviderInfo);
99  cameraImages[1] = tools::createByteImage(imageProviderInfo);
100 
101  resultImages = new CByteImage*[eNumberOfResultImages];
102 
103  for (int i = 0; i < eNumberOfResultImages; i++)
104  {
105  resultImages[i] = tools::createByteImage(imageProviderInfo);
106  //resultImages[i] = new CByteImage(3*cameraImages[0]->width, 2*cameraImages[0]->height, CByteImage::eRGB24);
107  }
108 
109  delete resultImages[eEverything];
110  resultImages[eEverything] = new CByteImage(
111  3 * cameraImages[0]->width, 2 * cameraImages[0]->height, CByteImage::eRGB24);
112 
113  camImgLeftGrey =
114  new CByteImage(cameraImages[0]->width, cameraImages[0]->height, CByteImage::eGrayScale);
115  camImgLeftGreySmall =
116  new CByteImage(cameraImages[0]->width / imageResizeFactorForOpticalFlowCalculation,
117  cameraImages[0]->height / imageResizeFactorForOpticalFlowCalculation,
118  CByteImage::eGrayScale);
119  //camImgLeftGreyOld = new CByteImage(camImgLeftGrey);
120  camImgLeftGreyOldSmall = new CByteImage(camImgLeftGreySmall);
121  tempImageRGB = new CByteImage(cameraImages[0]);
122  tempImageRGB1 = new CByteImage(cameraImages[0]);
123  tempImageRGB2 = new CByteImage(cameraImages[0]);
124  tempImageRGB3 = new CByteImage(cameraImages[0]);
125  tempImageRGB4 = new CByteImage(cameraImages[0]);
126  pSegmentedImage = new CByteImage(cameraImages[0]);
127  pOpticalFlowClusterImage = new CByteImage(cameraImages[0]);
128 
129  // retrieve stereo information
130  StereoCalibrationProviderInterfacePrx calibrationProviderPrx =
131  StereoCalibrationProviderInterfacePrx::checkedCast(imageProviderPrx);
132 
133  if (!calibrationProviderPrx)
134  {
135  ARMARX_ERROR << "Image provider with name " << providerName
136  << " is not a StereoCalibrationProvider" << std::endl;
137  return;
138  }
139 
140  stereoCalibration = visionx::tools::convert(calibrationProviderPrx->getStereoCalibration());
141 
142 
143  // connect to robot state proxy
144  ARMARX_INFO << getName() << " connecting to " << robotStateProxyName << armarx::flush;
145  robotStateProxy = getProxy<armarx::RobotStateComponentInterfacePrx>(robotStateProxyName);
146 
147  visionx::ImageDimension dim;
148  dim.width = 3 * cameraImages[0]->width;
149  dim.height = 2 * cameraImages[0]->height;
150  this->enableResultImages(1, dim, visionx::eRgb);
151 
152  // construct hand localizer
153  handLocalization = new CHandLocalisation(getProperty<int>("NumberOfParticles").getValue(),
154  2,
155  2,
156  stereoCalibration,
157  DSHT_HAND_MODEL_PATH); //6000, 2, 2
158  handLocalization->SetParticleVarianceFactor(0.3);
159  handModelVisualizer = new CHandModelVisualizer(stereoCalibration);
160  handModelVisualizer1 = new CHandModelVisualizer(stereoCalibration);
161  handModelVisualizer2 = new CHandModelVisualizer(stereoCalibration);
162  handModelVisualizer3 = new CHandModelVisualizer(stereoCalibration);
163 
164  // other member variables
165  firstRun = true;
166  oldCollisionProbability = 0;
167 
168  colors = new int[3 * (maxNumOptFlowClusters + 1)];
169 
170  for (int j = 0; j < 3 * (maxNumOptFlowClusters + 1); j++)
171  {
172  colors[j] = rand() % 220 + 30;
173  }
174 
175  // check if MemoryX is running
176  handNameInMemoryX = getProperty<std::string>("HandNameInMemoryX").getValue();
177 
178  if (this->getIceManager()->isObjectReachable(
179  getProperty<std::string>("ObjectMemoryObserverName").getValue()))
180  {
181  ARMARX_INFO_S << "Connecting to ObjectMemoryObserver";
182  objectMemoryObserver = getProxy<memoryx::ObjectMemoryObserverInterfacePrx>(
183  getProperty<std::string>("ObjectMemoryObserverName").getValue());
184  handMemoryChannel =
185  armarx::ChannelRefPtr::dynamicCast(objectMemoryObserver->requestObjectClassRepeated(
186  handNameInMemoryX, 200, armarx::DEFAULT_VIEWTARGET_PRIORITY));
187 
188  if (!handMemoryChannel)
189  {
190  ARMARX_IMPORTANT_S << "Object " << handNameInMemoryX
191  << " seems to be unknown to ObjectMemoryObserver";
192  }
193  }
194  else
195  {
196  ARMARX_INFO_S << "ObjectMemoryObserver not available";
197  objectMemoryObserver = NULL;
198  }
199 
200  listener = getTopic<VisualContactDetectionListenerPrx>("VisualContactDetection");
201  }
202 
203  void
204  VisualContactDetection::process()
205  {
206  bool doSomething = false;
207 
208  {
209  std::unique_lock lock(activationStateMutex);
210 
211  if (!waitForImages(8000))
212  {
213  ARMARX_IMPORTANT << "Timeout or error in wait for images";
214  }
215  else
216  {
217  // get images
218  int nNumberImages = getImages(cameraImages);
219  ARMARX_DEBUG << getName() << " got " << nNumberImages << " images";
220 
221  if (nNumberImages > 0 && active)
222  {
223  if ((IceUtil::Time::now() - timeOfLastExecution).toMilliSeconds() >=
224  getProperty<int>("MinWaitingTimeBetweenTwoFrames").getValue())
225  {
226  doSomething = true;
227  }
228  }
229  }
230  }
231 
232  if (doSomething)
233  {
234  IceUtil::Time startAll = IceUtil::Time::now();
235  timeOfLastExecution = IceUtil::Time::now();
236 
237  ::ImageProcessor::Zero(resultImages[eEverything]);
238 
239  omp_set_nested(true);
240 
241 #pragma omp parallel sections
242  {
243 #pragma omp section
244  {
245  IceUtil::Time start = IceUtil::Time::now();
246 
247  // localize the hand
248  localizeHand();
249 
250  ARMARX_VERBOSE << "localizeHand() took "
251  << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
252  }
253 
254 #pragma omp section
255  {
256  IceUtil::Time start = IceUtil::Time::now();
257 
258  // calculate the optical flow and cluster it
259  calculateOpticalFlowClusters();
260 
261  ARMARX_VERBOSE << "calculateOpticalFlowClusters() took "
262  << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
263  }
264  }
265 
266  // check for a cluster solely in front of the hand
267  IceUtil::Time start = IceUtil::Time::now();
268  bool collisionDetected = detectCollision();
269  listener->reportContactDetected(collisionDetected);
270  ARMARX_VERBOSE << "detectCollision() took "
271  << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
272 
273  start = IceUtil::Time::now();
274  drawVisualization(collisionDetected);
275  provideResultImages(resultImages);
276  ARMARX_VERBOSE << "drawVisualization() took "
277  << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
278  firstRun = false;
279 
280  ARMARX_VERBOSE << "Complete calculation took "
281  << (IceUtil::Time::now() - startAll).toMilliSeconds() << " ms";
282  }
283  }
284 
285  void
286  VisualContactDetection::localizeHand()
287  {
288  // get hand pose from robot state
289  armarx::PosePtr handNodePosePtr =
290  armarx::PosePtr::dynamicCast(robotStateProxy->getSynchronizedRobot()
291  ->getRobotNode(handFrameName)
292  ->getPoseInRootFrame());
293  Eigen::Matrix4f handNodePose = handNodePosePtr->toEigen();
294  armarx::PosePtr cameraNodePosePtr =
295  armarx::PosePtr::dynamicCast(robotStateProxy->getSynchronizedRobot()
296  ->getRobotNode(cameraFrameName)
297  ->getPoseInRootFrame());
298  Eigen::Matrix4f cameraNodePose = cameraNodePosePtr->toEigen();
299  Eigen::Matrix4f handPoseInCameraFrame = cameraNodePose.inverse() * handNodePose;
300 
301  // offset from TCP to palm
302  Eigen::Vector4f offsetTCPtoPalm = {0, 0, 0, 1};
303  Eigen::Vector4f palmPosition = cameraNodePose.inverse() * handNodePose * offsetTCPtoPalm;
304 
305  // // TODO: little hack for easier matching
306  // handPoseInCameraFrame(0,3) += 70;
307  // handPoseInCameraFrame(1,3) += 0;
308  // handPoseInCameraFrame(2,3) += -10;
309 
310  Vec3d tcpPosition = {
311  handPoseInCameraFrame(0, 3), handPoseInCameraFrame(1, 3), handPoseInCameraFrame(2, 3)};
312  Vec3d handNodePosition = {palmPosition(0), palmPosition(1), palmPosition(2)};
313  Mat3d handNodeOrientation = {handPoseInCameraFrame(0, 0),
314  handPoseInCameraFrame(0, 1),
315  handPoseInCameraFrame(0, 2),
316  handPoseInCameraFrame(1, 0),
317  handPoseInCameraFrame(1, 1),
318  handPoseInCameraFrame(1, 2),
319  handPoseInCameraFrame(2, 0),
320  handPoseInCameraFrame(2, 1),
321  handPoseInCameraFrame(2, 2)};
322 
323  ARMARX_VERBOSE_S << "hand pose: " << handNodePose;
324  ARMARX_VERBOSE_S << "camera pose: " << cameraNodePose;
325  ARMARX_VERBOSE_S << "c.i * h: " << handPoseInCameraFrame;
326  //ARMARX_VERBOSE_S << "c * h: " << cameraNodePose*handNodePose;
327 
328  // TODO: get finger config
329  double* fingerConfig = new double[6];
330 
331  if (false)
332  {
333  fingerConfig[0] = 30 * M_PI / 180; // 95 // palm
334  fingerConfig[1] = 5 * M_PI / 180; // 20 // thumb1
335  fingerConfig[2] = 5 * M_PI / 180; // 10 // thumb2
336  fingerConfig[3] = 5 * M_PI / 180; // 10 // index
337  fingerConfig[4] = 5 * M_PI / 180; // 5 // middle
338  fingerConfig[5] = 5 * M_PI / 180; // 5 // ring+pinky
339  }
340  else
341  {
342  armarx::SharedRobotInterfacePrx robot = robotStateProxy->getSynchronizedRobot();
343  fingerConfig[0] = 1.57f + robot->getRobotNode("Hand Palm 2 R")->getJointValue(); // palm
344  fingerConfig[1] = robot->getRobotNode("Thumb R J0")->getJointValue(); // 20 // thumb1
345  fingerConfig[2] = robot->getRobotNode("Thumb R J1")->getJointValue(); // 10 // thumb2
346  fingerConfig[3] =
347  0.5f * (robot->getRobotNode("Index R J0")->getJointValue() +
348  robot->getRobotNode("Index R J1")->getJointValue()); // 10 // index
349  fingerConfig[4] =
350  0.5f * (robot->getRobotNode("Middle R J0")->getJointValue() +
351  robot->getRobotNode("Middle R J1")->getJointValue()); // 5 // middle
352  fingerConfig[5] =
353  0.25f * (robot->getRobotNode("Ring R J0")->getJointValue() +
354  robot->getRobotNode("Ring R J1")->getJointValue() +
355  robot->getRobotNode("Pinky R J0")->getJointValue() +
356  robot->getRobotNode("Pinky R J1")->getJointValue()); // 5 // ring+pinky
357  }
358 
359 
360  // if MemoryX is available, get hand pose from there
361  if (objectMemoryObserver && handMemoryChannel)
362  {
363  memoryx::ChannelRefBaseSequence instances =
364  objectMemoryObserver->getObjectInstances(handMemoryChannel);
365 
366  if (instances.size() != 0)
367  {
368  ARMARX_VERBOSE_S << "Using hand pose from MemoryX";
369 
370  armarx::FramedPositionPtr position =
371  armarx::ChannelRefPtr::dynamicCast(instances.front())
372  ->get<armarx::FramedPosition>("position");
373  armarx::FramedOrientationPtr orientation =
374  armarx::ChannelRefPtr::dynamicCast(instances.front())
375  ->get<armarx::FramedOrientation>("orientation");
376  armarx::FramedPose handPoseMemory(orientation->toEigen(),
377  position->toEigen(),
378  position->getFrame(),
379  position->agent);
380 
381  ARMARX_VERBOSE_S << "Pose from memory: " << handPoseMemory;
382 
383  // convert to camera frame
384  if (position->getFrame().compare(cameraFrameName) != 0)
385  {
386  auto robot = robotStateProxy->getSynchronizedRobot();
387  Eigen::Matrix4f memoryNodePose =
388  armarx::PosePtr::dynamicCast(
389  robot->getRobotNode(position->getFrame())->getPoseInRootFrame())
390  ->toEigen();
391  Eigen::Matrix4f m =
392  cameraNodePose.inverse() * memoryNodePose * handPoseMemory.toEigen();
393  handPoseMemory = armarx::FramedPose(m, cameraFrameName, robot->getName());
394  }
395 
396  ARMARX_VERBOSE_S << "Pose from memory in camera coordinates: " << handPoseMemory;
397 
398  // use average of position from kinematic model and from MemoryX
399  const float ratio = 0.999f;
400  handNodePosition.x =
401  (1.0f - ratio) * handNodePosition.x + ratio * handPoseMemory.position->x;
402  handNodePosition.y =
403  (1.0f - ratio) * handNodePosition.y + ratio * handPoseMemory.position->y;
404  handNodePosition.z =
405  (1.0f - ratio) * handNodePosition.z + ratio * handPoseMemory.position->z;
406  }
407  else
408  {
409  ARMARX_VERBOSE_S << "Hand not yet localized by MemoryX";
410  }
411  }
412  else
413  {
414  ARMARX_VERBOSE_S << "objectMemoryObserver: " << objectMemoryObserver
415  << " handMemoryChannel: " << handMemoryChannel;
416  }
417 
418 
419  // debug test
420  if (false)
421  {
422  Vec3d xDirection = {100, 0, 0};
423  Vec3d yDirection = {0, 100, 0};
424  Vec3d zDirection = {0, 0, 100};
425  Math3d::MulMatVec(handNodeOrientation, xDirection, tcpPosition, xDirection);
426  Math3d::MulMatVec(handNodeOrientation, yDirection, tcpPosition, yDirection);
427  Math3d::MulMatVec(handNodeOrientation, zDirection, tcpPosition, zDirection);
428  Vec2d tcp, xAxis, yAxis, zAxis;
429  stereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
430  tcpPosition, tcp, false);
431  stereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
432  xDirection, xAxis, false);
433  stereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
434  yDirection, yAxis, false);
435  stereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
436  zDirection, zAxis, false);
437  handModelVisualizer->DrawLineIntoImage(
438  cameraImages[0], tcp.x, tcp.y, xAxis.x, xAxis.y, 255, 0, 0);
439  handModelVisualizer->DrawLineIntoImage(
440  cameraImages[0], tcp.x, tcp.y, yAxis.x, yAxis.y, 0, 255, 0);
441  handModelVisualizer->DrawLineIntoImage(
442  cameraImages[0], tcp.x, tcp.y, zAxis.x, zAxis.y, 0, 0, 255);
443  }
444 
445  // localize hand
446  if (useHandLocalization)
447  {
448  double* estimatedConfig = new double[12];
449  double confidenceRating;
450  handLocalization->LocaliseHand(cameraImages[0],
451  cameraImages[1],
452  handNodePosition,
453  handNodeOrientation,
454  fingerConfig,
455  estimatedConfig,
456  confidenceRating);
457  delete[] estimatedConfig;
458  }
459  else
460  {
461  handLocalization->SetSensorConfig(handNodePosition, handNodeOrientation, fingerConfig);
462  handLocalization->SetResultConfig(handNodePosition, handNodeOrientation, fingerConfig);
463  }
464 
465  // draw segmentation image
466  double* localizationResult = handLocalization->GetResultConfig();
467  handModelVisualizer->UpdateHandModel(localizationResult, drawComplexHandModelInResultImage);
468  std::string output = "Localization result:";
469 
470  for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
471  {
472  output += " ";
473  output += std::to_string(localizationResult[i]);
474  }
475 
476  ARMARX_VERBOSE_S << output;
477  delete[] localizationResult;
478  ::ImageProcessor::Zero(pSegmentedImage);
479  handModelVisualizer->DrawSegmentedImage(pSegmentedImage, drawComplexHandModelInResultImage);
480  }
481 
482  void
483  VisualContactDetection::calculateOpticalFlowClusters()
484  {
485  ::ImageProcessor::ConvertImage(cameraImages[0], camImgLeftGrey);
486  ::ImageProcessor::Resize(camImgLeftGrey, camImgLeftGreySmall);
487 
488  if (firstRun)
489  {
490  //::ImageProcessor::CopyImage(camImgLeftGrey, camImgLeftGreyOld);
491  ::ImageProcessor::CopyImage(camImgLeftGreySmall, camImgLeftGreyOldSmall);
492  }
493 
494  pCamLeftIpl = convertToIplImage(camImgLeftGreySmall);
495  pCamLeftOldIpl = convertToIplImage(camImgLeftGreyOldSmall);
496  cv::Mat mCamLeftMat = cv::cvarrToMat(pCamLeftIpl);
497  cv::Mat mCamLeftOldMat = cv::cvarrToMat(pCamLeftOldIpl);
498  cv::Mat mOpticalFlowMat;
499 
500  // prevImg – First 8-bit single-channel input image.
501  // nextImg – Second input image of the same size and the same type as prevImg .
502  // flow – Computed flow image that has the same size as prevImg and type CV_32FC2 .
503  // pyrScale – Parameter specifying the image scale (<1) to build pyramids for each image. pyrScale=0.5 means a classical pyramid, where each next layer is twice smaller than the previous one.
504  // levels – Number of pyramid layers including the initial image. levels=1 means that no extra layers are created and only the original images are used.
505  // winsize – Averaging window size. Larger values increase the algorithm robustness to image noise and give more chances for fast motion detection, but yield more blurred motion field.
506  // iterations – Number of iterations the algorithm does at each pyramid level.
507  // polyN – Size of the pixel neighborhood used to find polynomial expansion in each pixel. Larger values mean that the image will be approximated with smoother surfaces, yielding more robust algorithm and more blurred motion field. Typically, polyN =5 or 7.
508  // polySigma – Standard deviation of the Gaussian that is used to smooth derivatives used as a basis for the polynomial expansion. For polyN=5 , you can set polySigma=1.1 . For polyN=7 , a good value would be polySigma=1.5 .
509  // flags –
510  // Operation flags that can be a combination of the following:
511  // OPTFLOW_USE_INITIAL_FLOW Use the input flow as an initial flow approximation.
512  // OPTFLOW_FARNEBACK_GAUSSIAN Use the Gaussian \texttt{winsize}\times\texttt{winsize} filter instead of a box filter of the same size for optical flow estimation. Usually, this option gives z more accurate flow than with a box filter, at the cost of lower speed. Normally, winsize for a Gaussian window should be set to a larger value to achieve the same level of robustness.
513 
514  cv::calcOpticalFlowFarneback(mCamLeftMat,
515  mCamLeftOldMat,
516  mOpticalFlowMat,
517  0.5,
518  5,
519  20,
520  7,
521  7,
522  1.5,
523  0); // 0.5, 5, 30, 10, 7, 1.5, 0
524 
525  cv::Mat mVisOptFlow(mOpticalFlowMat.size(), CV_32FC3);
526 
527 
528 #pragma omp parallel for schedule(static, 40)
529  for (int j = 0; j < mOpticalFlowMat.rows; j++)
530  {
531  const int l = imageResizeFactorForOpticalFlowCalculation * j;
532 
533  for (int k = 0, m = 0; k < mOpticalFlowMat.cols;
534  k++, m += imageResizeFactorForOpticalFlowCalculation)
535  {
536  mVisOptFlow.at<cv::Vec3f>(j, k).val[0] =
537  0.4 * mOpticalFlowMat.at<cv::Vec2f>(j, k).val[0];
538  mVisOptFlow.at<cv::Vec3f>(j, k).val[1] =
539  0.4 * mOpticalFlowMat.at<cv::Vec2f>(j, k).val[1];
540  mVisOptFlow.at<cv::Vec3f>(j, k).val[2] = 0;
541 
542  for (int n = 0; n < imageResizeFactorForOpticalFlowCalculation; n++)
543  {
544  for (int o = 0; o < imageResizeFactorForOpticalFlowCalculation; o++)
545  {
546  pVisOptFlowRaw[3 * DSHT_IMAGE_WIDTH * (l + n) + 3 * (m + o)] =
547  0.5f * (mVisOptFlow.at<cv::Vec3f>(j, k).val[2] + 1);
548  pVisOptFlowRaw[3 * DSHT_IMAGE_WIDTH * (l + n) + 3 * (m + o) + 1] =
549  0.5f * (mVisOptFlow.at<cv::Vec3f>(j, k).val[1] + 1);
550  pVisOptFlowRaw[3 * DSHT_IMAGE_WIDTH * (l + n) + 3 * (m + o) + 2] =
551  0.5f * (mVisOptFlow.at<cv::Vec3f>(j, k).val[0] + 1);
552  }
553  }
554  }
555  }
556 
557 
558  // clustering by position and direction
559  std::vector<std::vector<float>> aPixelsAndFlowDirections;
560  std::vector<float> aPixelPosAndFlowDirection;
561  aPixelPosAndFlowDirection.resize(4);
562 
563  for (int j = 0; j < mOpticalFlowMat.rows; j += clusteringSampleStep)
564  {
565  for (int k = 0; k < mOpticalFlowMat.cols; k += clusteringSampleStep)
566  {
567  aPixelPosAndFlowDirection.at(0) = imageResizeFactorForOpticalFlowCalculation * j;
568  aPixelPosAndFlowDirection.at(1) = imageResizeFactorForOpticalFlowCalculation * k;
569  aPixelPosAndFlowDirection.at(2) = 4000 * mOpticalFlowMat.at<cv::Vec2f>(j, k).val[0];
570  aPixelPosAndFlowDirection.at(3) = 4000 * mOpticalFlowMat.at<cv::Vec2f>(j, k).val[1];
571 
572  aPixelsAndFlowDirections.push_back(aPixelPosAndFlowDirection);
573  }
574  }
575 
576  // remove points with zero motion
577  std::vector<std::vector<float>> aZeroMotionCluster;
578 
579  for (size_t j = 0; j < aPixelsAndFlowDirections.size(); j++)
580  {
581  float fAbsMotion = fabs(aPixelsAndFlowDirections.at(j).at(2)) +
582  fabs(aPixelsAndFlowDirections.at(j).at(3));
583 
584  if (fAbsMotion < 350) // 350
585  {
586  aZeroMotionCluster.push_back(aPixelsAndFlowDirections.at(j));
587  aPixelsAndFlowDirections.at(j) =
588  aPixelsAndFlowDirections.at(aPixelsAndFlowDirections.size() - 1);
589  aPixelsAndFlowDirections.pop_back();
590  j--;
591  }
592  }
593 
594  std::vector<std::vector<int>> aOldIndices;
595  const float fBIC = 0.08; // bigger = more clusters (default: 0.05)
596  clusterXMeans(aPixelsAndFlowDirections,
597  1,
598  maxNumOptFlowClusters,
599  fBIC,
600  opticalFlowClusters,
601  aOldIndices);
602 
603  opticalFlowClusters.push_back(aZeroMotionCluster);
604 
605  ARMARX_VERBOSE_S << opticalFlowClusters.size() << " clusters" << armarx::flush;
606 
607  //::ImageProcessor::CopyImage(camImgLeftGrey, camImgLeftGreyOld);
608  ::ImageProcessor::CopyImage(camImgLeftGreySmall, camImgLeftGreyOldSmall);
609  }
610 
611  bool
612  VisualContactDetection::detectCollision()
613  {
614  //************************************************************************************************************
615  // Determine push direction and area in front of the hand that will be analyzed
616  //************************************************************************************************************
617 
618 
619  Eigen::Matrix4f handNodePose =
620  armarx::PosePtr::dynamicCast(robotStateProxy->getSynchronizedRobot()
621  ->getRobotNode(handFrameName)
622  ->getPoseInRootFrame())
623  ->toEigen();
624  Vec3d handPosSensor = {handNodePose(0, 3), handNodePose(1, 3), handNodePose(2, 3)};
625 
626  if (firstRun)
627  {
628  Math3d::SetVec(oldHandPosSensor, handPosSensor);
629  }
630 
631 
632  Vec3d pushDirection, pushDirectionNormalized;
633  //Math3d::SubtractVecVec(handPosPF, oldHandPosPF, pushDirection);
634  Math3d::SubtractVecVec(handPosSensor, oldHandPosSensor, pushDirection);
635  Math3d::SetVec(pushDirectionNormalized, pushDirection);
636 
637  ARMARX_INFO << "Push direction: " << pushDirection.x << " " << pushDirection.y << " "
638  << pushDirection.z;
639 
640  if (Math3d::Length(pushDirection) > 0)
641  {
642  Math3d::NormalizeVec(pushDirectionNormalized);
643  Math3d::MulVecScalar(pushDirection,
644  pushDetectionBoxForwardOffsetToTCP / Math3d::Length(pushDirection),
645  pushDirection);
646  }
647 
648  // check if hand is moving forward
649  Eigen::Vector3f forwardAxis;
650  forwardAxis << 0.0f, 1.0f, 1.0f; // -1, 0, 1
651  forwardAxis.normalize();
652  forwardAxis = handNodePose.block<3, 3>(0, 0) * forwardAxis;
653  Vec3d forwardDirection = {forwardAxis(0), forwardAxis(1), forwardAxis(2)};
654  ARMARX_INFO << "Angle to forward direction: "
655  << Math3d::ScalarProduct(forwardDirection, pushDirectionNormalized);
656 
657  // // just for testing!!
658  // pushDirectionNormalized = forwardDirection;
659  // Math3d::MulVecScalar(forwardDirection, pushDetectionBoxForwardOffsetToTCP, pushDirection);
660 
661  if ((Math3d::ScalarProduct(forwardDirection, pushDirectionNormalized) < -0.6f) ||
662  (Math3d::Length(pushDirection) == 0))
663  {
664  ARMARX_INFO << "Not checking for collision because arm is not moving forward";
665  handPos2D.x = -1;
666  handPos2D.y = -1;
667  return false;
668  }
669 
670  // create box
671 
672  Eigen::Vector3f pushDirectionForBox = {pushDirection.x, pushDirection.y, pushDirection.z};
673  forwardAxis *= 1.0f * pushDetectionBoxForwardOffsetToTCP;
674  pushDirectionForBox = 0.5f * (pushDirectionForBox + forwardAxis);
675  Eigen::Matrix4f cameraNodePose =
676  armarx::PosePtr::dynamicCast(robotStateProxy->getSynchronizedRobot()
677  ->getRobotNode(cameraFrameName)
678  ->getPoseInRootFrame())
679  ->toEigen();
680  Eigen::Vector3f pushDirectionInCameraCoords =
681  cameraNodePose.block<3, 3>(0, 0) * pushDirectionForBox;
682  Eigen::Matrix4f handPosePF = handLocalization->GetHandPose();
683  Vec3d handPosPF = {handPosePF(0, 3), handPosePF(1, 3), handPosePF(2, 3)};
684  Vec3d pushTarget = {handPosPF.x + pushDirectionInCameraCoords(0),
685  handPosPF.y + pushDirectionInCameraCoords(1),
686  handPosPF.z + pushDirectionInCameraCoords(2)};
687 
688  stereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
689  handPosPF, handPos2D, false);
690  stereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
691  pushTarget, pushTarget2D, false);
692 
693  const float handCameraDistance =
694  (handNodePose.block<3, 1>(0, 3) - cameraNodePose.block<3, 1>(0, 3)).norm();
695  const float boxSize = 80.0f;
696  Vec2d pushTargetBoxSize = {boxSize * 600 / handCameraDistance,
697  boxSize * 600 / handCameraDistance};
698  ARMARX_INFO << "pushTargetBoxSize: " << pushTargetBoxSize.x;
699 
700  Math2d::SetVec(pushTargetBoxLeftUpperCorner,
701  pushTarget2D.x - pushTargetBoxSize.x,
702  pushTarget2D.y - pushTargetBoxSize.y);
703  Math2d::SetVec(pushTargetBoxRightLowerCorner,
704  pushTarget2D.x + pushTargetBoxSize.x,
705  pushTarget2D.y + pushTargetBoxSize.y);
706 
707  if (pushTargetBoxLeftUpperCorner.x < 0)
708  {
709  pushTargetBoxLeftUpperCorner.x = 0;
710  }
711 
712  if (pushTargetBoxLeftUpperCorner.y < 0)
713  {
714  pushTargetBoxLeftUpperCorner.y = 0;
715  }
716 
717  if (pushTargetBoxRightLowerCorner.x > DSHT_IMAGE_WIDTH - 1)
718  {
719  pushTargetBoxRightLowerCorner.x = DSHT_IMAGE_WIDTH - 1;
720  }
721 
722  if (pushTargetBoxRightLowerCorner.y > DSHT_IMAGE_HEIGHT - 1)
723  {
724  pushTargetBoxRightLowerCorner.y = DSHT_IMAGE_HEIGHT - 1;
725  }
726 
727  Math3d::SetVec(oldHandPosSensor, handPosSensor);
728 
729 
730  //************************************************************************************************************
731  // Visual contact detection
732  // Check if one of the clusters of optical flow is within the rectangle in the area where the hand
733  // pushes towards, but not in the rest of the image (except where the hand and arm are).
734  //************************************************************************************************************
735 
736  bool bContact = false;
737 
738  std::vector<int> aNumClusterPointsInPushingBox, aNumClusterPointsInHandArea,
739  aNumClusterPointsInRestOfImage;
740 
741  for (size_t j = 0; j < opticalFlowClusters.size(); j++)
742  {
743  aNumClusterPointsInPushingBox.push_back(0);
744  aNumClusterPointsInHandArea.push_back(0);
745  aNumClusterPointsInRestOfImage.push_back(0);
746  }
747 
748  for (size_t j = 0; j < opticalFlowClusters.size(); j++)
749  {
750  for (size_t k = 0; k < opticalFlowClusters.at(j).size(); k++)
751  {
752  const float y = opticalFlowClusters.at(j).at(k).at(0);
753  const float x = opticalFlowClusters.at(j).at(k).at(1);
754  int nIndex = DSHT_IMAGE_WIDTH * y + x;
755 
756  if (0 <= nIndex && nIndex < DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT)
757  {
758  if (pSegmentedImage->pixels[3 * nIndex])
759  {
760  aNumClusterPointsInHandArea.at(j)++;
761  }
762  else if (pushTargetBoxLeftUpperCorner.x <= x &&
763  x <= pushTargetBoxRightLowerCorner.x &&
764  pushTargetBoxLeftUpperCorner.y <= y &&
765  y <= pushTargetBoxRightLowerCorner.y)
766  {
767  aNumClusterPointsInPushingBox.at(j)++;
768  }
769  else
770  {
771  aNumClusterPointsInRestOfImage.at(j)++;
772  }
773  }
774  else
775  {
776  aNumClusterPointsInRestOfImage.at(j)++;
777  }
778  }
779  }
780 
781  float fBestRatio = 0;
782  int nBestIndex = -1;
783 
784  for (size_t j = 0; j < opticalFlowClusters.size(); j++)
785  {
786  if (aNumClusterPointsInPushingBox.at(j) > minNumPixelsInClusterForCollisionDetection)
787  {
788  float fRatio = (float)aNumClusterPointsInPushingBox.at(j) /
789  (float)(aNumClusterPointsInPushingBox.at(j) +
790  aNumClusterPointsInRestOfImage.at(j));
791 
792  if (fRatio > fBestRatio)
793  {
794  fBestRatio = fRatio;
795  nBestIndex = j;
796  }
797  }
798  }
799 
800  ARMARX_LOG << "Contact probability: " << fBestRatio;
801 
802  bContact = (fBestRatio > 0.5f && oldCollisionProbability > 0.5);
803  oldCollisionProbability = fBestRatio;
804 
805  if (bContact)
806  {
807 
808  ARMARX_LOG << "Region size: "
809  << clusteringSampleStep * clusteringSampleStep *
810  imageResizeFactorForOpticalFlowCalculation *
811  imageResizeFactorForOpticalFlowCalculation *
812  aNumClusterPointsInPushingBox.at(nBestIndex);
813  }
814 
815  return bContact;
816  }
817 
818  void
819  VisualContactDetection::drawVisualization(bool collisionDetected)
820  {
821  timesOfImageCapture.push_back(IceUtil::Time::now().toMilliSeconds());
822 
823 #pragma omp parallel sections
824  {
825 
826 #pragma omp section
827  {
828  if (recordImages)
829  {
830  CByteImage* pNew = new CByteImage(cameraImages[0]);
831  ::ImageProcessor::CopyImage(cameraImages[0], pNew);
832  cameraImagesForSaving.push_back(pNew);
833  }
834  }
835 
836 
837  // hand localization
838 
839 #pragma omp section
840  {
841  double* sensorConfig = handLocalization->GetSensorConfig();
842  handModelVisualizer1->UpdateHandModel(sensorConfig,
843  drawComplexHandModelInResultImage);
844  delete[] sensorConfig;
845  ::ImageProcessor::CopyImage(cameraImages[0], tempImageRGB1);
846  handModelVisualizer1->DrawHandModelV2(tempImageRGB1);
847 
848  for (int i = 0; i < cameraImages[0]->height; i++)
849  {
850  for (int j = 0; j < cameraImages[0]->width; j++)
851  {
852  resultImages[eEverything]
853  ->pixels[3 * (i * 3 * cameraImages[0]->width + j)] =
854  tempImageRGB1->pixels[3 * (i * cameraImages[0]->width + j)];
855  resultImages[eEverything]
856  ->pixels[3 * (i * 3 * cameraImages[0]->width + j) + 1] =
857  tempImageRGB1->pixels[3 * (i * cameraImages[0]->width + j) + 1];
858  resultImages[eEverything]
859  ->pixels[3 * (i * 3 * cameraImages[0]->width + j) + 2] =
860  tempImageRGB1->pixels[3 * (i * cameraImages[0]->width + j) + 2];
861  }
862  }
863  }
864 
865 
866 #pragma omp section
867  {
868  double* localizationResult = handLocalization->GetResultConfig();
869  handModelVisualizer2->UpdateHandModel(localizationResult,
870  drawComplexHandModelInResultImage);
871  delete[] localizationResult;
872  ::ImageProcessor::CopyImage(cameraImages[0], tempImageRGB2);
873  handModelVisualizer2->DrawHandModelV2(tempImageRGB2, true);
874 
875  for (int i = 0; i < cameraImages[0]->height; i++)
876  {
877  for (int j = 0, j2 = cameraImages[0]->width; j < cameraImages[0]->width;
878  j++, j2++)
879  {
880  resultImages[eEverything]
881  ->pixels[3 * (i * 3 * cameraImages[0]->width + j2)] =
882  tempImageRGB2->pixels[3 * (i * cameraImages[0]->width + j)];
883  resultImages[eEverything]
884  ->pixels[3 * (i * 3 * cameraImages[0]->width + j2) + 1] =
885  tempImageRGB2->pixels[3 * (i * cameraImages[0]->width + j) + 1];
886  resultImages[eEverything]
887  ->pixels[3 * (i * 3 * cameraImages[0]->width + j2) + 2] =
888  tempImageRGB2->pixels[3 * (i * cameraImages[0]->width + j) + 2];
889  }
890  }
891 
892  if (recordImages)
893  {
894  CByteImage* pNew = new CByteImage(tempImageRGB2);
895  ::ImageProcessor::CopyImage(tempImageRGB2, pNew);
896  localizationResultImages.push_back(pNew);
897  }
898  }
899 
900 
901 #pragma omp section
902  {
903  double* localizationResult = handLocalization->GetResultConfig();
904  handModelVisualizer3->UpdateHandModel(localizationResult,
905  drawComplexHandModelInResultImage);
906  delete[] localizationResult;
907  ::ImageProcessor::CopyImage(cameraImages[1], tempImageRGB3);
908  handModelVisualizer3->DrawHandModelV2(tempImageRGB3, false);
909 
910  for (int i = 0; i < cameraImages[0]->height; i++)
911  {
912  for (int j = 0, j2 = 2 * cameraImages[0]->width; j < cameraImages[0]->width;
913  j++, j2++)
914  {
915  resultImages[eEverything]
916  ->pixels[3 * (i * 3 * cameraImages[0]->width + j2)] =
917  tempImageRGB3->pixels[3 * (i * cameraImages[0]->width + j)];
918  resultImages[eEverything]
919  ->pixels[3 * (i * 3 * cameraImages[0]->width + j2) + 1] =
920  tempImageRGB3->pixels[3 * (i * cameraImages[0]->width + j) + 1];
921  resultImages[eEverything]
922  ->pixels[3 * (i * 3 * cameraImages[0]->width + j2) + 2] =
923  tempImageRGB3->pixels[3 * (i * cameraImages[0]->width + j) + 2];
924  }
925  }
926  }
927 
928 
929 #pragma omp section
930  {
931  for (int i = 0, i2 = cameraImages[0]->height; i < cameraImages[0]->height;
932  i++, i2++)
933  {
934  for (int j = 0; j < cameraImages[0]->width; j++)
935  {
936  resultImages[eEverything]
937  ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j)] =
938  pSegmentedImage->pixels[3 * (i * cameraImages[0]->width + j)];
939  resultImages[eEverything]
940  ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j) + 1] =
941  pSegmentedImage->pixels[3 * (i * cameraImages[0]->width + j) + 1];
942  resultImages[eEverything]
943  ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j) + 2] =
944  pSegmentedImage->pixels[3 * (i * cameraImages[0]->width + j) + 2];
945  }
946  }
947  }
948 
949 
950  // optical flow
951 
952 #pragma omp section
953  {
954  ::ImageProcessor::Zero(tempImageRGB4);
955 
956  // const int step = 2*clusteringSampleStep*imageResizeFactorForOpticalFlowCalculation;
957  // for (int j=0; j<DSHT_IMAGE_HEIGHT; j+=step)
958  // {
959  // for (int i=0; i<DSHT_IMAGE_WIDTH; i+=step)
960  // {
961  // const int flowX = 5*step*(pVisOptFlowRaw[3*(j*DSHT_IMAGE_WIDTH+i)+2]-0.5);
962  // const int flowY = 5*step*(pVisOptFlowRaw[3*(j*DSHT_IMAGE_WIDTH+i)+1]-0.5);
963  // handModelVisualizer->DrawLineIntoImage(tempImageRGB4, i, j, i+flowX, j+flowY, 255, 255, 255);
964  // }
965  // }
966 
967  for (int j = 0; j < 3 * DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT; j++)
968  {
969  int nValue = 255 * pVisOptFlowRaw[j];
970  //resultImages[eOpticalFlow]->pixels[j] = (0 > nValue) ? 0 : ((255 < nValue) ? 255 : nValue);
971  tempImageRGB4->pixels[j] = (0 > nValue) ? 0 : ((255 < nValue) ? 255 : nValue);
972  }
973 
974  for (int i = 0, i2 = cameraImages[0]->height; i < cameraImages[0]->height;
975  i++, i2++)
976  {
977  for (int j = 0, j2 = cameraImages[0]->width; j < cameraImages[0]->width;
978  j++, j2++)
979  {
980  resultImages[eEverything]
981  ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j2)] =
982  tempImageRGB4->pixels[3 * (i * cameraImages[0]->width + j)];
983  resultImages[eEverything]
984  ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j2) + 1] =
985  tempImageRGB4->pixels[3 * (i * cameraImages[0]->width + j) + 1];
986  resultImages[eEverything]
987  ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j2) + 2] =
988  tempImageRGB4->pixels[3 * (i * cameraImages[0]->width + j) + 2];
989  }
990  }
991 
992  if (recordImages)
993  {
994  CByteImage* pNew = new CByteImage(tempImageRGB4);
995  ::ImageProcessor::CopyImage(tempImageRGB4, pNew);
996  opticalFlowImages.push_back(pNew);
997  }
998  }
999 
1000 
1001 #pragma omp section
1002  {
1003  ::ImageProcessor::Zero(pOpticalFlowClusterImage);
1004 
1005  for (size_t j = 0; j < opticalFlowClusters.size(); j++)
1006  {
1007  ARMARX_VERBOSE_S << "Cluster " << j << ": " << opticalFlowClusters.at(j).size()
1008  << " points" << armarx::flush;
1009 
1010  for (size_t k = 0; k < opticalFlowClusters.at(j).size(); k++)
1011  {
1012  int nIndex = DSHT_IMAGE_WIDTH * opticalFlowClusters.at(j).at(k).at(0) +
1013  opticalFlowClusters.at(j).at(k).at(1);
1014 
1015  if (0 <= nIndex && nIndex < DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT)
1016  {
1017  for (int l = 0; l < imageResizeFactorForOpticalFlowCalculation *
1018  clusteringSampleStep;
1019  l++)
1020  {
1021  for (int m = 0; m < imageResizeFactorForOpticalFlowCalculation *
1022  clusteringSampleStep;
1023  m++)
1024  {
1025  int nIndex2 = nIndex + l * DSHT_IMAGE_WIDTH + m;
1026 
1027  if (nIndex2 < DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT)
1028  {
1029  pOpticalFlowClusterImage->pixels[3 * nIndex2] =
1030  colors[3 * j];
1031  pOpticalFlowClusterImage->pixels[3 * nIndex2 + 1] =
1032  colors[3 * j + 1];
1033  pOpticalFlowClusterImage->pixels[3 * nIndex2 + 2] =
1034  colors[3 * j + 2];
1035  }
1036  }
1037  }
1038  }
1039  }
1040  }
1041 
1042  // collision detection
1043 
1044  if (handPos2D.x != -1 || handPos2D.y != -1)
1045  {
1046  for (int j = 0; j < 3 * DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT; j++)
1047  {
1048  if (pSegmentedImage->pixels[j])
1049  {
1050  pOpticalFlowClusterImage->pixels[j] /= 2;
1051  }
1052  }
1053 
1054  handModelVisualizer->DrawCross(
1055  pOpticalFlowClusterImage, handPos2D.x, handPos2D.y, 255, 255, 255);
1056  int n = collisionDetected ? 8 : 2;
1057 
1058  for (int i = 0; i < n; i++)
1059  {
1060  handModelVisualizer->DrawLineIntoImage(pOpticalFlowClusterImage,
1061  pushTargetBoxLeftUpperCorner.x + i,
1062  pushTargetBoxLeftUpperCorner.y + i,
1063  pushTargetBoxRightLowerCorner.x - i,
1064  pushTargetBoxLeftUpperCorner.y + i,
1065  255,
1066  255,
1067  255);
1068  handModelVisualizer->DrawLineIntoImage(pOpticalFlowClusterImage,
1069  pushTargetBoxRightLowerCorner.x - i,
1070  pushTargetBoxLeftUpperCorner.y + 1,
1071  pushTargetBoxRightLowerCorner.x - i,
1072  pushTargetBoxRightLowerCorner.y - i,
1073  255,
1074  255,
1075  255);
1076  handModelVisualizer->DrawLineIntoImage(pOpticalFlowClusterImage,
1077  pushTargetBoxRightLowerCorner.x - i,
1078  pushTargetBoxRightLowerCorner.y - i,
1079  pushTargetBoxLeftUpperCorner.x + i,
1080  pushTargetBoxRightLowerCorner.y - i,
1081  255,
1082  255,
1083  255);
1084  handModelVisualizer->DrawLineIntoImage(pOpticalFlowClusterImage,
1085  pushTargetBoxLeftUpperCorner.x + i,
1086  pushTargetBoxRightLowerCorner.y - i,
1087  pushTargetBoxLeftUpperCorner.x + i,
1088  pushTargetBoxLeftUpperCorner.y + i,
1089  255,
1090  255,
1091  255);
1092  }
1093 
1094  for (int i = 0, i2 = cameraImages[0]->height; i < cameraImages[0]->height;
1095  i++, i2++)
1096  {
1097  for (int j = 0, j2 = 2 * cameraImages[0]->width; j < cameraImages[0]->width;
1098  j++, j2++)
1099  {
1100  resultImages[eEverything]
1101  ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j2)] =
1102  pOpticalFlowClusterImage
1103  ->pixels[3 * (i * cameraImages[0]->width + j)];
1104  resultImages[eEverything]
1105  ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j2) + 1] =
1106  pOpticalFlowClusterImage
1107  ->pixels[3 * (i * cameraImages[0]->width + j) + 1];
1108  resultImages[eEverything]
1109  ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j2) + 2] =
1110  pOpticalFlowClusterImage
1111  ->pixels[3 * (i * cameraImages[0]->width + j) + 2];
1112  }
1113  }
1114  }
1115 
1116  if (recordImages)
1117  {
1118  CByteImage* pNew = new CByteImage(pOpticalFlowClusterImage);
1119  ::ImageProcessor::CopyImage(pOpticalFlowClusterImage, pNew);
1120  opticalFlowClusterImages.push_back(pNew);
1121  }
1122  }
1123  }
1124  }
1125 
1126  void
1127  VisualContactDetection::onExitImageProcessor()
1128  {
1129  delete[] cameraImages;
1130 
1131  if (recordImages)
1132  {
1133  ARMARX_INFO << "Writing all visualization images to disk...";
1134  std::string path = "/localhome/ottenhau/VisColDet/";
1135  std::string fileNameCam = path + "cam0000.bmp";
1136  std::string fileNameLocRes = path + "locres0000.bmp";
1137  std::string fileNameOptFlow = path + "optflow0000.bmp";
1138  std::string fileNameOptFlowClus = path + "optflowclus0000.bmp";
1139  std::string fileNameTimes = path + "times.txt";
1140  FILE* pFile = fopen(fileNameTimes.c_str(), "wt");
1141  fprintf(pFile, "%ld \n", timesOfImageCapture.size() - 1);
1142 
1143  for (long i = 0; i < (long)timesOfImageCapture.size() - 1; i++)
1144  {
1145  fprintf(pFile, "%ld %ld \n", i, timesOfImageCapture.at(i));
1146  SetNumberInFileName(fileNameCam, i, 4);
1147  SetNumberInFileName(fileNameLocRes, i, 4);
1148  SetNumberInFileName(fileNameOptFlow, i, 4);
1149  SetNumberInFileName(fileNameOptFlowClus, i, 4);
1150  cameraImagesForSaving.at(i)->SaveToFile(fileNameCam.c_str());
1151  localizationResultImages.at(i)->SaveToFile(fileNameLocRes.c_str());
1152  opticalFlowImages.at(i)->SaveToFile(fileNameOptFlow.c_str());
1153  opticalFlowClusterImages.at(i)->SaveToFile(fileNameOptFlowClus.c_str());
1154  delete cameraImagesForSaving.at(i);
1155  delete localizationResultImages.at(i);
1156  delete opticalFlowImages.at(i);
1157  delete opticalFlowClusterImages.at(i);
1158 
1159  if (i % 20 == 0)
1160  {
1161  ARMARX_VERBOSE << "Image " << i << " of " << timesOfImageCapture.size();
1162  }
1163  }
1164 
1165  fclose(pFile);
1166  ARMARX_INFO << "Finished writing all visualization images to disk";
1167  }
1168  }
1169 
1170  void
1171  VisualContactDetection::extractAnglesFromRotationMatrix(const Mat3d& mat, Vec3d& angles)
1172  {
1173  angles.y = asin(mat.r3);
1174  angles.x = atan2((-mat.r6), (mat.r9));
1175  angles.z = atan2((-mat.r2), (mat.r1));
1176  }
1177 
1178  IplImage*
1179  VisualContactDetection::convertToIplImage(CByteImage* pByteImageRGB)
1180  {
1181  if (pByteImageRGB->type == CByteImage::eRGB24)
1182  {
1183  unsigned char cTemp;
1184 
1185  for (int j = 0; j < pByteImageRGB->width * pByteImageRGB->height; j++)
1186  {
1187  cTemp = pByteImageRGB->pixels[3 * j];
1188  pByteImageRGB->pixels[3 * j] = pByteImageRGB->pixels[3 * j + 2];
1189  pByteImageRGB->pixels[3 * j + 2] = cTemp;
1190  }
1191  }
1192 
1193  IplImage* pRet = IplImageAdaptor::Adapt(pByteImageRGB);
1194  return pRet;
1195  }
1196 
1197  void
1198  VisualContactDetection::clusterXMeans(
1199  const std::vector<std::vector<float>>& aPoints,
1200  const int nMinNumClusters,
1201  const int nMaxNumClusters,
1202  const float fBICFactor,
1203  std::vector<std::vector<std::vector<float>>>& aaPointClusters,
1204  std::vector<std::vector<int>>& aaOldIndices)
1205  {
1206  aaPointClusters.clear();
1207  aaOldIndices.clear();
1208  const int nNumberOfSamples = aPoints.size();
1209 
1210  if (nNumberOfSamples < nMaxNumClusters)
1211  {
1212  ARMARX_IMPORTANT_S << "Not enough points for clustering (only " << nNumberOfSamples
1213  << " points)" << armarx::flush;
1214  return;
1215  }
1216 
1217  ARMARX_VERBOSE_S << "Number of points: " << nNumberOfSamples;
1218 
1219 
1220  cv::Mat mSamples;
1221  const int nNumberOfDifferentInitialisations = 4;
1222  cv::TermCriteria tTerminationCriteria(
1223  cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, 0.01);
1224 
1225  // copy the points
1226  const int nDimension = aPoints.at(0).size();
1227  mSamples.create(nNumberOfSamples, nDimension, CV_32FC1);
1228 
1229  for (int i = 0; i < nNumberOfSamples; i++)
1230  {
1231  for (int j = 0; j < nDimension; j++)
1232  {
1233  mSamples.at<float>(i, j) = aPoints.at(i).at(j);
1234  }
1235  }
1236 
1237  std::vector<std::vector<std::vector<float>>>* aaPointClustersForAllK =
1238  new std::vector<std::vector<std::vector<float>>>[nMaxNumClusters + 1];
1239  std::vector<std::vector<int>>* aaOldIndicesForAllK =
1240  new std::vector<std::vector<int>>[nMaxNumClusters + 1];
1241 
1242 
1243  // execute k-means for several values of k and find the value for k that minimizes the
1244  // Bayesian Information Criterion (BIC)
1245  double dMinBIC = 10000000;
1246  int nOptK = nMinNumClusters;
1247 
1248 #pragma omp parallel for schedule(dynamic, 1)
1249  for (int k = nMaxNumClusters; k >= nMinNumClusters; k--)
1250  {
1251  double dKMeansCompactness, dLogVar, dBIC;
1252  cv::Mat mClusterLabelsLocal;
1253  mClusterLabelsLocal.create(nNumberOfSamples, 1, CV_32SC1);
1254  cv::Mat mClusterCentersLocal; // = NULL;
1255  dKMeansCompactness = cv::kmeans(mSamples,
1256  k,
1257  mClusterLabelsLocal,
1258  tTerminationCriteria,
1259  nNumberOfDifferentInitialisations,
1260  cv::KMEANS_RANDOM_CENTERS,
1261  mClusterCentersLocal);
1262 
1263  const int nNumberOfFreeParameters = (k - 1) + (nDimension * k) + k;
1264  dLogVar = log(dKMeansCompactness);
1265  dBIC = fBICFactor * 0.35 * dLogVar +
1266  ((double)nNumberOfFreeParameters / (double)nNumberOfSamples) *
1267  log((double)nNumberOfSamples);
1268 
1269 #pragma omp critical
1270  if (dBIC < dMinBIC)
1271  {
1272  dMinBIC = dBIC;
1273  nOptK = k;
1274  }
1275 
1276  if (dBIC == dMinBIC)
1277  {
1278  std::vector<float> vPoint;
1279  vPoint.resize(nDimension);
1280 
1281  for (int i = 0; i < k; i++)
1282  {
1283  std::vector<std::vector<float>> aNewCluster;
1284  aaPointClustersForAllK[k].push_back(aNewCluster);
1285  std::vector<int> aClusterIndices;
1286  aaOldIndicesForAllK[k].push_back(aClusterIndices);
1287  }
1288 
1289  for (int i = 0; i < nNumberOfSamples; i++)
1290  {
1291  const int nLabel = mClusterLabelsLocal.at<int>(i, 0);
1292 
1293  if ((nLabel >= 0) && (nLabel < k))
1294  {
1295  for (int j = 0; j < nDimension; j++)
1296  {
1297  vPoint.at(j) = mSamples.at<float>(i, j);
1298  }
1299 
1300  aaPointClustersForAllK[k].at(nLabel).push_back(vPoint);
1301  aaOldIndicesForAllK[k].at(nLabel).push_back(i);
1302  }
1303 
1304  //else
1305  //{
1306  // ARMARX_WARNING_S << "Invalid cluster label: " << nLabel << "\n nOptK: " << nOptK << ", i: " << i << ", nNumberOfSamples: " << nNumberOfSamples << armarx::flush;
1307  // break;
1308  //}
1309  }
1310  }
1311 
1312  //ARMARX_VERBOSE_S << "k-means with " << i << " clusters. log(var): " << dLogVar << " BIC: " << dBIC;
1313  }
1314 
1315  // return results with best k
1316  aaPointClusters = aaPointClustersForAllK[nOptK];
1317  aaOldIndices = aaOldIndicesForAllK[nOptK];
1318 
1319  delete[] aaPointClustersForAllK;
1320  delete[] aaOldIndicesForAllK;
1321  }
1322 
1323  armarx::FramedPoseBasePtr
1324  VisualContactDetection::getHandPose(const Ice::Current& c)
1325  {
1326  Eigen::Matrix4f handPose = handLocalization->GetHandPose();
1327  armarx::FramedPosePtr ret = new armarx::FramedPose(
1328  handPose, cameraFrameName, robotStateProxy->getSynchronizedRobot()->getName());
1329  return ret;
1330  }
1331 
1332  visionx::FramedPositionBaseList
1333  VisualContactDetection::getFingertipPositions(const Ice::Current& c)
1334  {
1335  visionx::FramedPositionBaseList ret;
1336  std::vector<Vec3d> fingertipPositions = handLocalization->GetFingertipPositions();
1337 
1338  for (size_t i = 0; i < fingertipPositions.size(); i++)
1339  {
1340  Eigen::Vector3f position;
1341  position << fingertipPositions.at(i).x, fingertipPositions.at(i).y,
1342  fingertipPositions.at(i).z;
1343  armarx::FramedPositionPtr pos = new armarx::FramedPosition(
1344  position, cameraFrameName, robotStateProxy->getSynchronizedRobot()->getName());
1345  ret.push_back(pos);
1346  }
1347 
1348  return ret;
1349  }
1350 
1351  void
1352  VisualContactDetection::activate(const Ice::Current& c)
1353  {
1354  std::unique_lock lock(activationStateMutex);
1355 
1356  if (!waitForImages(1000))
1357  {
1358  ARMARX_WARNING << "Timeout or error in wait for images";
1359  }
1360  else
1361  {
1362  // get images
1363  int nNumberImages = getImages(cameraImages);
1364  ARMARX_VERBOSE << getName() << " got " << nNumberImages << " images";
1365  ::ImageProcessor::ConvertImage(cameraImages[0], camImgLeftGrey);
1366  ::ImageProcessor::Resize(camImgLeftGrey, camImgLeftGreyOldSmall);
1367 
1368  Eigen::Matrix4f handNodePose =
1369  armarx::PosePtr::dynamicCast(robotStateProxy->getSynchronizedRobot()
1370  ->getRobotNode(handFrameName)
1371  ->getPoseInRootFrame())
1372  ->toEigen();
1373  Math3d::SetVec(
1374  oldHandPosSensor, handNodePose(0, 3), handNodePose(1, 3), handNodePose(2, 3));
1375  }
1376 
1377  timeOfLastExecution = IceUtil::Time::now();
1378  oldCollisionProbability = 0;
1379  active = true;
1380  }
1381 
1382  void
1383  VisualContactDetection::deactivate(const Ice::Current& c)
1384  {
1385  std::unique_lock lock(activationStateMutex);
1386  active = false;
1387  }
1388 
1389  void
1390  VisualContactDetection::SetNumberInFileName(std::string& sFileName, int nNumber, int nNumDigits)
1391  {
1392  for (int i = 0; i < nNumDigits; i++)
1393  {
1394  int nDecimalDivisor = 1;
1395 
1396  for (int j = 0; j < i; j++)
1397  {
1398  nDecimalDivisor *= 10;
1399  }
1400 
1401  sFileName.at(sFileName.length() - (5 + i)) = '0' + (nNumber / nDecimalDivisor) % 10;
1402  }
1403  }
1404 } // namespace visionx
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
visionx::VisualContactDetection::getHandPose
armarx::FramedPoseBasePtr getHandPose(const Ice::Current &c=Ice::emptyCurrent) override
Returns the hand pose.
Definition: VisualContactDetection.cpp:1324
ARMARX_IMPORTANT_S
#define ARMARX_IMPORTANT_S
Definition: Logging.h:210
armarx::ManagedIceObject::getIceManager
IceManagerPtr getIceManager() const
Returns the IceManager.
Definition: ManagedIceObject.cpp:366
visionx::VisualContactDetection::onConnectImageProcessor
void onConnectImageProcessor() override
Implement this method in the ImageProcessor in order execute parts when the component is fully initia...
Definition: VisualContactDetection.cpp:90
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:13
GfxTL::Vec3f
VectorXD< 3, float > Vec3f
Definition: VectorXD.h:733
visionx::VisualContactDetection::activate
void activate(const Ice::Current &c=Ice::emptyCurrent) override
Definition: VisualContactDetection.cpp:1352
GfxTL::Vec3d
VectorXD< 3, double > Vec3d
Definition: VectorXD.h:737
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
armarx::FramedPose
The FramedPose class.
Definition: FramedPose.h:280
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:36
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
DSHT_IMAGE_WIDTH
#define DSHT_IMAGE_WIDTH
Definition: HandLocalisationConstants.h:73
visionx::VisualContactDetection::onInitImageProcessor
void onInitImageProcessor() override
Setup the vision component.
Definition: VisualContactDetection.cpp:59
visionx::ImageProcessor::getImageProvider
ImageProviderInfo getImageProvider(std::string name, ImageType destinationImageType=eRgb, bool waitForProxy=false)
Select an ImageProvider.
Definition: ImageProcessor.cpp:167
visionx::VisualContactDetection::getFingertipPositions
visionx::FramedPositionBaseList getFingertipPositions(const Ice::Current &c=Ice::emptyCurrent) override
Returns the positions of the fingertips in this order: thumb, index, middle, ring,...
Definition: VisualContactDetection.cpp:1333
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
visionx::VisualContactDetection::onExitImageProcessor
void onExitImageProcessor() override
Exit the ImapeProcessor component.
Definition: VisualContactDetection.cpp:1127
DSHT_HAND_MODEL_PATH
#define DSHT_HAND_MODEL_PATH
Definition: HandLocalisationConstants.h:67
visionx::tools::createByteImage
CByteImage * createByteImage(const ImageFormatInfo &imageFormat, const ImageType imageType)
Creates a ByteImage for the destination type specified in the given imageProviderInfo.
visionx::CHandLocalisation::GetFingertipPositions
std::vector< Vec3d > GetFingertipPositions()
Definition: HandLocalisation.cpp:267
visionx::CHandModelVisualizer::DrawLineIntoImage
static void DrawLineIntoImage(CByteImage *pImage, int x1, int y1, int x2, int y2, int red, int green, int blue)
Definition: HandModelVisualizer.cpp:882
visionx::ImageProviderInfo
Definition: ImageProcessor.h:479
IceInternal::Handle< Pose >
visionx::tools::convert
CByteImage::ImageType convert(const ImageType visionxImageType)
Converts a VisionX image type into an image type of IVT's ByteImage.
Definition: TypeMapping.cpp:97
visionx::VisualContactDetection::process
void process() override
Process the vision component.
Definition: VisualContactDetection.cpp:204
M_PI
#define M_PI
Definition: MathTools.h:17
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
armarx::FramedPosition
The FramedPosition class.
Definition: FramedPose.h:157
visionx::CHandModelVisualizer::DrawSegmentedImage
void DrawSegmentedImage(CByteImage *pImage, bool bUseOpenInventorModel=true)
Definition: HandModelVisualizer.cpp:654
visionx::ImageProcessor::usingImageProvider
void usingImageProvider(std::string name)
Registers a delayed topic subscription and a delayed provider proxy retrieval which all will be avail...
Definition: ImageProcessor.cpp:128
visionx::CHandModelVisualizer
Definition: HandModelVisualizer.h:42
visionx::CHandLocalisation::LocaliseHand
void LocaliseHand(const CByteImage *pNewCamImageLeft, const CByteImage *pNewCamImageRight, const double *pSensorConfig, double *pEstimatedConfig, double &dConfidenceRating)
Definition: HandLocalisation.cpp:362
EPS
#define EPS
Definition: gdiam.cpp:1945
visionx::CHandModelVisualizer::UpdateHandModel
void UpdateHandModel(double *pConfig, bool bUpdateOpenInventorModel=true, bool bDrawCylinderInHand=false)
Definition: HandModelVisualizer.cpp:94
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
visionx::ImageProcessor::getImages
int getImages(CByteImage **ppImages)
Poll images from provider.
Definition: ImageProcessor.cpp:395
armarx::FramedOrientation
The FramedOrientation class.
Definition: FramedPose.h:215
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
ARMARX_LOG
#define ARMARX_LOG
Definition: Logging.h:165
visionx::CHandLocalisation
Definition: HandLocalisation.h:71
visionx::CHandLocalisation::GetHandPose
Eigen::Matrix4f GetHandPose()
Definition: HandLocalisation.cpp:233
GfxTL::Vec2d
VectorXD< 2, double > Vec2d
Definition: VectorXD.h:736
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
visionx::CHandLocalisation::SetResultConfig
void SetResultConfig(const Vec3d vPosition, const Mat3d mOrientation, const double *pConfigFingers)
Definition: HandLocalisation.cpp:307
armarx::ManagedIceObject::offeringTopic
void offeringTopic(const std::string &name)
Registers a topic for retrival after initialization.
Definition: ManagedIceObject.cpp:300
visionx::ImageProcessor::enableResultImages
void enableResultImages(int numberImages, ImageDimension imageDimension, ImageType imageType, const std::string &name="")
Enables visualization.
Definition: ImageProcessor.cpp:251
IceInternal::ProxyHandle<::IceProxy::armarx::SharedRobotInterface >
visionx::CHandLocalisation::SetParticleVarianceFactor
void SetParticleVarianceFactor(double dNewFactor=1.0)
Definition: HandLocalisation.h:100
ImageUtil.h
DSHT_NUM_PARAMETERS
#define DSHT_NUM_PARAMETERS
Definition: HandLocalisationConstants.h:53
GfxTL::Vec2f
VectorXD< 2, float > Vec2f
Definition: VectorXD.h:732
ARMARX_VERBOSE_S
#define ARMARX_VERBOSE_S
Definition: Logging.h:207
visionx::ImageProcessor::provideResultImages
void provideResultImages(CByteImage **images, armarx::MetaInfoSizeBasePtr info=nullptr)
sends result images for visualization
Definition: ImageProcessor.cpp:274
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:108
TypeMapping.h
ARMARX_INFO_S
#define ARMARX_INFO_S
Definition: Logging.h:202
visionx::CHandModelVisualizer::DrawHandModelV2
void DrawHandModelV2(CByteImage *pImage, bool bLeftCameraImage=true)
Definition: HandModelVisualizer.cpp:497
visionx::CHandLocalisation::GetResultConfig
double * GetResultConfig()
Definition: HandLocalisation.cpp:294
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:38
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
visionx::VisualContactDetection::deactivate
void deactivate(const Ice::Current &c=Ice::emptyCurrent) override
Definition: VisualContactDetection.cpp:1383
DSHT_IMAGE_HEIGHT
#define DSHT_IMAGE_HEIGHT
Definition: HandLocalisationConstants.h:74
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:154
Exception.h
VisualContactDetection.h
visionx::CHandLocalisation::SetSensorConfig
void SetSensorConfig(const Vec3d vPosition, const Mat3d mOrientation, const double *pConfigFingers)
Definition: HandLocalisation.cpp:341
visionx::ImageProcessor::waitForImages
bool waitForImages(int milliseconds=1000)
Wait for new images.
Definition: ImageProcessor.cpp:309
visionx::CHandLocalisation::GetSensorConfig
double * GetSensorConfig()
Definition: HandLocalisation.cpp:328
visionx::CHandModelVisualizer::DrawCross
static void DrawCross(CByteImage *pGreyImage, int x, int y, int nBrightness)
Definition: HandModelVisualizer.cpp:851
norm
double norm(const Point &a)
Definition: point.hpp:102