Hello all
I have found a C++ example of a Multi Camera Setup using Fusion API. I tried recreating this example in Python, but there is one particular place in the code where I am not really sure how to translate that into Python, since I am relatively new to Python and it seems like some functions used in the C++ example are not existing on the Python API. This is the link to the post of the example: https://community.stereolabs.com/t/3d-fused-pose-reprojection-into-2d-plane-from-the-perspective-of-each-zed-2i-stereo-camera-in-a-multi-camera-setup/2472/6?u=prot_on
And this is the snippet where I am struggling:
inline sl::float3 warpPoint(sl::float3 pt_curr, float* p_path) {
sl::float3 proj3D;
proj3D.x = pt_curr.x * p_path[0] + pt_curr.y * p_path[1] + pt_curr.z * p_path[2] + p_path[3];
proj3D.y = pt_curr.x * p_path[4] + pt_curr.y * p_path[5] + pt_curr.z * p_path[6] + p_path[7];
proj3D.z = pt_curr.x * p_path[8] + pt_curr.y * p_path[9] + pt_curr.z * p_path[10] + p_path[11];
return proj3D;
}
std::vector<sl::BodyData> reproIntoCameras(std::vector<sl::BodyData> &data, sl::Transform cam_pose, sl::COORDINATE_SYSTEM coord_in) {
std::vector<sl::BodyData> out;
// by default the sample is not in IMAGE Coordinate system, use this function to convert it back
sl::Transform coordChange = sl::getCoordinateTransformConversion4f( coord_in, sl::COORDINATE_SYSTEM::IMAGE);
// for each body
for (auto& it : data) {
out.emplace_back();
// for each keypoint
for (auto kp : it.keypoint) {
// warp the keypoint from WORLD to the desired Camera frame
auto pt_in_cam_ = warpPoint(kp, cam_pose.m);
// change the coord system of the kp to fit in IMAGE
out.back().keypoint.push_back(warpPoint(pt_in_cam_, coordChange.m));
}
}
return out;
}
I somewhat know what this code is supposed to be doing, but it looks like the Python API does not have certain properties and functions( float3, getCoordinateTransformConversion4f,…)
This is what i have so far:
transformation_matrix = [
1, 0, 0, 0,
0, -1, 0, 0,
0, 0, 0, 0,
0, 0, 0, 1
]
def warp_point(pt_curr, p_path):
"""
proj3D = np.zeros(3, dtype=np.float32)
proj3D[0] = pt_curr[0] * p_path[0] + pt_curr[1] * p_path[1] + pt_curr[2] * p_path[2] + p_path[3]
proj3D[1] = pt_curr[0] * p_path[4] + pt_curr[1] * p_path[5] + pt_curr[2] * p_path[6] + p_path[7]
proj3D[2] = pt_curr[0] * p_path[8] + pt_curr[1] * p_path[9] + pt_curr[2] * p_path[10] + p_path[11]
return proj3D
"""
# Convert pt_curr to a 1x4 matrix with the last element as 1 (homogeneous coordinates)
pt_curr_homogeneous = np.array([pt_curr[0], pt_curr[1], pt_curr[2], 1], dtype=np.float32)
# Perform matrix multiplication
proj3D = np.dot(p_path, pt_curr_homogeneous)
# Return the first 3 elements (excluding the homogeneous coordinate)
return proj3D[:3]
def repro_into_camera(body_list, cam_pose, coord_in):
out = []
# by default the sample is not in IMAGE Coordinate system, use this function to convert it back
test: Matrix4f = Matrix4f(transformation_matrix)
coord_change: Transform = Transform(test)
for body_data in body_list:
key_point_list = []
# for each keypoint ( np.array[float][float] )
for kp in body_data.keypoint:
# warp the keypoint from WORLD to the desired Camera frame
pt_in_cam = warp_point(kp, cam_pose.m)
pt_coord_img = warp_point(pt_in_cam, coord_change)
key_point_list.append(pt_coord_img)
numpy_kp_array = np.array(key_point_list)
new_body_data = sl.BodyData()
# todo this is not working since keypoint is not writable
new_body_data.keypoint = numpy_kp_array
out.append(new_body_data)
return out
I think there are a couple of things that are not correct in my code and where i am quite unsure. I tried to manually define a transformation matrix since the C++ function is not available in python. And i also do not know how i can assign the newly calcualted keypoints to the BodyData since this seems to be not writable.
Any help would be appreciated to find out whats wrong with my code / how i should rewrite it.
Thanks in advance
. If you print the bodies detected by the senders, on the sender’s side, do you get that both detect a person?