Skip to content

Fix sensor batch output allocation #34

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
Daniiiil1 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fix sensor batch output allocation #34

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
1 change: 1 addition & 0 deletions include/sl/c_api/ZEDController.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -291,6 +291,7 @@ class ZEDController {

bool isSensorsBatchReady = false;
std::vector<sl::SensorsData> sensorsDataBatch;
std::vector<SL_SensorsData> sensorsDataBatchC;

bool saveTexture;
sl::Mesh mesh;
Expand Down
7 changes: 4 additions & 3 deletions include/sl/c_api/zed_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -935,9 +935,10 @@ extern "C" {
*/
INTERFACE_API int sl_get_sensors_data_batch_count(int camera_id, int* count);
/**
\brief Retrieves all SL_SensorsData associated to most recent grabbed frame in the specified \ref COORDINATE_SYSTEM of InitParameters.
\note sl_get_sensors_data_batch needs to be called before this function to retrieve the size of the imu batch array.
\param [out] data : The SensorsData array to store the data.
\brief Retrieves all SL_SensorsData associated to most recent grabbed frame in the specified \ref COORDINATE_SYSTEM of InitParameters.
\note sl_get_sensors_data_batch_count needs to be called before this function to retrieve the size of the imu batch array.
\note The returned array is owned by the API and remains valid until the next call to this function for the same camera.
\param [out] data : The SensorsData array to store the data.
\param camera_id : Id of the camera instance.
\return \ref SL_ERROR_CODE "SL_ERROR_CODE_SUCCESS" if sensors data have been extracted.
\return \ref SL_ERROR_CODE "SL_ERROR_CODE_SENSORS_NOT_AVAILABLE" if the camera model is a \ref SL_MODEL "SL_MODEL_ZED".
Expand Down
18 changes: 10 additions & 8 deletions src/ZEDController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -634,16 +634,18 @@ sl::ERROR_CODE ZEDController::getSensorsDataBatch(SL_SensorsData** data)
{
if (!isNull())
{
if (data == nullptr)
return sl::ERROR_CODE::INVALID_FUNCTION_PARAMETERS;

if (isSensorsBatchReady)
{
int size = sensorsDataBatch.size();

memset(*data, 0, sizeof(SL_SensorsData) * size);
sensorsDataBatchC.assign(size, {});
*data = sensorsDataBatchC.data();

for (int i = 0; i < size; i++)
{

SL_SensorsData* sensorData = data[i];
SL_SensorsData* sensorData = &sensorsDataBatchC[i];
sl::SensorsData tmp_sensor_data = sensorsDataBatch[i];

sensorData->camera_moving_state = (int)tmp_sensor_data.camera_moving_state;
Expand Down Expand Up @@ -672,10 +674,10 @@ sl::ERROR_CODE ZEDController::getSensorsDataBatch(SL_SensorsData** data)
sensorData->imu.orientation.z = tmp_sensor_data.imu.pose.getOrientation().z;
sensorData->imu.orientation.w = tmp_sensor_data.imu.pose.getOrientation().w;

for (int i = 0; i < 9; i++) {
sensorData->imu.angular_velocity_convariance.p[i] = tmp_sensor_data.imu.angular_velocity_covariance.r[i];
sensorData->imu.linear_acceleration_convariance.p[i] = tmp_sensor_data.imu.linear_acceleration_covariance.r[i];
sensorData->imu.orientation_covariance.p[i] = tmp_sensor_data.imu.pose_covariance.r[i];
for (int covariance_index = 0; covariance_index < 9; covariance_index++) {
sensorData->imu.angular_velocity_convariance.p[covariance_index] = tmp_sensor_data.imu.angular_velocity_covariance.r[covariance_index];
sensorData->imu.linear_acceleration_convariance.p[covariance_index] = tmp_sensor_data.imu.linear_acceleration_covariance.r[covariance_index];
sensorData->imu.orientation_covariance.p[covariance_index] = tmp_sensor_data.imu.pose_covariance.r[covariance_index];
}

///Barometer
Expand Down
5 changes: 3 additions & 2 deletions src/ZEDController.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -288,8 +288,9 @@ class ZEDController {
bool areTextureReady = false;
bool isTextured = false;

bool isSensorsBatchReady = false;
std::vector<sl::SensorsData> sensorsDataBatch;
bool isSensorsBatchReady = false;
std::vector<sl::SensorsData> sensorsDataBatch;
std::vector<SL_SensorsData> sensorsDataBatchC;

bool saveTexture;
sl::Mesh mesh;
Expand Down