human-sensing
All Data Structures Functions Modules
main.cpp
1 /*
2  * Copyright (C) 2016 iCub Facility - Istituto Italiano di Tecnologia
3  * Author: Vadim Tikhanoff
4  * email: vadim.tikhanoff@iit.it
5  * Permission is granted to copy, distribute, and/or modify this program
6  * under the terms of the GNU General Public License, version 2 or any
7  * later version published by the Free Software Foundation.
8  *
9  * A copy of the license can be found at
10  * http://www.robotcub.org/icub/license/gpl.txt
11  *
12  * This program is distributed in the hope that it will be useful, but
13  * WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15  * Public License for more details
16  */
17 // C++ std library dependencies
18 #include <atomic>
19 #include <chrono>
20 #include <cstdio>
21 #include <string>
22 #include <thread>
23 #include <vector>
24 #include <array>
25 
26 // OpenCV dependencies
27 #include <opencv2/core/core.hpp>
28 #include <opencv2/opencv.hpp>
29 
30 // Other 3rdpary depencencies
31 #include <gflags/gflags.h> // DEFINE_bool, DEFINE_int32, DEFINE_int64, DEFINE_uint64, DEFINE_double, DEFINE_string
32 #include <glog/logging.h> // google::InitGoogleLogging, CHECK, CHECK_EQ, LOG, VLOG, ...
33 
34 // OpenPose dependencies
35  #include <openpose/core/headers.hpp>
36  #include <openpose/experimental/headers.hpp>
37  #include <openpose/filestream/headers.hpp>
38  #include <openpose/gui/headers.hpp>
39  #include <openpose/pose/headers.hpp>
40  #include <openpose/producer/headers.hpp>
41  #include <openpose/thread/headers.hpp>
42  #include <openpose/utilities/headers.hpp>
43  #include <openpose/wrapper/headers.hpp>
44 
45 // yarp dependencies
46 #include <yarp/os/BufferedPort.h>
47 #include <yarp/os/ResourceFinder.h>
48 #include <yarp/os/RFModule.h>
49 #include <yarp/os/Network.h>
50 #include <yarp/os/Log.h>
51 #include <yarp/os/Time.h>
52 #include <yarp/os/LogStream.h>
53 #include <yarp/os/Semaphore.h>
54 #include <yarp/sig/Image.h>
55 #include <yarp/os/RpcClient.h>
56 
57 #include <opencv2/core/core.hpp>
58 #include <opencv2/opencv.hpp>
59 
60 /********************************************************/
61 class ImageInput : public op::WorkerProducer<std::shared_ptr<std::vector<op::Datum>>>
62 {
63 private:
64  std::string moduleName;
65  yarp::os::RpcServer handlerPort;
66  yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > inPort;
67  bool mClosed;
68 public:
69  /********************************************************/
70  ImageInput(const std::string& moduleName) : mClosed{false}
71  {
72  this->moduleName = moduleName;
73  }
74 
75  /********************************************************/
76  ~ImageInput()
77  {
78  inPort.close();
79  }
80 
81  /********************************************************/
82  void initializationOnThread() {
83  inPort.open("/" + moduleName + "/image:i");
84  }
85 
86  /********************************************************/
87  std::shared_ptr<std::vector<op::Datum>> workProducer()
88  {
89  if (mClosed)
90  {
91  mClosed = true;
92  return nullptr;
93  }
94  else
95  {
96  // Create new datum
97  auto datumsPtr = std::make_shared<std::vector<op::Datum>>();
98  datumsPtr->emplace_back();
99  auto& datum = datumsPtr->at(0);
100  yarp::sig::ImageOf<yarp::sig::PixelRgb> *inImage = inPort.read();
101  cv::Mat in_cv = cv::cvarrToMat((IplImage *)inImage->getIplImage());
102  // Fill datum
103  datum.cvInputData = in_cv;
104  // If empty frame -> return nullptr
105  if (datum.cvInputData.empty())
106  {
107  mClosed = true;
108  op::log("Empty frame detected. Closing program.", op::Priority::Max);
109  datumsPtr = nullptr;
110  }
111  return datumsPtr;
112  }
113  }
114 
115  /********************************************************/
116  bool isFinished() const
117  {
118  return mClosed;
119  }
120 };
121 
122 /********************************************************/
123 class ImageProcessing : public op::Worker<std::shared_ptr<std::vector<op::Datum>>>
124 {
125 private:
126  std::string moduleName;
127  yarp::os::BufferedPort<yarp::os::Bottle> targetPort;
128 public:
129  std::map<unsigned int, std::string> mapParts {
130  {0, "Nose"},
131  {1, "Neck"},
132  {2, "RShoulder"},
133  {3, "RElbow"},
134  {4, "RWrist"},
135  {5, "LShoulder"},
136  {6, "LElbow"},
137  {7, "LWrist"},
138  {8, "RHip"},
139  {9, "RKnee"},
140  {10, "RAnkle"},
141  {11, "LHip"},
142  {12, "LKnee"},
143  {13, "LAnkle"},
144  {14, "REye"},
145  {15, "LEye"},
146  {16, "REar"},
147  {17, "LEar"},
148  {18, "Background"}
149  };
150  /********************************************************/
151  ImageProcessing(const std::string& moduleName)
152  {
153  this->moduleName = moduleName;
154  }
155 
156  ~ImageProcessing()
157  {
158  targetPort.close();
159  }
160  /********************************************************/
161  void initializationOnThread()
162  {
163  targetPort.open("/"+ moduleName + "/target:o");
164  }
165 
166  /********************************************************/
167  void work(std::shared_ptr<std::vector<op::Datum>>& datumsPtr)
168  {
169  if (datumsPtr != nullptr && !datumsPtr->empty())
170  {
171  yarp::os::Bottle &peopleBottle = targetPort.prepare();
172  peopleBottle.clear();
173  yarp::os::Bottle &mainList = peopleBottle.addList();
174  auto& tDatumsNoPtr = *datumsPtr;
175  // Record people pose data
176  //std::vector<op::Array<float>> pose(tDatumsNoPtr.size());
177  op::Array<float> pose(tDatumsNoPtr.size());
178  for (auto i = 0; i < tDatumsNoPtr.size(); i++)
179  {
180  pose = tDatumsNoPtr[i].poseKeypoints;
181 
182  if (!pose.empty() && pose.getNumberDimensions() != 3)
183  op::error("pose.getNumberDimensions() != 3.", __LINE__, __FUNCTION__, __FILE__);
184 
185  const auto numberPeople = pose.getSize(0);
186  const auto numberBodyParts = pose.getSize(1);
187 
188  //std::cout << "Number of people is " << numberPeople << std::endl;
189 
190  for (auto person = 0 ; person < numberPeople ; person++)
191  {
192  yarp::os::Bottle &peopleList = mainList.addList();
193  for (auto bodyPart = 0 ; bodyPart < numberBodyParts ; bodyPart++)
194  {
195  yarp::os::Bottle &partList = peopleList.addList();
196  const auto finalIndex = 3*(person*numberBodyParts + bodyPart);
197  partList.addString(mapParts[bodyPart].c_str());
198  partList.addDouble(pose[finalIndex]);
199  partList.addDouble(pose[finalIndex+1]);
200  partList.addDouble(pose[finalIndex+2]);
201  }
202  }
203  }
204  if (peopleBottle.size())
205  targetPort.write();
206  }
207  }
208 };
209 
210 /**********************************************************/
211 class ImageOutput : public op::WorkerConsumer<std::shared_ptr<std::vector<op::Datum>>>
212 {
213 private:
214  std::string moduleName;
215  yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > outPort;
216 public:
217 
218  /********************************************************/
219  ImageOutput(const std::string& moduleName)
220  {
221  this->moduleName = moduleName;
222  }
223 
224  /********************************************************/
225  void initializationOnThread()
226  {
227  outPort.open("/" + moduleName + "/image:o");
228  }
229 
230  /********************************************************/
231  ~ImageOutput()
232  {
233  outPort.close();
234  }
235 
236  /********************************************************/
237  void workConsumer(const std::shared_ptr<std::vector<op::Datum>>& datumsPtr)
238  {
239  if (datumsPtr != nullptr && !datumsPtr->empty())
240  {
241  yarp::sig::ImageOf<yarp::sig::PixelRgb> &outImage = outPort.prepare();
242  outImage.resize(datumsPtr->at(0).cvOutputData.cols, datumsPtr->at(0).cvOutputData.rows);
243 
244  IplImage colour = datumsPtr->at(0).cvOutputData;
245  outImage.resize(colour.width, colour.height);
246  cvCopy( &colour, (IplImage *) outImage.getIplImage());
247  outPort.write();
248  }
249  else
250  op::log("Nullptr or empty datumsPtr found.", op::Priority::Max, __LINE__, __FUNCTION__, __FILE__);
251  }
252 };
253 
254 /********************************************************/
255 class Module : public yarp::os::RFModule
256 {
257 private:
258  yarp::os::ResourceFinder *rf;
259  yarp::os::RpcServer rpcPort;
260 
261  std::string model_name;
262  std::string model_folder;
263  std::string net_resolution;
264  std::string img_resolution;
265  int num_gpu;
266  int num_gpu_start;
267  int num_scales;
268  float scale_gap;
269  int keypoint_scale;
270  bool heatmaps_add_parts;
271  bool heatmaps_add_bkg;
272  bool heatmaps_add_PAFs;
273  int heatmaps_scale_mode;
274  int render_pose;
275  int part_to_show;
276  bool disable_blending;
277  double alpha_pose;
278  double alpha_heatmap;
279  double render_threshold;
280  bool body_enable;
281  bool hand_enable;
282  std::string hand_net_resolution;
283  int hand_scale_number;
284  double hand_scale_range;
285  bool hand_tracking;
286  double hand_alpha_pose;
287  double hand_alpha_heatmap;
288  double hand_render_threshold;
289  int hand_render;
290 
291  ImageInput *inputClass;
292  ImageProcessing *processingClass;
293  ImageOutput *outputClass;
294 
295  op::Wrapper<std::vector<op::Datum>> opWrapper{op::ThreadManagerMode::Asynchronous};
296 
297  bool closing;
298 public:
299  /********************************************************/
300  bool configure(yarp::os::ResourceFinder &rf)
301  {
302  this->rf=&rf;
303  std::string moduleName = rf.check("name", yarp::os::Value("yarpOpenPose"), "module name (string)").asString();
304 
305  model_name = rf.check("model_name", yarp::os::Value("COCO"), "Model to be used e.g. COCO, MPI, MPI_4_layers. (string)").asString();
306  model_folder = rf.check("model_folder", yarp::os::Value("/models"), "Folder where the pose models (COCO and MPI) are located. (string)").asString();
307  net_resolution = rf.check("net_resolution", yarp::os::Value("656x368"), "The resolution of the net, multiples of 16. (string)").asString();
308  img_resolution = rf.check("img_resolution", yarp::os::Value("320x240"), "The resolution of the image (display and output). (string)").asString();
309  num_gpu = rf.check("num_gpu", yarp::os::Value("1"), "The number of GPU devices to use.(int)").asInt();
310  num_gpu_start = rf.check("num_gpu_start", yarp::os::Value("0"), "The GPU device start number.(int)").asInt();
311  num_scales = rf.check("num_scales", yarp::os::Value("1"), "Number of scales to average.(int)").asInt();
312  scale_gap = rf.check("scale_gap", yarp::os::Value("0.3"), "Scale gap between scales. No effect unless num_scales>1. Initial scale is always 1. If you want to change the initial scale,"
313  " you actually want to multiply the `net_resolution` by your desired initial scale.(float)").asDouble();
314 
315  keypoint_scale = rf.check("keypoint_scale", yarp::os::Value("0"), "Scaling of the (x,y) coordinates of the final pose data array (op::Datum::pose), i.e. the scale of the (x,y) coordinates that"
316  " will be saved with the `write_pose` & `write_pose_json` flags. Select `0` to scale it to the original source resolution, `1`"
317  " to scale it to the net output size (set with `net_resolution`), `2` to scale it to the final output size (set with "
318  " `resolution`), `3` to scale it in the range [0,1], and 4 for range [-1,1]. Non related with `num_scales` and `scale_gap`.(int)").asInt();
319 
320  heatmaps_add_parts = rf.check("heatmaps_add_parts", yarp::os::Value("false"), "If true, it will add the body part heatmaps to the final op::Datum::poseHeatMaps array (program speed will decrease). Not"
321  " required for our library, enable it only if you intend to process this information later. If more than one `add_heatmaps_X`"
322  " flag is enabled, it will place then in sequential memory order: body parts + bkg + PAFs. It will follow the order on"
323  " POSE_BODY_PART_MAPPING in `include/openpose/pose/poseParameters.hpp`.(bool)").asBool();
324  heatmaps_add_bkg = rf.check("heatmaps_add_bkg", yarp::os::Value("false"), "Same functionality as `add_heatmaps_parts`, but adding the heatmap corresponding to background. (bool)").asBool();
325 
326  heatmaps_add_PAFs = rf.check("heatmaps_add_PAFs", yarp::os::Value("false"),"Same functionality as `add_heatmaps_parts`, but adding the PAFs.(bool)").asBool();
327  heatmaps_scale_mode = rf.check("heatmaps_scale_mode", yarp::os::Value("2"), "Set 0 to scale op::Datum::poseHeatMaps in the range [0,1], 1 for [-1,1]; and 2 for integer rounded [0,255].(int)").asInt();
328  //no_render_output = rf.check("no_render_output", yarp::os::Value("false"), "If false, it will fill image with the original image + desired part to be shown. If true, it will leave them empty.(bool)").asBool();
329  render_pose = rf.check("render_pose", yarp::os::Value("2"), "Set to 0 for no rendering, 1 for CPU rendering (slightly faster), and 2 for GPU rendering(int)").asInt();
330  part_to_show = rf.check("part_to_show", yarp::os::Value("0"),"Part to show from the start.(int)").asInt();
331  disable_blending = rf.check("disable_blending", yarp::os::Value("false"), "If false, it will blend the results with the original frame. If true, it will only display the results.").asBool();
332  alpha_pose = rf.check("alpha_pose", yarp::os::Value("0.6"), "Blending factor (range 0-1) for the body part rendering. 1 will show it completely, 0 will hide it.(double)").asDouble();
333  alpha_heatmap = rf.check("alpha_heatmap", yarp::os::Value("0.7"), "Blending factor (range 0-1) between heatmap and original frame. 1 will only show the heatmap, 0 will only show the frame.(double)").asDouble();
334  render_threshold = rf.check("render_threshold", yarp::os::Value("0.05"), "Only estimated keypoints whose score confidences are higher than this threshold will be rendered. Generally, a high threshold (> 0.5) will only render very clear body parts.(double)").asDouble();
335  body_enable = rf.check("body_enable", yarp::os::Value("true"), "Disable body keypoint detection. Option only possible for faster (but less accurate) face. (bool)").asBool();
336  hand_enable = rf.check("hand_enable", yarp::os::Value("false"), "Enables hand keypoint detection. It will share some parameters from the body pose, e.g."
337  " `model_folder`. Analogously to `--face`, it will also slow down the performance, increase"
338  " the required GPU memory and its speed depends on the number of people.(int)").asBool();
339  hand_net_resolution = rf.check("hand_net_resolution", yarp::os::Value("368x368"), "Multiples of 16 and squared. Analogous to `net_resolution` but applied to the hand keypoint (string)").asString();
340  hand_scale_number = rf.check("hand_scale_number", yarp::os::Value("1"), "Analogous to `scale_number` but applied to the hand keypoint detector.(int)").asInt();
341  hand_scale_range = rf.check("hand_scale_range", yarp::os::Value("0.4"), "Analogous purpose than `scale_gap` but applied to the hand keypoint detector. Total range"
342  " between smallest and biggest scale. The scales will be centered in ratio 1. E.g. if"
343  " scaleRange = 0.4 and scalesNumber = 2, then there will be 2 scales, 0.8 and 1.2.(double)").asDouble();
344  hand_tracking = rf.check("hand_tracking", yarp::os::Value("false"), "Adding hand tracking might improve hand keypoints detection for webcam (if the frame rate"
345  " is high enough, i.e. >7 FPS per GPU) and video. This is not person ID tracking, it"
346  " simply looks for hands in positions at which hands were located in previous frames, but"
347  " it does not guarantee the same person ID among frames (bool)").asBool();
348  hand_alpha_pose = rf.check("hand_alpha_pose", yarp::os::Value("0.6"), "Analogous to `alpha_pose` but applied to hand.(double)").asDouble();
349  hand_alpha_heatmap = rf.check("hand_alpha_heatmap", yarp::os::Value("0.7"), "Analogous to `alpha_heatmap` but applied to hand.(double)").asDouble();
350  hand_render_threshold = rf.check("hand_render_threshold", yarp::os::Value("0.2"), "Analogous to `render_threshold`, but applied to the hand keypoints.(double)").asDouble();
351  hand_render = rf.check("hand_render", yarp::os::Value("-1"), "Analogous to `render_pose` but applied to the hand. Extra option: -1 to use the same(int)").asInt();
352 
353  setName(moduleName.c_str());
354  rpcPort.open(("/"+getName("/rpc")).c_str());
355  closing = false;
356 
357  yDebug() << "Starting yarpOpenPose";
358 
359  // Applying user defined configuration
360  auto outputSize = op::flagsToPoint(img_resolution, img_resolution);
361  // netInputSize
362  auto netInputSize = op::flagsToPoint(net_resolution, net_resolution);
363  //pose model
364  op::PoseModel poseModel = gflagToPoseModel(model_name);
365  // scaleMode
366  op::ScaleMode keypointScale = op::flagsToScaleMode(keypoint_scale);
367  // handNetInputSize
368  auto handNetInputSize = op::flagsToPoint(hand_net_resolution, hand_net_resolution);
369 
370  // heatmaps to add
371  std::vector<op::HeatMapType> heatMapTypes = gflagToHeatMaps(heatmaps_add_parts, heatmaps_add_bkg, heatmaps_add_PAFs);
372  op::check(heatmaps_scale_mode >= 0 && heatmaps_scale_mode <= 2, "Non valid `heatmaps_scale_mode`.", __LINE__, __FUNCTION__, __FILE__);
373  op::ScaleMode heatMapsScaleMode = (heatmaps_scale_mode == 0 ? op::ScaleMode::PlusMinusOne : (heatmaps_scale_mode == 1 ? op::ScaleMode::ZeroToOne : op::ScaleMode::UnsignedChar ));
374 
375  // Pose configuration
376  const op::WrapperStructPose wrapperStructPose{body_enable, netInputSize, outputSize, keypointScale, num_gpu, num_gpu_start, num_scales, scale_gap,
377  op::flagsToRenderMode(render_pose), poseModel, !disable_blending, (float)alpha_pose, (float)alpha_heatmap,
378  part_to_show, model_folder, heatMapTypes, heatMapsScaleMode, (float)render_threshold};
379 
380  // Hand configuration
381  const op::WrapperStructHand wrapperStructHand{hand_enable, handNetInputSize, hand_scale_number, (float)hand_scale_range,
382  hand_tracking, op::flagsToRenderMode(hand_render, render_pose),
383  (float)hand_alpha_pose, (float)hand_alpha_heatmap, (float)hand_render_threshold};
384 
385  opWrapper.configure(wrapperStructPose, wrapperStructHand, op::WrapperStructInput{}, op::WrapperStructOutput{});
386 
387  yDebug() << "Starting thread(s)";
388  attach(rpcPort);
389  opWrapper.start();
390  yDebug() << "Done starting thread(s)";
391 
392  // User processing
393  inputClass = new ImageInput(moduleName);
394  outputClass = new ImageOutput(moduleName);
395  processingClass = new ImageProcessing(moduleName);
396 
397  inputClass->initializationOnThread();
398  outputClass->initializationOnThread();
399  processingClass->initializationOnThread();
400 
401  yarp::os::Network::connect("/icub/camcalib/left/out", "/yarpOpenPose/image:i", "udp");
402  yarp::os::Network::connect("/yarpOpenPose/image:o", "/out", "udp");
403  yDebug() << "Running processses";
404 
405  return true;
406  }
407 
408  /**********************************************************/
409  op::PoseModel gflagToPoseModel(const std::string& poseModeString)
410  {
411  //op::log("", op::Priority::Low, __LINE__, __FUNCTION__, __FILE__);
412  if (poseModeString == "COCO")
413  return op::PoseModel::COCO_18;
414  else if (poseModeString == "MPI")
415  return op::PoseModel::MPI_15;
416  else if (poseModeString == "MPI_4_layers")
417  return op::PoseModel::MPI_15_4;
418  else
419  {
420  yError() << "String does not correspond to any model (COCO, MPI, MPI_4_layers)";
421  return op::PoseModel::COCO_18;
422  }
423  }
424 
425  /**********************************************************/
426  op::ScaleMode gflagToScaleMode(const int scaleMode)
427  {
428  if (scaleMode == 0)
429  return op::ScaleMode::InputResolution;
430  else if (scaleMode == 1)
431  return op::ScaleMode::NetOutputResolution;
432  else if (scaleMode == 2)
433  return op::ScaleMode::OutputResolution;
434  else if (scaleMode == 3)
435  return op::ScaleMode::ZeroToOne;
436  else if (scaleMode == 4)
437  return op::ScaleMode::PlusMinusOne;
438  else
439  {
440  const std::string message = "String does not correspond to any scale mode: (0, 1, 2, 3, 4) for (InputResolution, NetOutputResolution, OutputResolution, ZeroToOne, PlusMinusOne).";
441  yError() << message;
442  return op::ScaleMode::InputResolution;
443  }
444  }
445 
446  /**********************************************************/
447  std::vector<op::HeatMapType> gflagToHeatMaps(const bool heatmaps_add_parts, const bool heatmaps_add_bkg, const bool heatmaps_add_PAFs)
448  {
449 
450  std::vector<op::HeatMapType> heatMapTypes;
451  if (heatmaps_add_parts)
452  heatMapTypes.emplace_back(op::HeatMapType::Parts);
453  if (heatmaps_add_bkg)
454  heatMapTypes.emplace_back(op::HeatMapType::Background);
455  if (heatmaps_add_PAFs)
456  heatMapTypes.emplace_back(op::HeatMapType::PAFs);
457  return heatMapTypes;
458  }
459 
460  /**********************************************************/
461  bool close()
462  {
463  delete inputClass;
464  delete outputClass;
465  delete processingClass;
466  return true;
467  }
468  /**********************************************************/
469  bool quit(){
470  closing = true;
471  opWrapper.stop();
472  return true;
473  }
474  /********************************************************/
475  double getPeriod()
476  {
477  return 0.1;
478  }
479  /********************************************************/
480  bool updateModule()
481  {
482  auto datumToProcess = inputClass->workProducer();
483  if (datumToProcess != nullptr)
484  {
485  auto successfullyEmplaced = opWrapper.waitAndEmplace(datumToProcess);
486  // Pop frame
487  std::shared_ptr<std::vector<op::Datum>> datumProcessed;
488  if (successfullyEmplaced && opWrapper.waitAndPop(datumProcessed))
489  {
490  outputClass->workConsumer(datumProcessed);
491  processingClass->work(datumProcessed);
492  }
493  else
494  yError() << "Processed datum could not be emplaced.";
495  }
496  return !closing;
497  }
498 };
499 
500 int main(int argc, char *argv[])
501 {
502  yarp::os::Network::init();
503  // Initializing google logging (Caffe uses it for logging)
504  google::InitGoogleLogging("yarpOpenPose");
505  // Parsing command line flags
506  gflags::ParseCommandLineFlags(&argc, &argv, true);
507 
508  yarp::os::Network yarp;
509  if (!yarp.checkNetwork())
510  {
511  yError("YARP server not available!");
512  return 1;
513  }
514 
515  Module module;
516  yarp::os::ResourceFinder rf;
517 
518  rf.setVerbose( true );
519  rf.setDefaultContext( "yarpOpenPose" );
520  rf.setDefaultConfigFile( "yarpOpenPose.ini" );
521  rf.setDefault("name","yarpOpenPose");
522  rf.configure(argc,argv);
523 
524  return module.runModule(rf);
525 }