36 #include <visp3/core/vpConfig.h>
38 #if defined(VISP_HAVE_NLOHMANN_JSON) && defined(VISP_HAVE_THREADS)
40 #include <visp3/dnn_tracker/vpMegaPose.h>
42 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
43 #include <arpa/inet.h>
45 #include <netinet/in.h>
46 #include <sys/socket.h>
56 using json = nlohmann::json;
63 void encode(std::vector<uint8_t> &)
73 void encode(std::vector<uint8_t> &buffer,
const T &
object) =
delete;
78 void encode(std::vector<uint8_t> &buffer,
const int &
object)
80 const uint32_t v = htonl(
object);
81 const uint8_t *varr = (uint8_t *)&v;
82 buffer.insert(buffer.end(), varr, varr + 4);
86 void encode(std::vector<uint8_t> &buffer,
const float &
object)
88 assert((
sizeof(uint32_t) ==
sizeof(
float)));
89 const uint32_t *pointer =
reinterpret_cast<const uint32_t *
>(&object);
90 const uint32_t v = htonl(*pointer);
91 const uint8_t *varr = (uint8_t *)&v;
92 buffer.insert(buffer.end(), varr, varr + 4);
96 void encode(std::vector<uint8_t> &buffer,
const std::string &
object)
98 const int size =
static_cast<int>(
object.size());
100 const uint8_t *chars = (uint8_t *)&
object[0];
101 buffer.insert(buffer.end(), chars, chars + size);
105 void encode(std::vector<uint8_t> &buffer,
const std::vector<T> &
object)
107 const int size =
static_cast<int>(
object.size());
108 encode(buffer, size);
109 for (
const T &value :
object) {
110 encode(buffer, value);
115 template<
typename T,
typename ...Rest>
116 void encode(std::vector<uint8_t> &buffer,
const T &
object,
const Rest& ...rest)
118 encode(buffer,
object);
119 encode(buffer, rest...);
123 void encode(std::vector<uint8_t> &buffer,
const vpImage<vpRGBa> &
object)
125 const int height =
object.getHeight(), width =
object.getWidth();
126 encode(buffer, height, width, 4);
127 const uint32_t sentSize = height * width * 4;
129 buffer.reserve(buffer.size() + sentSize);
130 const uint8_t *
const bitmap = (uint8_t *)
object.bitmap;
131 buffer.insert(buffer.end(), bitmap, bitmap + sentSize);
138 const int height =
object.getHeight(), width =
object.getWidth();
139 encode(buffer, height, width);
141 const uint16_t hostTest = 1;
142 const uint16_t netTest = htons(hostTest);
143 const uint8_t endianness = hostTest == netTest ?
'>' :
'<';
144 const uint32_t sentSize = height * width * 2;
146 buffer.reserve(buffer.size() + sentSize + 1);
147 buffer.push_back(endianness);
148 const uint8_t *
const bitmap = (uint8_t *)
object.bitmap;
149 buffer.insert(buffer.end(), bitmap, bitmap + sentSize);
155 encode(buffer, (
float)
object.get_px(), (
float)
object.get_py(),
156 (
float)
object.get_u0(), (
float)
object.get_v0());
162 std::vector<float> array;
164 const double *
const data =
object.data;
165 for (
unsigned i = 0; i < 16; ++i) {
166 array.push_back((
float)data[i]);
168 encode(buffer, array);
173 void decode(
const std::vector<uint8_t> &,
unsigned int &)
184 void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index, T &t) =
delete;
187 void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index,
int &value)
189 const uint8_t *ptr = &buffer[index];
190 value = ntohl(*((uint32_t *)ptr));
191 index +=
sizeof(int);
194 void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index,
float &value)
196 const uint8_t *ptr = &buffer[index];
197 const uint32_t v = ntohl(*((uint32_t *)ptr));
198 memcpy(&value, &v,
sizeof(uint32_t));
199 index +=
sizeof(float);
202 void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index, std::string &value)
205 decode(buffer, index, size);
207 value.replace(0, size, (
char *)&buffer[index], size);
212 void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index, std::vector<T> &value)
215 decode(buffer, index, size);
217 for (
int i = 0; i < size; ++i) {
219 decode(buffer, index, t);
225 void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index,
vpHomogeneousMatrix &value)
227 std::vector<float> values;
228 decode(buffer, index, values);
229 assert(values.size() == 16);
230 for (
int i = 0; i < 16; ++i) {
231 value.
data[i] = values[i];
239 template<
typename T,
typename ...Rest>
240 void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index, T &
object, Rest& ...rest)
242 decode(buffer, index,
object);
243 decode(buffer, index, rest...);
247 void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index,
vpImage<vpRGBa> &value)
249 int height, width, channels;
250 decode(buffer, index, height, width, channels);
251 value.
resize(height, width);
253 for (
int i = 0; i < height; ++i) {
254 for (
int j = 0; j < width; ++j) {
255 value.
bitmap[i * width + j] =
vpRGBa(buffer[index], buffer[index + 1], buffer[index + 2], 255);
260 else if (channels == 4) {
261 const unsigned copySize = height * width * channels;
262 memcpy((uint8_t *)value.
bitmap, &buffer[index], copySize);
268 #define MEGAPOSE_CODE_SIZE 4
271 if (code != vpMegaPose::ServerMessage::ERR) {
276 decode(buffer, index, message);
280 const std::unordered_map<vpMegaPose::ServerMessage, std::string> vpMegaPose::m_codeMap =
282 {ServerMessage::ERR,
"RERR"},
283 {ServerMessage::OK,
"OKOK"},
284 {ServerMessage::GET_POSE,
"GETP"},
285 {ServerMessage::RET_POSE,
"RETP"},
286 {ServerMessage::SET_INTR,
"INTR"},
287 {ServerMessage::GET_VIZ,
"GETV"},
288 {ServerMessage::RET_VIZ,
"RETV"},
289 {ServerMessage::GET_SCORE,
"GSCO"},
290 {ServerMessage::RET_SCORE,
"RSCO"},
291 {ServerMessage::SET_SO3_GRID_SIZE,
"SO3G"},
292 {ServerMessage::GET_LIST_OBJECTS,
"GLSO"},
293 {ServerMessage::RET_LIST_OBJECTS,
"RLSO"},
294 {ServerMessage::EXIT,
"EXIT"},
299 return m_codeMap.at(messageType);
304 for (
auto it : m_codeMap) {
305 if (it.second == s) {
314 const uint32_t size = htonl(
static_cast<uint32_t
>(data.size()));
315 const std::string code = messageToString(messageType);
316 uint8_t arr[
sizeof(size) + MEGAPOSE_CODE_SIZE];
317 memcpy(arr, (uint8_t *)&size,
sizeof(size));
319 memcpy(arr +
sizeof(size), (uint8_t *)code.c_str(), MEGAPOSE_CODE_SIZE);
321 std::vector<uint8_t> header(arr, arr +
sizeof(size) + MEGAPOSE_CODE_SIZE);
322 data.insert(data.begin(), header.begin(), header.end());
325 std::pair<vpMegaPose::ServerMessage, std::vector<uint8_t>> vpMegaPose::readMessage()
const
328 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
329 size_t readCount = read(m_serverSocket, &size,
sizeof(uint32_t));
331 size_t readCount = recv(m_serverSocket,
reinterpret_cast<char *
>(&size),
sizeof(uint32_t), 0);
333 if (readCount !=
sizeof(uint32_t)) {
338 unsigned char code[MEGAPOSE_CODE_SIZE];
339 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
340 readCount = read(m_serverSocket, code, MEGAPOSE_CODE_SIZE);
342 readCount = recv(m_serverSocket,
reinterpret_cast<char *
>(code), MEGAPOSE_CODE_SIZE, 0);
344 if (readCount != MEGAPOSE_CODE_SIZE) {
348 std::vector<uint8_t> data;
350 unsigned read_size = 4096;
351 unsigned read_total = 0;
352 while (read_total < size) {
353 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
354 int actually_read = read(m_serverSocket, &data[read_total], read_size);
356 int actually_read = recv(m_serverSocket,
reinterpret_cast<char *
>(&data[read_total]), read_size, 0);
358 if (actually_read <= 0) {
361 read_total += actually_read;
363 std::string codeStr(code, code + MEGAPOSE_CODE_SIZE);
365 return std::make_pair(c, data);
372 if (WSAStartup(MAKEWORD(2, 0), &WSAData) != 0) {
376 struct sockaddr_in serv_addr;
377 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
378 if ((m_serverSocket = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
380 if ((m_serverSocket =
static_cast<int>(socket(AF_INET, SOCK_STREAM, 0))) < 0) {
384 serv_addr.sin_family = AF_INET;
385 serv_addr.sin_port = htons(port);
387 if (inet_pton(AF_INET, host.c_str(), &serv_addr.sin_addr) <= 0) {
391 if ((m_fd = connect(m_serverSocket, (
struct sockaddr *)&serv_addr,
sizeof(serv_addr))) < 0) {
399 std::vector<uint8_t> data;
400 makeMessage(ServerMessage::EXIT, data);
401 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
410 std::vector<vpMegaPoseEstimate>
413 const std::vector<vpRect>*
const detections,
const std::vector<vpHomogeneousMatrix>*
const initial_cTos,
414 int refinerIterations)
416 const std::lock_guard<std::mutex> lock(m_mutex);
417 std::vector<uint8_t> data;
420 parametersJson[
"labels"] = labels;
422 if (detections ==
nullptr && initial_cTos ==
nullptr) {
426 if (detections !=
nullptr) {
427 if (detections->size() != labels.size()) {
430 json detectionsJson = json::array();
431 for (
const vpRect &bb : *detections) {
433 to_megapose_json(j, bb);
434 detectionsJson.push_back(j);
436 parametersJson[
"detections"] = detectionsJson;
439 if (initial_cTos !=
nullptr) {
440 if (initial_cTos->size() != labels.size()) {
443 json cToJson = json::array();
446 to_megapose_json(j, cTo);
447 cToJson.push_back(j);
449 parametersJson[
"initial_cTos"] = cToJson;
451 if (refinerIterations >= 0) {
452 parametersJson[
"refiner_iterations"] = refinerIterations;
454 if (depth !=
nullptr) {
455 if (depth_to_m <= 0.0) {
458 parametersJson[
"use_depth"] =
true;
459 parametersJson[
"depth_scale_to_m"] = depth_to_m;
462 parametersJson[
"use_depth"] =
false;
464 encode(data, parametersJson.dump());
465 if (depth !=
nullptr) {
466 encode(data, *depth);
468 makeMessage(ServerMessage::GET_POSE, data);
469 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
473 std::vector<uint8_t> data_result;
474 std::tie(code, data_result) = readMessage();
476 unsigned int index = 0;
477 if (code != ServerMessage::RET_POSE) {
478 handleWrongReturnMessage(code, data_result);
481 decode(data_result, index, jsonStr);
482 json jsonValue = json::parse(jsonStr);
483 std::vector<vpMegaPoseEstimate> result = jsonValue;
488 const std::vector<std::string>&labels,
const std::vector<vpHomogeneousMatrix>&cTos)
490 const std::lock_guard<std::mutex> lock(m_mutex);
491 std::vector<uint8_t> data;
492 if (cTos.size() != labels.size()) {
493 throw vpException(vpException::generalExceptionEnum::badValue,
"The number of poses should be the same as the number of object labels");
497 json cToJson = json::array();
500 to_megapose_json(j, cTo);
501 cToJson.push_back(j);
503 parametersJson[
"cTos"] = cToJson;
504 parametersJson[
"labels"] = labels;
506 encode(data, parametersJson.dump());
507 makeMessage(ServerMessage::GET_SCORE, data);
508 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
511 std::vector<uint8_t> data_result;
512 std::tie(code, data_result) = readMessage();
514 if (code != ServerMessage::RET_SCORE) {
515 handleWrongReturnMessage(code, data_result);
517 unsigned int index = 0;
519 decode(data_result, index, jsonStr);
520 json jsonValue = json::parse(jsonStr);
521 std::vector<double> result = jsonValue;
528 const std::lock_guard<std::mutex> lock(m_mutex);
529 std::vector<uint8_t> data;
532 message[
"px"] = cam.
get_px();
533 message[
"py"] = cam.
get_py();
534 message[
"u0"] = cam.
get_u0();
535 message[
"v0"] = cam.
get_v0();
536 message[
"h"] = height;
537 message[
"w"] = width;
539 encode(data, message.dump());
540 makeMessage(ServerMessage::SET_INTR, data);
542 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
544 std::vector<uint8_t> data_result;
545 std::tie(code, data_result) = readMessage();
546 if (code != ServerMessage::OK) {
547 handleWrongReturnMessage(code, data_result);
552 const std::vector<vpHomogeneousMatrix>&poses,
const std::string& viewType)
554 const std::lock_guard<std::mutex> lock(m_mutex);
555 std::vector<uint8_t> data;
557 j[
"labels"] = objectNames;
558 json cToJson = json::array();
561 to_megapose_json(j, cTo);
562 cToJson.push_back(j);
564 j[
"poses"] = cToJson;
565 j[
"type"] = viewType;
566 encode(data, j.dump());
567 makeMessage(ServerMessage::GET_VIZ, data);
568 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
570 std::vector<uint8_t> data_result;
571 std::tie(code, data_result) = readMessage();
573 if (code != ServerMessage::RET_VIZ) {
574 handleWrongReturnMessage(code, data_result);
577 unsigned int index = 0;
578 decode(data_result, index, result);
584 const std::lock_guard<std::mutex> lock(m_mutex);
585 std::vector<uint8_t> data;
587 j[
"so3_grid_size"] = num;
588 encode(data, j.dump());
589 makeMessage(ServerMessage::SET_SO3_GRID_SIZE, data);
590 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
592 std::vector<uint8_t> data_result;
593 std::tie(code, data_result) = readMessage();
594 if (code != ServerMessage::OK) {
595 handleWrongReturnMessage(code, data_result);
601 const std::lock_guard<std::mutex> lock(m_mutex);
602 std::vector<uint8_t> data;
603 makeMessage(ServerMessage::GET_LIST_OBJECTS, data);
604 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
606 std::vector<uint8_t> data_result;
607 std::tie(code, data_result) = readMessage();
608 if (code != ServerMessage::RET_LIST_OBJECTS) {
609 handleWrongReturnMessage(code, data_result);
611 unsigned int index = 0;
613 decode(data_result, index, jsonStr);
614 json jsonValue = json::parse(jsonStr);
615 std::vector<std::string> result = jsonValue;
622 class VISP_EXPORT dummy_vpMegaPose
625 dummy_vpMegaPose() { };
628 #if !defined(VISP_BUILD_SHARED_LIBS)
630 void dummy_vpMegaPose_fct() { };
Type * data
Address of the first element of the data array.
Generic class defining intrinsic camera parameters.
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Type * bitmap
points toward the bitmap
void setIntrinsics(const vpCameraParameters &cam, unsigned height, unsigned width)
vpImage< vpRGBa > viewObjects(const std::vector< std::string > &objectNames, const std::vector< vpHomogeneousMatrix > &poses, const std::string &viewType)
std::vector< vpMegaPoseEstimate > estimatePoses(const vpImage< vpRGBa > &image, const std::vector< std::string > &objectNames, const vpImage< uint16_t > *const depth=nullptr, const double depthToM=0.f, const std::vector< vpRect > *const detections=nullptr, const std::vector< vpHomogeneousMatrix > *const initial_cTos=nullptr, int refinerIterations=-1)
std::vector< std::string > getObjectNames()
Query the server to find the name of all of the objects it knows.
void setCoarseNumSamples(const unsigned num)
std::vector< double > scorePoses(const vpImage< vpRGBa > &image, const std::vector< std::string > &objectNames, const std::vector< vpHomogeneousMatrix > &cTos)
vpMegaPose(const std::string &host, int port, const vpCameraParameters &cam, unsigned height, unsigned width)
Defines a rectangle in the plane.