VisualContactDetection.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package VisionX::Component
19  * @author David Schiebener (schiebener at kit dot edu)
20  * @date 2011
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 
26 #pragma once
27 
28 #include <map>
29 #include <string>
30 #include <set>
31 
32 // VisionX
34 #include <VisionX/interface/components/VisualContactDetection.h>
35 #include <VisionX/interface/components/HandLocalization.h>
36 
37 
38 // Core
42 
43 // MemoryX
44 #include <MemoryX/interface/observers/ObjectMemoryObserverInterface.h>
45 
46 // IVT
47 #include <Math/Math3d.h>
48 #include <Math/Math2d.h>
49 #include <Image/IplImageAdaptor.h>
50 
51 // OpenCV
52 //#include <highgui.h>
53 #include <opencv2/highgui/highgui.hpp>
54 //#include <OpenCVLegacy.h>
55 
56 class CStereoCalibration;
57 class CByteImage;
58 
59 namespace visionx
60 {
61  // forward declarations
62  class CHandLocalisation;
63  class CHandModelVisualizer;
64 
65 
68  {
69  public:
72  {
73  defineOptionalProperty<std::string>("ImageProviderAdapterName", "Armar3ImageProvider", "Ice Adapter name of the image provider");
74  defineOptionalProperty<std::string>("RobotStateProxyName", "RobotStateComponent", "Ice Adapter name of the robot state proxy");
75  defineOptionalProperty<std::string>("HandFrameName", "TCP R", "Name of the robot state frame of the hand that will be localized");
76  defineOptionalProperty<std::string>("CameraFrameName", "EyeLeftCamera", "Name of the robot state frame of the primary camera");
77  defineOptionalProperty<int>("NumberOfParticles", 1500, "Number of particles for the particle filter for hand localization");
78  defineOptionalProperty<std::string>("ObjectMemoryObserverName", "ObjectMemoryObserver", "Name of the object memory observer proxy");
79  defineOptionalProperty<std::string>("HandNameInMemoryX", "handright3b", "Name of the hand in the object memory");
80  defineOptionalProperty<int>("MinNumPixelsInClusterForCollisionDetection", 3000, "Minimal number of pixels that a motion cluster must contain to be considered in collision detection");
81  defineOptionalProperty<bool>("UseHandLocalization", true, "Switch on or off whether the hand should be localized with the particle filter. If it is switched off, the hand pose from MemoryX is used (if available, otherwise just from the kinematic model).");
82  defineOptionalProperty<int>("MinWaitingTimeBetweenTwoFrames", 300, "Minimal time to wait between two frames so that enough motion happens (in ms)");
83  }
84  };
85 
86  /**
87  * VisualContactDetection determines if the robot hand is colliding with another object, causing it to move.
88  * To this end, it localizes the robot hand using the marker ball and the finger tips, calculates the optical
89  * flow in the image, clusters it, and looks for a cluster that exist solely in front of the hand.
90  *
91  * \componentproperties
92  * \prop VisionX.VisualContactDetection.ImageProviderAdapterName: Name of the
93  * image provider that delivers the camera images.
94  * \prop VisionX.VisualContactDetection.RobotStateProxyName: Name of the robot state
95  * proxy used to obtain the current robot state.
96  * \prop VisionX.VisualContactDetection.HandFrameName: Name of the frame in the robot
97  * model that corresponds to the localized hand.
98  * \prop VisionX.VisualContactDetection.CameraFrameName: Name of the robot state frame of the primary camera
99  */
101  virtual public visionx::ImageProcessor,
102  virtual public visionx::VisualContactDetectionInterface
103  {
104  public:
105 
106  /**
107  * @see Component::getDefaultName()
108  */
109  std::string getDefaultName() const override
110  {
111  return "VisualContactDetection";
112  }
113 
114 
115  /**
116  * Returns the hand pose.
117  */
118  armarx::FramedPoseBasePtr getHandPose(const Ice::Current& c = Ice::emptyCurrent) override;
119 
120  /**
121  * Returns the positions of the fingertips in this order: thumb, index, middle, ring, pinky.
122  */
123  visionx::FramedPositionBaseList getFingertipPositions(const Ice::Current& c = Ice::emptyCurrent) override; // ordering: thumb, index, middle, ring, pinky
124 
125  void activate(const Ice::Current& c = Ice::emptyCurrent) override;
126  void deactivate(const Ice::Current& c = Ice::emptyCurrent) override;
127 
128 
129  protected:
130  // inherited from VisionComponent
131  void onInitImageProcessor() override;
132  void onConnectImageProcessor() override;
133  void onExitImageProcessor() override;
134 
135  void process() override;
136 
137 
138 
139  /**
140  * @see PropertyUser::createPropertyDefinitions()
141  */
143  {
147  }
148 
149  private:
150  std::string providerName;
151  ImageProviderInterfacePrx imageProviderPrx;
152  std::string robotStateProxyName;
154  std::string handFrameName, cameraFrameName;
155  memoryx::ObjectMemoryObserverInterfacePrx objectMemoryObserver;
156  std::string handNameInMemoryX;
157  armarx::ChannelRefPtr handMemoryChannel;
158  bool useHandLocalization;
159  bool active;
160  std::mutex activationStateMutex;
161  IceUtil::Time timeOfLastExecution;
162 
163  CStereoCalibration* stereoCalibration;
164  CByteImage** cameraImages;
165  CByteImage** resultImages;
166 
167  void localizeHand();
168  CHandLocalisation* handLocalization;
169  CHandModelVisualizer* handModelVisualizer, *handModelVisualizer1, *handModelVisualizer2, *handModelVisualizer3;
170  static const bool drawComplexHandModelInResultImage = false;
171 
172  void calculateOpticalFlowClusters();
173  IplImage* pCamLeftIpl, *pCamLeftOldIpl;
174  CByteImage* camImgLeftGrey, *camImgLeftGreySmall, *camImgLeftGreyOld, *camImgLeftGreyOldSmall, *tempImageRGB, *pSegmentedImage, *pOpticalFlowClusterImage;
175  bool firstRun;
176  Vec3d oldHandPosSensor;
177  std::vector<std::vector<std::vector<float> > > opticalFlowClusters;
178  float* pVisOptFlowRaw;
179  int* colors;
180  static const int imageResizeFactorForOpticalFlowCalculation = 4;
181  static const int maxNumOptFlowClusters = 5;
182  int minNumPixelsInClusterForCollisionDetection;
183  static const int clusteringSampleStep = 1;
184  static constexpr float pushDetectionBoxForwardOffsetToTCP = 150.0f;
185  float oldCollisionProbability;
186 
187  bool detectCollision();
188  Vec2d handPos2D, pushTarget2D, pushTargetBoxLeftUpperCorner, pushTargetBoxRightLowerCorner;
189 
190  void drawVisualization(bool collisionDetected);
191  CByteImage* tempImageRGB1, *tempImageRGB2, *tempImageRGB3, *tempImageRGB4;
192 
193  static const bool recordImages = true;
194  int visualizationImageNumber;
195  std::vector<CByteImage*> cameraImagesForSaving, localizationResultImages, opticalFlowImages, opticalFlowClusterImages;
196  std::vector<long> timesOfImageCapture;
197 
198  static void extractAnglesFromRotationMatrix(const Mat3d& mat, Vec3d& angles);
199  static IplImage* convertToIplImage(CByteImage* pByteImageRGB);
200  static void clusterXMeans(const std::vector<std::vector<float> >& aPoints, const int nMinNumClusters, const int nMaxNumClusters, const float fBICFactor,
201  std::vector<std::vector<std::vector<float> > >& aaPointClusters, std::vector<std::vector<int> >& aaOldIndices);
202 
203  void SetNumberInFileName(std::string& sFileName, int nNumber, int nNumDigits = 4);
204 
205  VisualContactDetectionListenerPrx listener;
206 
207  enum EResultImageIndices
208  {
209  eEverything,
210  //eCameraImage,
211  //ePoseFromKinematik,
212  //ePoseFromLocalization,
213  //ePoseFromLocalizationOI,
214  //eSegmentedImage,
215  //eOpticalFlow,
216  //eOpticalFlowClusters,
217  eNumberOfResultImages
218  };
219 
220  };
221 
222 }
223 
LinkedPose.h
visionx::VisualContactDetection::getHandPose
armarx::FramedPoseBasePtr getHandPose(const Ice::Current &c=Ice::emptyCurrent) override
Returns the hand pose.
Definition: VisualContactDetection.cpp:1123
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
visionx::VisualContactDetection::activate
void activate(const Ice::Current &c=Ice::emptyCurrent) override
Definition: VisualContactDetection.cpp:1150
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
visionx::VisualContactDetection::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: VisualContactDetection.h:142
visionx::ImageProcessor
The ImageProcessor class provides an interface for access to ImageProviders via Ice and shared memory...
Definition: ImageProcessor.h:87
visionx::VisualContactDetection::onInitImageProcessor
void onInitImageProcessor() override
Setup the vision component.
Definition: VisualContactDetection.cpp:58
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
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
GfxTL::Vec2d
VectorXD< 2, double > Vec2d
Definition: VectorXD.h:694
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
visionx::VisualContactDetectionPropertyDefinitions::VisualContactDetectionPropertyDefinitions
VisualContactDetectionPropertyDefinitions(std::string prefix)
Definition: VisualContactDetection.h:70
visionx::VisualContactDetection::onExitImageProcessor
void onExitImageProcessor() override
Exit the ImapeProcessor component.
Definition: VisualContactDetection.cpp:940
Observer.h
IceInternal::Handle< ChannelRef >
visionx::VisualContactDetection::process
void process() override
Process the vision component.
Definition: VisualContactDetection.cpp:189
visionx::VisualContactDetection
VisualContactDetection determines if the robot hand is colliding with another object,...
Definition: VisualContactDetection.h:100
ImageProcessor.h
GfxTL::Vec3d
VectorXD< 3, double > Vec3d
Definition: VectorXD.h:695
visionx::CHandModelVisualizer
Definition: HandModelVisualizer.h:43
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
visionx::CHandLocalisation
Definition: HandLocalisation.h:72
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
IceUtil::Handle< class PropertyDefinitionContainer >
visionx::ImageProcessorPropertyDefinitions
Definition: ImageProcessor.h:61
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
visionx::VisualContactDetection::getDefaultName
std::string getDefaultName() const override
Definition: VisualContactDetection.h:109
visionx::VisualContactDetectionPropertyDefinitions
Definition: VisualContactDetection.h:66
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
visionx::VisualContactDetection::deactivate
void deactivate(const Ice::Current &c=Ice::emptyCurrent) override
Definition: VisualContactDetection.cpp:1177
ChannelRef.h