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