Visual Servoing Platform  version 3.6.1 under development (2024-10-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 
54 BEGIN_VISP_NAMESPACE
67 {
68 public:
71  float score;
73 };
74 
75 // Don't use the default ViSP JSON conversion to avoid potential regressions (that wouldn't be detected)
76 // and very specific parsing on the server side
77 inline void from_megapose_json(const nlohmann::json &j, vpHomogeneousMatrix &T)
78 {
79  std::vector<double> values = j;
80  assert(values.size() == 16);
81  std::copy(values.begin(), values.end(), T.data);
82 }
83 
84 inline void to_megapose_json(nlohmann::json &j, const vpHomogeneousMatrix &T)
85 {
86  std::vector<double> values;
87  values.reserve(16);
88  for (unsigned i = 0; i < 16; ++i) {
89  values.push_back(T.data[i]);
90  }
91  j = values;
92 }
93 
94 inline void to_megapose_json(nlohmann::json &j, const vpRect &d)
95 {
96  std::vector<double> values = {
97  d.getLeft(), d.getTop(), d.getRight(), d.getBottom()
98  };
99  j = values;
100 }
101 
102 inline void from_megapose_json(const nlohmann::json &j, vpRect &d)
103 {
104  std::vector<double> values = j.get<std::vector<double>>();
105  assert((values.size() == 4));
106  d.setLeft(values[0]);
107  d.setTop(values[1]);
108  d.setRight(values[2]);
109  d.setBottom(values[3]);
110 }
111 
112 inline void from_json(const nlohmann::json &j, vpMegaPoseEstimate &m)
113 {
114  m.score = j["score"];
115  from_megapose_json(j.at("cTo"), m.cTo);
116  from_megapose_json(j.at("boundingBox"), m.boundingBox);
117 }
118 
137 class VISP_EXPORT vpMegaPose
138 {
139 public:
145  {
146  UNKNOWN = 0,
147  ERR = 1,
148  OK = 2,
149  GET_POSE = 3,
150  RET_POSE = 4,
151  GET_VIZ = 5,
152  RET_VIZ = 6,
153  SET_INTR = 7,
154  GET_SCORE = 8,
155  RET_SCORE = 9,
156  SET_SO3_GRID_SIZE = 10,
157  GET_LIST_OBJECTS = 11,
158  RET_LIST_OBJECTS = 12,
159  EXIT = 13
160  };
170  vpMegaPose(const std::string &host, int port, const vpCameraParameters &cam, unsigned height, unsigned width);
171 
190  std::vector<vpMegaPoseEstimate> estimatePoses(const vpImage<vpRGBa> &image, const std::vector<std::string> &objectNames,
191  const vpImage<uint16_t> *const depth = nullptr, const double depthToM = 0.f,
192  const std::vector<vpRect> *const detections = nullptr,
193  const std::vector<vpHomogeneousMatrix> *const initial_cTos = nullptr,
194  int refinerIterations = -1);
205  std::vector<double> scorePoses(const vpImage<vpRGBa> &image, const std::vector<std::string> &objectNames,
206  const std::vector<vpHomogeneousMatrix> &cTos);
207 
215  void setIntrinsics(const vpCameraParameters &cam, unsigned height, unsigned width);
216 
217  vpImage<vpRGBa> viewObjects(const std::vector<std::string> &objectNames,
218  const std::vector<vpHomogeneousMatrix> &poses, const std::string &viewType);
219 
224  void setCoarseNumSamples(const unsigned num);
225 
230  std::vector<std::string> getObjectNames();
231 
232  ~vpMegaPose();
233 
234 private:
235  // Server connection data
236  int m_serverSocket;
237  int m_fd;
238 
239  std::mutex m_mutex; // Since client-server communications are synchronous, avoid multiple parallel communications
240 
241  void makeMessage(const vpMegaPose::ServerMessage messageType, std::vector<uint8_t> &data) const;
242  std::pair<vpMegaPose::ServerMessage, std::vector<uint8_t>> readMessage() const;
243 
244  const static std::unordered_map<vpMegaPose::ServerMessage, std::string> m_codeMap;
245  static std::string messageToString(const vpMegaPose::ServerMessage messageType);
246  static vpMegaPose::ServerMessage stringToMessage(const std::string &s);
247 };
248 
249 END_VISP_NAMESPACE
250 #endif // VISP_HAVE_NLOHMANN_JSON
251 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
Generic class defining intrinsic camera parameters.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix cTo
Definition: vpMegaPose.h:70
Defines a rectangle in the plane.
Definition: vpRect.h:79