27 #include <opencv2/core/core.hpp> 28 #include <opencv2/opencv.hpp> 31 #include <gflags/gflags.h> 32 #include <glog/logging.h> 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> 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> 57 #include <opencv2/core/core.hpp> 58 #include <opencv2/opencv.hpp> 61 class ImageInput :
public op::WorkerProducer<std::shared_ptr<std::vector<op::Datum>>>
64 std::string moduleName;
65 yarp::os::RpcServer handlerPort;
66 yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > inPort;
70 ImageInput(
const std::string& moduleName) : mClosed{
false}
72 this->moduleName = moduleName;
82 void initializationOnThread() {
83 inPort.open(
"/" + moduleName +
"/image:i");
87 std::shared_ptr<std::vector<op::Datum>> workProducer()
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());
103 datum.cvInputData = in_cv;
105 if (datum.cvInputData.empty())
108 op::log(
"Empty frame detected. Closing program.", op::Priority::Max);
116 bool isFinished()
const 123 class ImageProcessing :
public op::Worker<std::shared_ptr<std::vector<op::Datum>>>
126 std::string moduleName;
127 yarp::os::BufferedPort<yarp::os::Bottle> targetPort;
129 std::map<unsigned int, std::string> mapParts {
151 ImageProcessing(
const std::string& moduleName)
153 this->moduleName = moduleName;
161 void initializationOnThread()
163 targetPort.open(
"/"+ moduleName +
"/target:o");
167 void work(std::shared_ptr<std::vector<op::Datum>>& datumsPtr)
169 if (datumsPtr !=
nullptr && !datumsPtr->empty())
171 yarp::os::Bottle &peopleBottle = targetPort.prepare();
172 peopleBottle.clear();
173 yarp::os::Bottle &mainList = peopleBottle.addList();
174 auto& tDatumsNoPtr = *datumsPtr;
177 op::Array<float> pose(tDatumsNoPtr.size());
178 for (
auto i = 0; i < tDatumsNoPtr.size(); i++)
180 pose = tDatumsNoPtr[i].poseKeypoints;
182 if (!pose.empty() && pose.getNumberDimensions() != 3)
183 op::error(
"pose.getNumberDimensions() != 3.", __LINE__, __FUNCTION__, __FILE__);
185 const auto numberPeople = pose.getSize(0);
186 const auto numberBodyParts = pose.getSize(1);
190 for (
auto person = 0 ; person < numberPeople ; person++)
192 yarp::os::Bottle &peopleList = mainList.addList();
193 for (
auto bodyPart = 0 ; bodyPart < numberBodyParts ; bodyPart++)
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]);
204 if (peopleBottle.size())
211 class ImageOutput :
public op::WorkerConsumer<std::shared_ptr<std::vector<op::Datum>>>
214 std::string moduleName;
215 yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > outPort;
219 ImageOutput(
const std::string& moduleName)
221 this->moduleName = moduleName;
225 void initializationOnThread()
227 outPort.open(
"/" + moduleName +
"/image:o");
237 void workConsumer(
const std::shared_ptr<std::vector<op::Datum>>& datumsPtr)
239 if (datumsPtr !=
nullptr && !datumsPtr->empty())
241 yarp::sig::ImageOf<yarp::sig::PixelRgb> &outImage = outPort.prepare();
242 outImage.resize(datumsPtr->at(0).cvOutputData.cols, datumsPtr->at(0).cvOutputData.rows);
244 IplImage colour = datumsPtr->at(0).cvOutputData;
245 outImage.resize(colour.width, colour.height);
246 cvCopy( &colour, (IplImage *) outImage.getIplImage());
250 op::log(
"Nullptr or empty datumsPtr found.", op::Priority::Max, __LINE__, __FUNCTION__, __FILE__);
255 class Module :
public yarp::os::RFModule
258 yarp::os::ResourceFinder *rf;
259 yarp::os::RpcServer rpcPort;
261 std::string model_name;
262 std::string model_folder;
263 std::string net_resolution;
264 std::string img_resolution;
270 bool heatmaps_add_parts;
271 bool heatmaps_add_bkg;
272 bool heatmaps_add_PAFs;
273 int heatmaps_scale_mode;
276 bool disable_blending;
278 double alpha_heatmap;
279 double render_threshold;
282 std::string hand_net_resolution;
283 int hand_scale_number;
284 double hand_scale_range;
286 double hand_alpha_pose;
287 double hand_alpha_heatmap;
288 double hand_render_threshold;
291 ImageInput *inputClass;
292 ImageProcessing *processingClass;
293 ImageOutput *outputClass;
295 op::Wrapper<std::vector<op::Datum>> opWrapper{op::ThreadManagerMode::Asynchronous};
300 bool configure(yarp::os::ResourceFinder &rf)
303 std::string moduleName = rf.check(
"name", yarp::os::Value(
"yarpOpenPose"),
"module name (string)").asString();
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();
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();
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();
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();
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();
353 setName(moduleName.c_str());
354 rpcPort.open((
"/"+getName(
"/rpc")).c_str());
357 yDebug() <<
"Starting yarpOpenPose";
360 auto outputSize = op::flagsToPoint(img_resolution, img_resolution);
362 auto netInputSize = op::flagsToPoint(net_resolution, net_resolution);
364 op::PoseModel poseModel = gflagToPoseModel(model_name);
366 op::ScaleMode keypointScale = op::flagsToScaleMode(keypoint_scale);
368 auto handNetInputSize = op::flagsToPoint(hand_net_resolution, hand_net_resolution);
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 ));
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};
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};
385 opWrapper.configure(wrapperStructPose, wrapperStructHand, op::WrapperStructInput{}, op::WrapperStructOutput{});
387 yDebug() <<
"Starting thread(s)";
390 yDebug() <<
"Done starting thread(s)";
393 inputClass =
new ImageInput(moduleName);
394 outputClass =
new ImageOutput(moduleName);
395 processingClass =
new ImageProcessing(moduleName);
397 inputClass->initializationOnThread();
398 outputClass->initializationOnThread();
399 processingClass->initializationOnThread();
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";
409 op::PoseModel gflagToPoseModel(
const std::string& poseModeString)
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;
420 yError() <<
"String does not correspond to any model (COCO, MPI, MPI_4_layers)";
421 return op::PoseModel::COCO_18;
426 op::ScaleMode gflagToScaleMode(
const int scaleMode)
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;
440 const std::string message =
"String does not correspond to any scale mode: (0, 1, 2, 3, 4) for (InputResolution, NetOutputResolution, OutputResolution, ZeroToOne, PlusMinusOne).";
442 return op::ScaleMode::InputResolution;
447 std::vector<op::HeatMapType> gflagToHeatMaps(
const bool heatmaps_add_parts,
const bool heatmaps_add_bkg,
const bool heatmaps_add_PAFs)
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);
465 delete processingClass;
482 auto datumToProcess = inputClass->workProducer();
483 if (datumToProcess !=
nullptr)
485 auto successfullyEmplaced = opWrapper.waitAndEmplace(datumToProcess);
487 std::shared_ptr<std::vector<op::Datum>> datumProcessed;
488 if (successfullyEmplaced && opWrapper.waitAndPop(datumProcessed))
490 outputClass->workConsumer(datumProcessed);
491 processingClass->work(datumProcessed);
494 yError() <<
"Processed datum could not be emplaced.";
500 int main(
int argc,
char *argv[])
502 yarp::os::Network::init();
504 google::InitGoogleLogging(
"yarpOpenPose");
506 gflags::ParseCommandLineFlags(&argc, &argv,
true);
508 yarp::os::Network yarp;
509 if (!yarp.checkNetwork())
511 yError(
"YARP server not available!");
516 yarp::os::ResourceFinder rf;
518 rf.setVerbose(
true );
519 rf.setDefaultContext(
"yarpOpenPose" );
520 rf.setDefaultConfigFile(
"yarpOpenPose.ini" );
521 rf.setDefault(
"name",
"yarpOpenPose");
522 rf.configure(argc,argv);
524 return module.runModule(rf);