Skip to content

fix(sdk): harden QR/Aruco APIs and detection wrappers #124

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
JoDavidsson wants to merge 1 commit into
base: main
Choose a base branch
from
Open

fix(sdk): harden QR/Aruco APIs and detection wrappers #124

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 14 additions & 1 deletion sdk/include/Posemesh/Portals.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
#define __PORTALS_HPP__

#include <algorithm>
#include <cctype>
#include <string>

namespace psm {
Expand All @@ -24,7 +25,19 @@ class Portals final {

static std::string extractShortId(const std::string& portalContents)
{
return portalContents.substr(m_aukiLighthousePrefix.size());
auto it = std::search(
portalContents.begin(),
portalContents.end(),
m_aukiLighthousePrefix.begin(),
m_aukiLighthousePrefix.end(),
[](unsigned char ch1, unsigned char ch2) {
return std::toupper(ch1) == std::toupper(ch2);
});
if (it == portalContents.end()) {
return portalContents;
}
it += m_aukiLighthousePrefix.size();
return std::string(it, portalContents.end());
}

private:
Expand Down
7 changes: 5 additions & 2 deletions sdk/platform/Web/ArucoDetection.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,18 +41,21 @@ __internalPosemeshAPI.builderFunctions.push(function() {
};

__internalPosemesh.ArucoDetection.detectArucoFromLuminanceLandmarkObservations = function(imageBytes, width, height, markerFormat) {
let imageBytesVector = undefined;
let imageBytesVector = undefined, observations = undefined;
try {
imageBytesVector = __internalPosemeshAPI.toVectorUint8(imageBytes)
let result = []
let observations = __internalPosemesh.ArucoDetection.__detectArucoFromLuminanceLandmarkObservations(imageBytesVector, width, height, markerFormat)
observations = __internalPosemesh.ArucoDetection.__detectArucoFromLuminanceLandmarkObservations(imageBytesVector, width, height, markerFormat)

for (let i = 0; i < observations.size(); i++) {
result.push(observations.get(i))
}

return result;
} finally {
if (observations) {
observations.delete();
}
if (imageBytesVector) {
imageBytesVector.delete();
}
Expand Down
7 changes: 5 additions & 2 deletions sdk/platform/Web/QRDetection.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,18 +41,21 @@ __internalPosemeshAPI.builderFunctions.push(function() {
};

__internalPosemesh.QRDetection.detectQRFromLuminanceLandmarkObservations = function(imageBytes, width, height) {
let imageBytesVector = undefined;
let imageBytesVector = undefined, observations = undefined;
try {
imageBytesVector = __internalPosemeshAPI.toVectorUint8(imageBytes)
let result = []
let observations = __internalPosemesh.QRDetection.__detectQRFromLuminanceLandmarkObservations(imageBytesVector, width, height)
observations = __internalPosemesh.QRDetection.__detectQRFromLuminanceLandmarkObservations(imageBytesVector, width, height)

for (let i = 0; i < observations.size(); i++) {
result.push(observations.get(i))
}

return result;
} finally {
if (observations) {
observations.delete();
}
if (imageBytesVector) {
imageBytesVector.delete();
}
Expand Down
17 changes: 14 additions & 3 deletions sdk/src/ArucoDetection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,13 +71,19 @@ bool ArucoDetection::detectArucoFromLuminance(
std::vector<std::string>& outContents,
std::vector<Vector2>& outCorners)
{
outContents.clear();
outCorners.clear();

try {
if (width <= 0) {
throw std::invalid_argument("width");
}
if (height <= 0) {
throw std::invalid_argument("height");
}
if (!imageBytes) {
throw std::invalid_argument("imageBytes");
}
if (imageBytesSize != width * height * sizeof(std::uint8_t)) {
throw std::invalid_argument("imageBytesSize");
}
Expand All @@ -102,7 +108,7 @@ bool ArucoDetection::detectArucoFromLuminance(
outContents.reserve(contentsFound.size());

outCorners.clear();
outCorners.reserve(cornersFound.size());
outCorners.reserve(cornersFound.size() * 4);

for (std::size_t i = 0; i < contentsFound.size(); ++i) {
outContents.push_back(std::to_string(contentsFound[i]));
Expand Down Expand Up @@ -207,10 +213,15 @@ std::vector<LandmarkObservation> ArucoDetection::detectArucoFromLuminance(
std::vector<LandmarkObservation> observations;

if (detected) {
for (int i = 0; i < outContents.size(); i++) {
if (outCorners.size() < outContents.size() * 4) {
std::cerr << "ArucoDetection::detectArucoFromLuminance(): Observation corner count mismatch." << std::endl;
return observations;
}
for (std::size_t i = 0; i < outContents.size(); i++) {
const std::size_t cornerBaseIndex = i * 4;
for (int j = 0; j < 4; j++) {
LandmarkObservation l;
l.setPosition(outCorners[i * 4 + j]);
l.setPosition(outCorners[cornerBaseIndex + j]);
l.setType(arucoMarkerFormatToString(markerFormat));
std::string content = outContents[i];
if (Portals::isAukiQR(content)) {
Expand Down
46 changes: 39 additions & 7 deletions sdk/src/C/ArucoDetection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,19 @@ bool PSM_API psm_aruco_detection_detect_aruco(
assert(!"psm_aruco_detection_detect_aruco(): out_corners is null");
return false;
}
*out_contents = nullptr;
if (out_contents_count) {
*out_contents_count = 0;
}
*out_corners = nullptr;
if (out_corners_count) {
*out_corners_count = 0;
}
if (!image_bytes && image_bytes_size != 0) {
assert(!"psm_aruco_detection_detect_aruco(): image_bytes is null and image_bytes_size is non-zero");
return false;
}

std::vector<std::string> contents;
std::vector<psm::Vector2> corners;
const bool result = psm::ArucoDetection::detectArucoFromLuminance(image_bytes, image_bytes_size, width, height, (psm::ArucoMarkerFormat)marker_format, contents, corners);
Expand All @@ -43,12 +56,15 @@ bool PSM_API psm_aruco_detection_detect_aruco(
contents_buffer_size += content.size() + 1;
}
std::unique_ptr<char[]> contents_buffer(new (std::nothrow) char[contents_buffer_size]);
if (!contents_buffer) {
return false;
}
char** contents_prefix_ptr = reinterpret_cast<char**>(contents_buffer.get());
char* contents_content_ptr = contents_buffer.get() + contents_prefix_offset;
for (const auto& content : contents) {
*contents_prefix_ptr = contents_content_ptr;
contents_prefix_ptr++;
std::memcpy(contents_content_ptr, content.data(), content.size() + 1);
std::memcpy(contents_content_ptr, content.c_str(), content.size() + 1);
contents_content_ptr += content.size() + 1;
}
*contents_prefix_ptr = nullptr;
Expand All @@ -63,6 +79,9 @@ bool PSM_API psm_aruco_detection_detect_aruco(
const auto corners_prefix_offset = corners_buffer_size;
corners_buffer_size += corners.size() * sizeof(psm_vector2_t);
std::unique_ptr<char[]> corners_buffer(new (std::nothrow) char[corners_buffer_size]);
if (!corners_buffer) {
return false;
}
psm_vector2_t** corners_prefix_ptr = reinterpret_cast<psm_vector2_t**>(corners_buffer.get());
psm_vector2_t* corners_content_ptr = reinterpret_cast<psm_vector2_t*>(corners_buffer.get() + corners_prefix_offset);
for (auto& corner : corners) {
Expand All @@ -88,7 +107,7 @@ void PSM_API psm_aruco_detection_detect_aruco_free(const char* const* contents,
{
delete[] const_cast<char*>(reinterpret_cast<const char*>(contents));
if (corners) {
for (const auto* const* corner = corners; corner; ++corner) {
for (const auto* const* corner = corners; *corner; ++corner) {
(*corner)->~Vector2();
}
delete[] const_cast<char*>(reinterpret_cast<const char*>(corners));
Expand All @@ -108,9 +127,19 @@ bool PSM_API psm_aruco_detection_detect_aruco_landmark_observations(
assert(!"psm_aruco_detection_detect_aruco_landmark_observations(): out_observations is null");
return false;
}
std::vector<std::string> contents;
std::vector<psm::Vector2> corners;
std::vector<uint8_t> image_bytes_vector(image_bytes, image_bytes + image_bytes_size);
*out_observations = nullptr;
if (out_observations_count) {
*out_observations_count = 0;
}
if (!image_bytes && image_bytes_size != 0) {
assert(!"psm_aruco_detection_detect_aruco_landmark_observations(): image_bytes is null and image_bytes_size is non-zero");
return false;
}

std::vector<uint8_t> image_bytes_vector;
if (image_bytes_size > 0) {
image_bytes_vector.assign(image_bytes, image_bytes + image_bytes_size);
}
const std::vector<psm::LandmarkObservation> observations = psm::ArucoDetection::detectArucoFromLuminance(image_bytes_vector, width, height, (psm::ArucoMarkerFormat)marker_format);
if (observations.size() == 0) {
return false;
Expand All @@ -126,7 +155,10 @@ bool PSM_API psm_aruco_detection_detect_aruco_landmark_observations(
landmark_observation_buffer_size = ((landmark_observation_buffer_size + alignof(psm_landmark_observation_t) - 1) / alignof(psm_landmark_observation_t)) * alignof(psm_landmark_observation_t); // Ensure alignment
const auto landmark_observation_prefix_offset = landmark_observation_buffer_size;
landmark_observation_buffer_size += observations.size() * sizeof(psm_landmark_observation_t);
std::unique_ptr<psm_landmark_observation_t[]> landmark_observation_buffer(new (std::nothrow) psm_landmark_observation_t[landmark_observation_buffer_size]);
std::unique_ptr<char[]> landmark_observation_buffer(new (std::nothrow) char[landmark_observation_buffer_size]);
if (!landmark_observation_buffer) {
return false;
}
psm_landmark_observation_t** landmark_observation_prefix_ptr = reinterpret_cast<psm_landmark_observation_t**>(landmark_observation_buffer.get());
psm_landmark_observation_t* landmark_observation_content_ptr = reinterpret_cast<psm_landmark_observation_t*>(landmark_observation_buffer.get() + landmark_observation_prefix_offset);
for (auto& observation : observations) {
Expand All @@ -150,6 +182,6 @@ void PSM_API psm_aruco_detection_detect_aruco_landmark_observations_free(const p
for (const auto* const* observation = observations; *observation; ++observation) {
(*observation)->~LandmarkObservation();
}
delete[] const_cast<psm_landmark_observation_t*>(reinterpret_cast<const psm_landmark_observation_t*>(observations));
delete[] const_cast<char*>(reinterpret_cast<const char*>(observations));
}
}
7 changes: 7 additions & 0 deletions sdk/src/C/CalibrationHelpers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,16 @@

const psm_matrix4x4_t* psm_calibration_helpers_get_calibration_matrix(psm_pose_t* domain, psm_pose_t* observed, bool only_rotate_around_y)
{
if (!domain || !observed) {
return nullptr;
}

psm::Matrix4x4 calibrationMatrix = psm::CalibrationHelpers::getCalibrationMatrix(*static_cast<const psm::Pose*>(domain), *static_cast<const psm::Pose*>(observed), only_rotate_around_y);

psm_matrix4x4_t* result = psm_matrix4x4_create();
if (!result) {
return nullptr;
}
psm_matrix4x4_set_m00(result, calibrationMatrix.getM00());
psm_matrix4x4_set_m10(result, calibrationMatrix.getM10());
psm_matrix4x4_set_m20(result, calibrationMatrix.getM20());
Expand Down
22 changes: 22 additions & 0 deletions sdk/src/C/Config.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,9 @@ const char* const* psm_config_get_bootstraps(const psm_config_t* config, uint32_
buffer_size += bootstrap.size() + 1;
}
char* buffer = new (std::nothrow) char[buffer_size];
if (!buffer) {
return nullptr;
}
char** prefix_ptr = reinterpret_cast<char**>(buffer);
char* content_ptr = buffer + prefix_offset;
for (const auto& bootstrap : bootstraps) {
Expand Down Expand Up @@ -128,6 +131,9 @@ const char* const* psm_config_get_relays(const psm_config_t* config, uint32_t* o
buffer_size += relay.size() + 1;
}
char* buffer = new (std::nothrow) char[buffer_size];
if (!buffer) {
return nullptr;
}
char** prefix_ptr = reinterpret_cast<char**>(buffer);
char* content_ptr = buffer + prefix_offset;
for (const auto& relay : relays) {
Expand Down Expand Up @@ -195,6 +201,12 @@ const uint8_t* psm_config_get_private_key(const psm_config_t* config, uint32_t*
}

auto* result = new (std::nothrow) std::uint8_t[private_key.size()];
if (!result) {
if (out_private_key_size) {
*out_private_key_size = 0;
}
return nullptr;
}
std::memcpy(result, private_key.data(), private_key.size());
if (out_private_key_size) {
*out_private_key_size = static_cast<std::uint32_t>(private_key.size());
Expand All @@ -217,6 +229,10 @@ void psm_config_set_private_key(psm_config_t* config, const uint8_t* private_key
assert(!"psm_config_set_private_key(): private_key is null and private_key_size is non-zero");
return;
}
if (!private_key || private_key_size == 0) {
config->setPrivateKey(std::vector<std::uint8_t> {});
return;
}
config->setPrivateKey(std::vector<std::uint8_t> { private_key, private_key + private_key_size });
}

Expand All @@ -234,6 +250,9 @@ const char* PSM_API psm_config_get_private_key_path(const psm_config_t* config)
}

auto* result = new (std::nothrow) char[private_key_path.size() + 1];
if (!result) {
return nullptr;
}
std::memcpy(result, private_key_path.c_str(), private_key_path.size() + 1);
return result;
}
Expand Down Expand Up @@ -284,6 +303,9 @@ const char* PSM_API psm_config_get_name(const psm_config_t* config)
}

auto* result = new (std::nothrow) char[name.size() + 1];
if (!result) {
return nullptr;
}
std::memcpy(result, name.c_str(), name.size() + 1);
return result;
}
Expand Down
16 changes: 16 additions & 0 deletions sdk/src/C/PoseEstimation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,13 +10,29 @@ enum psm_pose_estimation_solve_pnp_result psm_pose_estimation_solve_pnp(
psm_pose_t* out_pose,
psm_solve_pnp_method_e method)
{
if (!landmarks || landmarks_count <= 0) {
return PSM_POSE_ESTIMATION_SOLVE_PNP_RESULT_FAILED;
}
if (!landmark_observations || landmark_observations_count <= 0) {
return PSM_POSE_ESTIMATION_SOLVE_PNP_RESULT_FAILED;
}
if (!camera_matrix || !out_pose) {
return PSM_POSE_ESTIMATION_SOLVE_PNP_RESULT_FAILED;
}

std::vector<psm::Landmark> l(landmarks_count);
for (int i = 0; i < landmarks_count; i++) {
if (!landmarks[i]) {
return PSM_POSE_ESTIMATION_SOLVE_PNP_RESULT_FAILED;
}
l[i] = *(landmarks[i]);
}

std::vector<psm::LandmarkObservation> lo(landmark_observations_count);
for (int i = 0; i < landmark_observations_count; i++) {
if (!landmark_observations[i]) {
return PSM_POSE_ESTIMATION_SOLVE_PNP_RESULT_FAILED;
}
lo[i] = *(landmark_observations[i]);
}

Expand Down
Loading