36#include <visp3/dnn_tracker/vpMegaPose.h>
37#include <visp3/core/vpConfig.h>
39#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
42#include <netinet/in.h>
43#include <sys/socket.h>
53using json = nlohmann::json;
60void encode(std::vector<uint8_t> &)
70void encode(std::vector<uint8_t> &buffer,
const T &
object) =
delete;
75void encode(std::vector<uint8_t> &buffer,
const int &
object)
77 const uint32_t v = htonl(
object);
78 const uint8_t *varr = (uint8_t *)&v;
79 buffer.insert(buffer.end(), varr, varr + 4);
83void encode(std::vector<uint8_t> &buffer,
const float &
object)
85 assert((
sizeof(uint32_t) ==
sizeof(
float)));
86 const uint32_t *pointer =
reinterpret_cast<const uint32_t *
>(&object);
87 const uint32_t v = htonl(*pointer);
88 const uint8_t *varr = (uint8_t *)&v;
89 buffer.insert(buffer.end(), varr, varr + 4);
93void encode(std::vector<uint8_t> &buffer,
const std::string &
object)
95 const int size =
static_cast<int>(
object.size());
97 const uint8_t *chars = (uint8_t *)&
object[0];
98 buffer.insert(buffer.end(), chars, chars + size);
102void encode(std::vector<uint8_t> &buffer,
const std::vector<T> &
object)
104 const int size =
static_cast<int>(
object.size());
105 encode(buffer, size);
106 for (
const T &value :
object) {
107 encode(buffer, value);
112template<
typename T,
typename ...Rest>
113void encode(std::vector<uint8_t> &buffer,
const T &
object,
const Rest& ...rest)
115 encode(buffer,
object);
116 encode(buffer, rest...);
120void encode(std::vector<uint8_t> &buffer,
const vpImage<vpRGBa> &
object)
122 const int height =
object.getHeight(), width =
object.getWidth();
123 encode(buffer, height, width, 4);
124 const uint32_t sentSize = height * width * 4;
126 buffer.reserve(buffer.size() + sentSize);
127 const uint8_t *
const bitmap = (uint8_t *)
object.bitmap;
128 buffer.insert(buffer.end(), bitmap, bitmap + sentSize);
135 const int height =
object.getHeight(), width =
object.getWidth();
136 encode(buffer, height, width);
138 const uint16_t hostTest = 1;
139 const uint16_t netTest = htons(hostTest);
140 const uint8_t endianness = hostTest == netTest ?
'>' :
'<';
141 const uint32_t sentSize = height * width * 2;
143 buffer.reserve(buffer.size() + sentSize + 1);
144 buffer.push_back(endianness);
145 const uint8_t *
const bitmap = (uint8_t *)
object.bitmap;
146 buffer.insert(buffer.end(), bitmap, bitmap + sentSize);
152 encode(buffer, (
float)
object.get_px(), (
float)
object.get_py(),
153 (
float)
object.get_u0(), (
float)
object.get_v0());
159 std::vector<float> array;
161 const double *
const data =
object.data;
162 for (
unsigned i = 0; i < 16; ++i) {
163 array.push_back((
float)data[i]);
165 encode(buffer, array);
170void decode(
const std::vector<uint8_t> &,
unsigned int &)
181void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index, T &t) =
delete;
184void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index,
int &value)
186 const uint8_t *ptr = &buffer[index];
187 value = ntohl(*((uint32_t *)ptr));
188 index +=
sizeof(int);
191void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index,
float &value)
193 const uint8_t *ptr = &buffer[index];
194 const uint32_t v = ntohl(*((uint32_t *)ptr));
195 memcpy(&value, &v,
sizeof(uint32_t));
196 index +=
sizeof(float);
199void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index, std::string &value)
202 decode(buffer, index, size);
204 value.replace(0, size, (
char *)&buffer[index], size);
209void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index, std::vector<T> &value)
212 decode(buffer, index, size);
214 for (
int i = 0; i < size; ++i) {
216 decode(buffer, index, t);
222void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index,
vpHomogeneousMatrix &value)
224 std::vector<float> values;
225 decode(buffer, index, values);
226 assert(values.size() == 16);
227 for (
int i = 0; i < 16; ++i) {
228 value.
data[i] = values[i];
236template<
typename T,
typename ...Rest>
237void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index, T &
object, Rest& ...rest)
239 decode(buffer, index,
object);
240 decode(buffer, index, rest...);
244void decode(
const std::vector<uint8_t> &buffer,
unsigned int &index,
vpImage<vpRGBa> &value)
246 int height, width, channels;
247 decode(buffer, index, height, width, channels);
248 value.
resize(height, width);
250 for (
int i = 0; i < height; ++i) {
251 for (
int j = 0; j < width; ++j) {
252 value.
bitmap[i * width + j] =
vpRGBa(buffer[index], buffer[index + 1], buffer[index + 2], 255);
257 else if (channels == 4) {
258 const unsigned copySize = height * width * channels;
259 memcpy((uint8_t *)value.
bitmap, &buffer[index], copySize);
265#define MEGAPOSE_CODE_SIZE 4
273 decode(buffer, index, message);
277const std::unordered_map<vpMegaPose::ServerMessage, std::string> vpMegaPose::m_codeMap =
279 {ServerMessage::ERR,
"RERR"},
280 {ServerMessage::OK,
"OKOK"},
281 {ServerMessage::GET_POSE,
"GETP"},
282 {ServerMessage::RET_POSE,
"RETP"},
283 {ServerMessage::SET_INTR,
"INTR"},
284 {ServerMessage::GET_VIZ,
"GETV"},
285 {ServerMessage::RET_VIZ,
"RETV"},
286 {ServerMessage::GET_SCORE,
"GSCO"},
287 {ServerMessage::RET_SCORE,
"RSCO"},
288 {ServerMessage::SET_SO3_GRID_SIZE,
"SO3G"},
289 {ServerMessage::GET_LIST_OBJECTS,
"GLSO"},
290 {ServerMessage::RET_LIST_OBJECTS,
"RLSO"},
291 {ServerMessage::EXIT,
"EXIT"},
297 return m_codeMap.at(messageType);
302 for (
auto it : m_codeMap) {
303 if (it.second == s) {
312 const uint32_t size = htonl(
static_cast<uint32_t
>(data.size()));
313 const std::string code = messageToString(messageType);
314 uint8_t arr[
sizeof(size) + MEGAPOSE_CODE_SIZE];
315 memcpy(arr, (uint8_t *)&size,
sizeof(size));
317 memcpy(arr +
sizeof(size), (uint8_t *)code.c_str(), MEGAPOSE_CODE_SIZE);
319 std::vector<uint8_t> header(arr, arr +
sizeof(size) + MEGAPOSE_CODE_SIZE);
320 data.insert(data.begin(), header.begin(), header.end());
323std::pair<vpMegaPose::ServerMessage, std::vector<uint8_t>> vpMegaPose::readMessage()
const
326#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
327 size_t readCount = read(m_serverSocket, &size,
sizeof(uint32_t));
329 size_t readCount = recv(m_serverSocket,
reinterpret_cast<char*
>(&size),
sizeof(uint32_t), 0);
331 if (readCount !=
sizeof(uint32_t)) {
336 unsigned char code[MEGAPOSE_CODE_SIZE];
337#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
338 readCount = read(m_serverSocket, code, MEGAPOSE_CODE_SIZE);
340 readCount = recv(m_serverSocket,
reinterpret_cast<char*
>(code), MEGAPOSE_CODE_SIZE, 0);
342 if (readCount != MEGAPOSE_CODE_SIZE) {
346 std::vector<uint8_t> data;
348 unsigned read_size = 4096;
349 unsigned read_total = 0;
350 while (read_total < size) {
351#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
352 int actually_read = read(m_serverSocket, &data[read_total], read_size);
354 int actually_read = recv(m_serverSocket,
reinterpret_cast<char*
>(&data[read_total]), read_size, 0);
356 if (actually_read <= 0) {
359 read_total += actually_read;
361 std::string codeStr(code, code + MEGAPOSE_CODE_SIZE);
363 return std::make_pair(c, data);
370 if (WSAStartup(MAKEWORD(2, 0), &WSAData) != 0) {
374 struct sockaddr_in serv_addr;
375#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
376 if ((m_serverSocket = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
378 if ((m_serverSocket =
static_cast<int>(socket(AF_INET, SOCK_STREAM, 0))) < 0) {
382 serv_addr.sin_family = AF_INET;
383 serv_addr.sin_port = htons(port);
385 if (inet_pton(AF_INET, host.c_str(), &serv_addr.sin_addr) <= 0) {
389 if ((m_fd = connect(m_serverSocket, (
struct sockaddr *)&serv_addr,
sizeof(serv_addr))) < 0) {
397 std::vector<uint8_t> data;
399 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
408std::vector<vpMegaPoseEstimate>
411 const std::vector<vpRect>*
const detections,
const std::vector<vpHomogeneousMatrix>*
const initial_cTos,
412 int refinerIterations)
414 const std::lock_guard<std::mutex> lock(m_mutex);
415 std::vector<uint8_t> data;
418 parametersJson[
"labels"] = labels;
420 if (detections ==
nullptr && initial_cTos ==
nullptr) {
424 if (detections !=
nullptr) {
425 if (detections->size() != labels.size()) {
428 json detectionsJson = json::array();
429 for (
const vpRect &bb : *detections) {
431 to_megapose_json(j, bb);
432 detectionsJson.push_back(j);
434 parametersJson[
"detections"] = detectionsJson;
437 if (initial_cTos !=
nullptr) {
438 if (initial_cTos->size() != labels.size()) {
441 json cToJson = json::array();
444 to_megapose_json(j, cTo);
445 cToJson.push_back(j);
447 parametersJson[
"initial_cTos"] = cToJson;
449 if (refinerIterations >= 0) {
450 parametersJson[
"refiner_iterations"] = refinerIterations;
452 if (depth !=
nullptr) {
453 if (depth_to_m <= 0.0) {
456 parametersJson[
"use_depth"] =
true;
457 parametersJson[
"depth_scale_to_m"] = depth_to_m;
460 parametersJson[
"use_depth"] =
false;
462 encode(data, parametersJson.dump());
463 if (depth !=
nullptr) {
464 encode(data, *depth);
467 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
471 std::vector<uint8_t> data_result;
472 std::tie(code, data_result) = readMessage();
474 unsigned int index = 0;
476 handleWrongReturnMessage(code, data_result);
479 decode(data_result, index, jsonStr);
480 json jsonValue = json::parse(jsonStr);
481 std::vector<vpMegaPoseEstimate> result = jsonValue;
486 const std::vector<std::string>&labels,
const std::vector<vpHomogeneousMatrix>&cTos)
488 const std::lock_guard<std::mutex> lock(m_mutex);
489 std::vector<uint8_t> data;
490 if (cTos.size() != labels.size()) {
495 json cToJson = json::array();
498 to_megapose_json(j, cTo);
499 cToJson.push_back(j);
501 parametersJson[
"cTos"] = cToJson;
502 parametersJson[
"labels"] = labels;
504 encode(data, parametersJson.dump());
506 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
509 std::vector<uint8_t> data_result;
510 std::tie(code, data_result) = readMessage();
513 handleWrongReturnMessage(code, data_result);
515 unsigned int index = 0;
517 decode(data_result, index, jsonStr);
518 json jsonValue = json::parse(jsonStr);
519 std::vector<double> result = jsonValue;
526 const std::lock_guard<std::mutex> lock(m_mutex);
527 std::vector<uint8_t> data;
530 message[
"px"] = cam.
get_px();
531 message[
"py"] = cam.
get_py();
532 message[
"u0"] = cam.
get_u0();
533 message[
"v0"] = cam.
get_v0();
534 message[
"h"] = height;
535 message[
"w"] = width;
537 encode(data, message.dump());
540 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
542 std::vector<uint8_t> data_result;
543 std::tie(code, data_result) = readMessage();
545 handleWrongReturnMessage(code, data_result);
550 const std::vector<vpHomogeneousMatrix>&poses,
const std::string& viewType)
552 const std::lock_guard<std::mutex> lock(m_mutex);
553 std::vector<uint8_t> data;
555 j[
"labels"] = objectNames;
556 json cToJson = json::array();
559 to_megapose_json(j, cTo);
560 cToJson.push_back(j);
562 j[
"poses"] = cToJson;
563 j[
"type"] = viewType;
564 encode(data, j.dump());
566 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
568 std::vector<uint8_t> data_result;
569 std::tie(code, data_result) = readMessage();
572 handleWrongReturnMessage(code, data_result);
575 unsigned int index = 0;
576 decode(data_result, index, result);
582 const std::lock_guard<std::mutex> lock(m_mutex);
583 std::vector<uint8_t> data;
585 j[
"so3_grid_size"] = num;
586 encode(data, j.dump());
588 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
590 std::vector<uint8_t> data_result;
591 std::tie(code, data_result) = readMessage();
593 handleWrongReturnMessage(code, data_result);
599 const std::lock_guard<std::mutex> lock(m_mutex);
600 std::vector<uint8_t> data;
602 send(m_serverSocket,
reinterpret_cast<const char *
>(data.data()),
static_cast<int>(data.size()), 0);
604 std::vector<uint8_t> data_result;
605 std::tie(code, data_result) = readMessage();
607 handleWrongReturnMessage(code, data_result);
609 unsigned int index = 0;
611 decode(data_result, index, jsonStr);
612 json jsonValue = json::parse(jsonStr);
613 std::vector<std::string> result = jsonValue;
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.
Definition of the vpImage class member functions.
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.
@ GET_LIST_OBJECTS
Ask the server to set the number of samples for coarse estimation.
@ GET_VIZ
Code sent when server returns pose estimates.
@ SET_SO3_GRID_SIZE
Code sent when server returns a pose score.
@ RET_SCORE
Ask the server to score a pose estimate.
@ GET_POSE
Server has successfully completed operation, no return value expected.
@ RET_VIZ
Ask the server for a rendering of the object.
@ OK
An error occurred server side.
@ SET_INTR
Code sent when server returns the rendering of an object.
@ GET_SCORE
Set the intrinsics for the MegaPose server.
@ RET_POSE
Ask the server to estimate poses.
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.