OpticalFlow.h
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::ArmarXObjects::OpticalFlow
17  * @author David Sippel ( uddoe at student dot kit dot edu )
18  * @date 2016
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #pragma once
24 
25 
26 #include <mutex>
27 #include <queue>
28 
29 #include <Eigen/Core>
30 #include <Eigen/Geometry>
31 
32 #include <opencv2/opencv.hpp>
33 
36 
38 #include <VisionX/interface/components/Calibration.h>
39 #include <VisionX/interface/components/OpticalFlowInterface.h>
42 
43 #include <Image/IplImageAdaptor.h>
44 
45 namespace armarx
46 {
47 
49  {
53  };
54 
55  /**
56  * @class OpticalFlowPropertyDefinitions
57  * @brief
58  */
60  {
61  public:
64  {
65  //defineRequiredProperty<std::string>("PropertyName", "Description");
66  //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
67  defineOptionalProperty<float>("Framerate", 0.0, "the framerate");
68  defineOptionalProperty<std::string>(
69  "providerName", "Armar3ImageProvider", "ImageProvider name");
70  defineOptionalProperty<OPTICAL_FLOW_METHOD>(
71  "Method", Chessboard, "use chessboard for feature tracking")
72  .map("Chessboard", Chessboard)
73  .map("Dense", Dense)
74  .map("Feature", Feature);
75 
76  defineOptionalProperty<int>("Chessboard.Width", 7, "Chessboard width");
77  defineOptionalProperty<int>("Chessboard.Height", 5, "Chessboard height");
78  defineOptionalProperty<std::string>(
79  "OpticalFlowTopicName", "OpticalFlowTopic", "OpticalFlowTopicName name");
80  defineOptionalProperty<std::string>("DebugObserverName",
81  "DebugObserver",
82  "Name of the topic the DebugObserver listens on");
83  defineOptionalProperty<std::string>(
84  "RobotNodeSetName", "Robot", "Name of the RobotNodeSet");
85 
86  defineOptionalProperty<std::string>("CalibrationUpdateTopicName",
87  "StereoCalibrationInterface",
88  "Topic name of the stereo calibration provider");
89  }
90  };
91 
92  /**
93  * @defgroup Component-OpticalFlow OpticalFlow
94  * @ingroup VisionX-Components
95  * A description of the component OpticalFlow.
96  *
97  * @class OpticalFlow
98  * @ingroup Component-OpticalFlow
99  * @brief Brief description of class OpticalFlow.
100  *
101  * Detailed description of class OpticalFlow.
102  */
103  class OpticalFlow : virtual public visionx::ImageProcessor, virtual public OpticalFlowInterface
104  {
105  public:
106  /**
107  * @see armarx::ManagedIceObject::getDefaultName()
108  */
109  std::string
110  getDefaultName() const override
111  {
112  return "OpticalFlow";
113  }
114 
115  void
116  reportStereoCalibrationChanged(const visionx::StereoCalibration& stereoCalibration,
117  bool,
118  const std::string&,
119  const Ice::Current& c = Ice::emptyCurrent) override
120  {
121 
122  std::unique_lock lock(imageMutex);
123 
124  float f_x = stereoCalibration.calibrationRight.cameraParam.focalLength[0];
125  float f_y = stereoCalibration.calibrationRight.cameraParam.focalLength[1];
126 
127  unitFactorX =
128  2.0 * std::atan(imageDimension.width / (2.0 * f_x)) / imageDimension.width;
129  unitFactorY =
130  2.0 * std::atan(imageDimension.height / (2.0 * f_y)) / imageDimension.height;
131 
132  previousTime = 0;
133  }
134 
135 
136  protected:
137  /**
138  * @see PropertyUser::createPropertyDefinitions()
139  */
141 
142  // ImageProcessor interface
143  protected:
144  void onInitImageProcessor() override;
145  void onConnectImageProcessor() override;
146  void onExitImageProcessor() override;
147  void process() override;
148 
149  private:
150  std::vector<float> removeHighestAndLowestMember(std::vector<float>& input);
151  void
152  drawDenseFlowMap(const cv::Mat& flow, cv::Mat& cflowmap, int step, const cv::Scalar& color);
153  void computeAverageDenseFlow(const cv::Mat& flow, double& mean, double& rmse);
154  void ComputeOpticalFlow(cv::Mat resultImage, cv::Mat grayImage);
155 
156  CByteImage** cameraImages;
157  std::string providerName;
158  std::mutex imageMutex;
159  cv::Mat previousImage, old2;
160  std::vector<cv::Point2f> previousFeatures;
161  visionx::ImageProviderInterfacePrx imageProviderPrx;
162  OpticalFlowListenerPrx prx;
163  visionx::ImageDimension imageDimension;
164 
165  float resultX, resultY, timeDiff;
166 
167  Ice::Long previousTime;
168 
169  int chessboardWidth, chessboardHeight;
170 
171  visionx::FPSCounter fpsCounter;
172 
173 
174  DebugObserverInterfacePrx debugObserver;
175 
176  float frameRate;
177  float unitFactorX, unitFactorY;
178 
179  OPTICAL_FLOW_METHOD method;
180  };
181 } // namespace armarx
Feature
Definition: Feature.hpp:45
armarx::OpticalFlowPropertyDefinitions
Definition: OpticalFlow.h:59
DebugObserver.h
armarx::OpticalFlow::reportStereoCalibrationChanged
void reportStereoCalibrationChanged(const visionx::StereoCalibration &stereoCalibration, bool, const std::string &, const Ice::Current &c=Ice::emptyCurrent) override
Definition: OpticalFlow.h:116
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
visionx::ImageProcessor
The ImageProcessor class provides an interface for access to ImageProviders via Ice and shared memory...
Definition: ImageProcessor.h:98
armarx::OPTICAL_FLOW_METHOD
OPTICAL_FLOW_METHOD
Definition: OpticalFlow.h:48
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:345
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
visionx::ImageProcessorPropertyDefinitions::ImageProcessorPropertyDefinitions
ImageProcessorPropertyDefinitions(std::string prefix)
Definition: ImageProcessor.h:64
armarx::Dense
@ Dense
Definition: OpticalFlow.h:51
armarx::Chessboard
@ Chessboard
Definition: OpticalFlow.h:50
armarx::mean
std::optional< float > mean(const boost::circular_buffer< NameValueMap > &buffer, const std::string &key)
Definition: KinematicUnitGuiPlugin.cpp:1620
ImageProcessor.h
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:12
armarx::VariantType::Long
const VariantTypeId Long
Definition: Variant.h:918
armarx::OpticalFlow::process
void process() override
Process the vision component.
Definition: OpticalFlow.cpp:100
armarx::OpticalFlow::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: OpticalFlow.cpp:30
Component.h
armarx::OpticalFlow::onConnectImageProcessor
void onConnectImageProcessor() override
Implement this method in the ImageProcessor in order execute parts when the component is fully initia...
Definition: OpticalFlow.cpp:56
armarx::OpticalFlow::getDefaultName
std::string getDefaultName() const override
Definition: OpticalFlow.h:110
armarx::OpticalFlow
Brief description of class OpticalFlow.
Definition: OpticalFlow.h:103
armarx::OpticalFlow::onExitImageProcessor
void onExitImageProcessor() override
Exit the ImapeProcessor component.
Definition: OpticalFlow.cpp:95
IceUtil::Handle< class PropertyDefinitionContainer >
visionx::ImageProcessorPropertyDefinitions
Definition: ImageProcessor.h:61
Scalar
float Scalar
Definition: basic.h:15
IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface >
ImageUtil.h
armarx::OpticalFlowPropertyDefinitions::OpticalFlowPropertyDefinitions
OpticalFlowPropertyDefinitions(std::string prefix)
Definition: OpticalFlow.h:62
armarx::OpticalFlow::onInitImageProcessor
void onInitImageProcessor() override
Setup the vision component.
Definition: OpticalFlow.cpp:37
FPSCounter.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
visionx::FPSCounter
The FPSCounter class provides methods for calculating the frames per second (FPS) count in periodic t...
Definition: FPSCounter.h:36