Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpMegaPose.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * MegaPose wrapper.
33  *
34 *****************************************************************************/
35 #ifndef _vpMegaPose_h_
36 #define _vpMegaPose_h_
37 
38 #include <visp3/core/vpConfig.h>
39 #if defined(VISP_HAVE_NLOHMANN_JSON) && defined(VISP_HAVE_THREADS)
40 
41 #include <vector>
42 #include <string>
43 #include <unordered_map>
44 #include <mutex>
45 
46 #include <visp3/core/vpHomogeneousMatrix.h>
47 #include <visp3/core/vpCameraParameters.h>
48 #include <visp3/core/vpImage.h>
49 #include <visp3/core/vpRGBa.h>
50 #include <visp3/core/vpRect.h>
51 
52 #include <nlohmann/json.hpp>
53 
66 {
67 public:
70  float score;
72 };
73 
74 // Don't use the default ViSP JSON conversion to avoid potential regressions (that wouldn't be detected)
75 // and very specific parsing on the server side
76 inline void from_megapose_json(const nlohmann::json &j, vpHomogeneousMatrix &T)
77 {
78  std::vector<double> values = j;
79  assert(values.size() == 16);
80  std::copy(values.begin(), values.end(), T.data);
81 }
82 
83 inline void to_megapose_json(nlohmann::json &j, const vpHomogeneousMatrix &T)
84 {
85  std::vector<double> values;
86  values.reserve(16);
87  for (unsigned i = 0; i < 16; ++i) {
88  values.push_back(T.data[i]);
89  }
90  j = values;
91 }
92 
93 inline void to_megapose_json(nlohmann::json &j, const vpRect &d)
94 {
95  std::vector<double> values = {
96  d.getLeft(), d.getTop(), d.getRight(), d.getBottom()
97  };
98  j = values;
99 }
100 
101 inline void from_megapose_json(const nlohmann::json &j, vpRect &d)
102 {
103  std::vector<double> values = j.get<std::vector<double>>();
104  assert((values.size() == 4));
105  d.setLeft(values[0]);
106  d.setTop(values[1]);
107  d.setRight(values[2]);
108  d.setBottom(values[3]);
109 }
110 
111 inline void from_json(const nlohmann::json &j, vpMegaPoseEstimate &m)
112 {
113  m.score = j["score"];
114  from_megapose_json(j.at("cTo"), m.cTo);
115  from_megapose_json(j.at("boundingBox"), m.boundingBox);
116 }
117 
136 class VISP_EXPORT vpMegaPose
137 {
138 public:
144  {
145  UNKNOWN = 0,
146  ERR = 1,
147  OK = 2,
148  GET_POSE = 3,
149  RET_POSE = 4,
150  GET_VIZ = 5,
151  RET_VIZ = 6,
152  SET_INTR = 7,
153  GET_SCORE = 8,
154  RET_SCORE = 9,
155  SET_SO3_GRID_SIZE = 10,
156  GET_LIST_OBJECTS = 11,
157  RET_LIST_OBJECTS = 12,
158  EXIT = 13
159  };
169  vpMegaPose(const std::string &host, int port, const vpCameraParameters &cam, unsigned height, unsigned width);
170 
189  std::vector<vpMegaPoseEstimate> estimatePoses(const vpImage<vpRGBa> &image, const std::vector<std::string> &objectNames,
190  const vpImage<uint16_t> *const depth = nullptr, const double depthToM = 0.f,
191  const std::vector<vpRect> *const detections = nullptr,
192  const std::vector<vpHomogeneousMatrix> *const initial_cTos = nullptr,
193  int refinerIterations = -1);
204  std::vector<double> scorePoses(const vpImage<vpRGBa> &image, const std::vector<std::string> &objectNames,
205  const std::vector<vpHomogeneousMatrix> &cTos);
206 
214  void setIntrinsics(const vpCameraParameters &cam, unsigned height, unsigned width);
215 
216  vpImage<vpRGBa> viewObjects(const std::vector<std::string> &objectNames,
217  const std::vector<vpHomogeneousMatrix> &poses, const std::string &viewType);
218 
223  void setCoarseNumSamples(const unsigned num);
224 
229  std::vector<std::string> getObjectNames();
230 
231  ~vpMegaPose();
232 
233 private:
234  // Server connection data
235  int m_serverSocket;
236  int m_fd;
237 
238  std::mutex m_mutex; // Since client-server communications are synchronous, avoid multiple parallel communications
239 
240  void makeMessage(const vpMegaPose::ServerMessage messageType, std::vector<uint8_t> &data) const;
241  std::pair<vpMegaPose::ServerMessage, std::vector<uint8_t>> readMessage() const;
242 
243  const static std::unordered_map<vpMegaPose::ServerMessage, std::string> m_codeMap;
244  static std::string messageToString(const vpMegaPose::ServerMessage messageType);
245  static vpMegaPose::ServerMessage stringToMessage(const std::string &s);
246 };
247 
248 #endif // VISP_HAVE_NLOHMANN_JSON
249 
250 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:138
Generic class defining intrinsic camera parameters.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix cTo
Definition: vpMegaPose.h:69
Defines a rectangle in the plane.
Definition: vpRect.h:76