From de5d84d1ac46669febbf968cac00acc61c792d7f Mon Sep 17 00:00:00 2001 From: Phillip Schmidt Date: Mon, 11 Mar 2019 16:00:45 -0700 Subject: [PATCH 1/5] renaming of wheel odometry API: uses translational velocity (measurements) --- include/librealsense2/h/rs_sensor.h | 4 ++-- include/librealsense2/hpp/rs_sensor.hpp | 6 +++--- src/core/motion.h | 2 +- src/rs.cpp | 6 +++--- src/tm2/tm-device.cpp | 4 ++-- src/tm2/tm-device.h | 2 +- third-party/libtm/libtm/include/TrackingData.h | 4 ++-- third-party/libtm/libtm/src/CompleteTask.h | 2 +- third-party/libtm/libtm/src/Device.cpp | 6 +++--- third-party/libtm/libtm/src/Message.h | 6 +++--- 10 files changed, 21 insertions(+), 21 deletions(-) diff --git a/include/librealsense2/h/rs_sensor.h b/include/librealsense2/h/rs_sensor.h index 0858dc3a18..ad03b8fc3b 100644 --- a/include/librealsense2/h/rs_sensor.h +++ b/include/librealsense2/h/rs_sensor.h @@ -519,11 +519,11 @@ int rs2_load_wheel_odometry_config(const rs2_sensor* sensor, const unsigned char /** Send wheel odometry data for each individual sensor (wheel) * \param[in] wo_sensor_id - Zero-based index of (wheel) sensor with the same type within device * \param[in] frame_num - Monotonocally increasing frame number, managed per sensor. -* \param[in] angular_velocity - Angular velocity of the wheel sensor [rad/sec] +* \param[in] translational_velocity - Translational velocity of the wheel sensor [meter/sec] * \return true on success */ int rs2_send_wheel_odometry(const rs2_sensor* sensor, char wo_sensor_id, unsigned int frame_num, - const rs2_vector angular_velocity, rs2_error** error); + const rs2_vector translational_velocity, rs2_error** error); #ifdef __cplusplus } diff --git a/include/librealsense2/hpp/rs_sensor.hpp b/include/librealsense2/hpp/rs_sensor.hpp index b9474b78d0..cb340a339a 100644 --- a/include/librealsense2/hpp/rs_sensor.hpp +++ b/include/librealsense2/hpp/rs_sensor.hpp @@ -542,13 +542,13 @@ namespace rs2 /** Send wheel odometry data for each individual sensor (wheel) * \param[in] wo_sensor_id - Zero-based index of (wheel) sensor with the same type within device * \param[in] frame_num - Monotonocally increasing frame number, managed per sensor. - * \param[in] angular_velocity - Angular velocity in rad/sec + * \param[in] translational_velocity - Translational velocity in meter/sec * \return true on success */ - bool send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const rs2_vector& angular_velocity) + bool send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const rs2_vector& translational_velocity) { rs2_error* e = nullptr; - auto res = rs2_send_wheel_odometry(_sensor.get(), wo_sensor_id, frame_num, angular_velocity, &e); + auto res = rs2_send_wheel_odometry(_sensor.get(), wo_sensor_id, frame_num, translational_velocity, &e); error::handle(e); return !!res; } diff --git a/src/core/motion.h b/src/core/motion.h index 196d1b98a7..53ce7a94f5 100644 --- a/src/core/motion.h +++ b/src/core/motion.h @@ -38,7 +38,7 @@ namespace librealsense { public: virtual bool load_wheel_odometery_config(const std::vector& odometry_config_buf) const = 0; - virtual bool send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const float3& angular_velocity) const = 0; + virtual bool send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const float3& translational_velocity) const = 0; virtual ~wheel_odometry_interface() = default; }; MAP_EXTENSION(RS2_EXTENSION_WHEEL_ODOMETER, librealsense::wheel_odometry_interface); diff --git a/src/rs.cpp b/src/rs.cpp index 8ac85a1270..b445ba31da 100644 --- a/src/rs.cpp +++ b/src/rs.cpp @@ -2307,11 +2307,11 @@ int rs2_load_wheel_odometry_config(const rs2_sensor* sensor, const unsigned char HANDLE_EXCEPTIONS_AND_RETURN(0, sensor, odometry_blob, blob_size) int rs2_send_wheel_odometry(const rs2_sensor* sensor, char wo_sensor_id, unsigned int frame_num, - const rs2_vector angular_velocity, rs2_error** error) BEGIN_API_CALL + const rs2_vector translational_velocity, rs2_error** error) BEGIN_API_CALL { VALIDATE_NOT_NULL(sensor); auto wo_snr = VALIDATE_INTERFACE(sensor->sensor, librealsense::wheel_odometry_interface); - return wo_snr->send_wheel_odometry(wo_sensor_id, frame_num, { angular_velocity.x, angular_velocity.y, angular_velocity.z }); + return wo_snr->send_wheel_odometry(wo_sensor_id, frame_num, { translational_velocity.x, translational_velocity.y, translational_velocity.z }); } -HANDLE_EXCEPTIONS_AND_RETURN(0, sensor, wo_sensor_id, frame_num, angular_velocity) +HANDLE_EXCEPTIONS_AND_RETURN(0, sensor, wo_sensor_id, frame_num, translational_velocity) diff --git a/src/tm2/tm-device.cpp b/src/tm2/tm-device.cpp index f47d495bb4..012687cc06 100644 --- a/src/tm2/tm-device.cpp +++ b/src/tm2/tm-device.cpp @@ -1128,7 +1128,7 @@ namespace librealsense return (status == Status::SUCCESS); } - bool tm2_sensor::send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const float3& angular_velocity) const + bool tm2_sensor::send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const float3& translational_velocity) const { if (!_tm_dev) throw wrong_api_call_sequence_exception("T2xx tracking device is not available"); @@ -1136,7 +1136,7 @@ namespace librealsense perc::TrackingData::VelocimeterFrame vel_fr; vel_fr.sensorIndex = wo_sensor_id; vel_fr.frameId = frame_num; - vel_fr.angularVelocity = { angular_velocity.x, angular_velocity.y, angular_velocity.z }; + vel_fr.translationalVelocity = { translational_velocity.x, translational_velocity.y, translational_velocity.z }; auto status = _tm_dev->SendFrame(vel_fr); if (status != Status::SUCCESS) diff --git a/src/tm2/tm-device.h b/src/tm2/tm-device.h index 6e5829b067..85ffdf2bdc 100644 --- a/src/tm2/tm-device.h +++ b/src/tm2/tm-device.h @@ -92,7 +92,7 @@ namespace librealsense // Wheel odometer bool load_wheel_odometery_config(const std::vector& odometry_config_buf) const ; - bool send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const float3& angular_velocity) const; + bool send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const float3& translational_velocity) const; enum async_op_state { _async_init = 1 << 0, diff --git a/third-party/libtm/libtm/include/TrackingData.h b/third-party/libtm/libtm/include/TrackingData.h index f3da075269..11aeae27a2 100644 --- a/third-party/libtm/libtm/include/TrackingData.h +++ b/third-party/libtm/libtm/include/TrackingData.h @@ -229,7 +229,7 @@ namespace perc VelocimeterFrame() : temperature(0), sensorIndex(0), frameId(0) {}; uint8_t sensorIndex; /**< Zero based index of sensor with the same type within device */ uint32_t frameId; /**< A running index of frames from every unique sensor, starting from 0 */ - Axis angularVelocity; /**< X, Y, Z values of velocimeter, in radians/sec */ + Axis translationalVelocity; /**< X, Y, Z values of velocimeter, in meters/sec */ float temperature; /**< Velocimeter temperature */ }; @@ -893,4 +893,4 @@ namespace perc uint8_t* image; /**< New controller FW image to be updated */ }; }; -} \ No newline at end of file +} diff --git a/third-party/libtm/libtm/src/CompleteTask.h b/third-party/libtm/libtm/src/CompleteTask.h index 26f42d950a..e6182b7681 100644 --- a/third-party/libtm/libtm/src/CompleteTask.h +++ b/third-party/libtm/libtm/src/CompleteTask.h @@ -320,7 +320,7 @@ namespace perc { interrupt_message_velocimeter_stream_metadata metaData = ((interrupt_message_velocimeter_stream*)imuHeader)->metadata; mFrame.sensorIndex = GET_SENSOR_INDEX(imuHeader->bSensorID); mFrame.frameId = imuHeader->dwFrameId; - mFrame.angularVelocity.set(metaData.flVx, metaData.flVy, metaData.flVz); + mFrame.translationalVelocity.set(metaData.flVx, metaData.flVy, metaData.flVz); mFrame.temperature = metaData.flTemperature; mFrame.timestamp = imuHeader->llNanoseconds; mFrame.arrivalTimeStamp = imuHeader->llArrivalNanoseconds; diff --git a/third-party/libtm/libtm/src/Device.cpp b/third-party/libtm/libtm/src/Device.cpp index 9206709bc3..ddd6cf8ae6 100644 --- a/third-party/libtm/libtm/src/Device.cpp +++ b/third-party/libtm/libtm/src/Device.cpp @@ -2213,9 +2213,9 @@ namespace perc { req->metadata.dwMetadataLength = offsetof(bulk_message_velocimeter_stream_metadata, dwFrameLength) - sizeof(req->metadata.dwMetadataLength); req->metadata.flTemperature = frame.temperature; req->metadata.dwFrameLength = sizeof(bulk_message_velocimeter_stream_metadata) - offsetof(bulk_message_velocimeter_stream_metadata, dwFrameLength) - sizeof(req->metadata.dwFrameLength); - req->metadata.flVx = frame.angularVelocity.x; - req->metadata.flVy = frame.angularVelocity.y; - req->metadata.flVz = frame.angularVelocity.z; + req->metadata.flVx = frame.translationalVelocity.x; + req->metadata.flVy = frame.translationalVelocity.y; + req->metadata.flVz = frame.translationalVelocity.z; int actual; auto rc = libusb_bulk_transfer(mDevice, mStreamEndpoint | TO_DEVICE, (unsigned char*)req, (int)buf.size(), &actual, 100); diff --git a/third-party/libtm/libtm/src/Message.h b/third-party/libtm/libtm/src/Message.h index e28505d395..b3d246385a 100644 --- a/third-party/libtm/libtm/src/Message.h +++ b/third-party/libtm/libtm/src/Message.h @@ -1336,9 +1336,9 @@ namespace perc uint32_t dwMetadataLength; /**< Metadata length in bytes (4 bytes) */ float_t flTemperature; /**< velocimeter temperature */ uint32_t dwFrameLength; /**< Length of frame below (12 bytes) */ - float_t flVx; /**< X value of velocimeter, in radians/sec */ - float_t flVy; /**< Y value of velocimeter, in radians/sec */ - float_t flVz; /**< Z value of velocimeter, in radians/sec */ + float_t flVx; /**< X value of velocimeter, in meters/sec */ + float_t flVy; /**< Y value of velocimeter, in meters/sec */ + float_t flVz; /**< Z value of velocimeter, in meters/sec */ } bulk_message_velocimeter_stream_metadata; /** From 7495ae7dbbc242e2e81c599dbb6d9da0f8cc3932 Mon Sep 17 00:00:00 2001 From: Phillip Schmidt Date: Mon, 11 Mar 2019 16:49:03 -0700 Subject: [PATCH 2/5] renaming: include libtm_util --- .../libtm/tools/libtm_util/libtm_util.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/third-party/libtm/tools/libtm_util/libtm_util.cpp b/third-party/libtm/tools/libtm_util/libtm_util.cpp index e69a746cbe..dd3c17c847 100644 --- a/third-party/libtm/tools/libtm_util/libtm_util.cpp +++ b/third-party/libtm/tools/libtm_util/libtm_util.cpp @@ -1579,7 +1579,7 @@ class CommonListener : public TrackingManager::Listener, public TrackingDevice:: gStatistics.inc(message); LOGV("Got Velocimeter[%u] frame (%" PRId64 "): Timestamp %" PRId64 ", FrameID = %d, Temperature = %.0f, AngularVelocity[%f, %f, %f]", message.sensorIndex, gStatistics.velocimeter[message.sensorIndex].frames, - message.timestamp, message.frameId, message.temperature, message.angularVelocity.x, message.angularVelocity.y, message.angularVelocity.z); + message.timestamp, message.frameId, message.temperature, message.translationalVelocity.x, message.translationalVelocity.y, message.translationalVelocity.z); if (gConfiguration.statistics == true) { @@ -1588,7 +1588,7 @@ class CommonListener : public TrackingManager::Listener, public TrackingDevice:: velocimeterCSV << std::fixed << unsigned(message.sensorIndex) << "," << gStatistics.velocimeter[message.sensorIndex].frames << "," << timeStamp.hostCurrentSystemTime << "," << message.systemTimestamp << "," << timeStamp.fwTimeStamp << "," << timeStamp.arrivalTimeStamp << "," << timeStamp.latency << "," - << message.frameId << "," << message.temperature << "," << message.angularVelocity.x << "," << message.angularVelocity.y << "," << message.angularVelocity.z << "\n"; + << message.frameId << "," << message.temperature << "," << message.translationalVelocity.x << "," << message.translationalVelocity.y << "," << message.translationalVelocity.z << "\n"; } }; @@ -2998,7 +2998,7 @@ int parseArguments(int argc, char *argv[]) printf(" Parameters: \n"); printf(" Example: \"libtm_util.exe -velocimeter velocimeterfile.csv\" : Enable Velocimeter and sends all velocimeter frames input file to the FW\n"); printf(" Velocimeter file must include the following line(s) in the following pattern:\n"); - printf(" FrameId,angularVelocity.X,angularVelocity.Y,angularVelocity.Z,timestamp,arrivaltimeStamp\n"); + printf(" FrameId,translationalVelocity.X,translationalVelocity.Y,translationalVelocity.Z,timestamp,arrivaltimeStamp\n"); printf(" File example: 0,1.0,2.0,3.0,0,0 : Velocimeter frame ID 0 with velocity (1.0,2.0,3.0) and timestamp 0\n"); return -1; } @@ -3387,7 +3387,7 @@ void saveOutput() std::ostringstream csvHeader; csvHeader << "Velocimeter Index," << "Frame Number," << "Host Timestamp (NanoSec)," << "Host Correlated to FW Timestamp (NanoSec)," << "FW Timestamp (NanoSec)," << "Arrival Timestamp (NanoSec)," << "FW to Host Latency (NanoSec)," - << "Frame Id," << "Temperature (Celsius)," << "Angular Velocity X (Radians/Sec)," << "Angular Velocity Y (Radians/Sec)," << "Angular Velocity Z (Radians/Sec)," << "\n"; + << "Frame Id," << "Temperature (Celsius)," << "Translational Velocity X (Meters/Sec)," << "Translational Velocity Y (Meters/Sec)," << "Translational Velocity Z (Meters/Sec)," << "\n"; static int velocimeterCount = 0; std::string fileHeaderName(gFileHeaderName); @@ -3535,7 +3535,7 @@ void printSupportedProfiles(TrackingData::Profile& profile) for (uint8_t i = 0; i < VelocimeterProfileMax; i++) { - LOGD("%02d | Velocimeter | 0x%02X | %01d | %-6d | %-5d | %-6d | %-5d | %-5d | %-7d | %d", totalProfiles, SensorType::Velocimeter, profile.gyro[i].sensorIndex, + LOGD("%02d | Velocimeter | 0x%02X | %01d | %-6d | %-5d | %-6d | %-5d | %-5d | %-7d | %d", totalProfiles, SensorType::Velocimeter, profile.velocimeter[i].sensorIndex, 0, 0, 0, 0, 0, profile.velocimeter[i].enabled, profile.velocimeter[i].outputEnabled); totalProfiles++; } @@ -4214,7 +4214,7 @@ int main(int argc, char *argv[]) { std::string cell; uint32_t frameId = 0; - float_t angularVelocity[3] = { 0 }; + float_t translationalVelocity[3] = { 0 }; int64_t timestamp[2] = { 0 }; uint32_t i = 0; @@ -4226,7 +4226,7 @@ int main(int argc, char *argv[]) } else if ((i == 1) || (i == 2) || (i == 3)) { - angularVelocity[i - 1] = stof(cell.c_str()); + translationalVelocity[i - 1] = stof(cell.c_str()); } else { @@ -4238,13 +4238,13 @@ int main(int argc, char *argv[]) TrackingData::VelocimeterFrame frame; frame.frameId = frameId; - frame.angularVelocity.x = angularVelocity[0]; - frame.angularVelocity.y = angularVelocity[1]; - frame.angularVelocity.z = angularVelocity[2]; + frame.translationalVelocity.x = translationalVelocity[0]; + frame.translationalVelocity.y = translationalVelocity[1]; + frame.translationalVelocity.z = translationalVelocity[2]; frame.timestamp = timestamp[0]; frame.arrivalTimeStamp = timestamp[1]; - LOGD("Sending velocimeter frame[%d]: AngularVelocity[%f, %f, %f], Timestamp %" PRId64 ", ArrivalTimestamp %" PRId64 "", frame.frameId, frame.angularVelocity.x, frame.angularVelocity.y, frame.angularVelocity.z, frame.timestamp, frame.arrivalTimeStamp); + LOGD("Sending velocimeter frame[%d]: TranslationalVelocity[%f, %f, %f], Timestamp %" PRId64 ", ArrivalTimestamp %" PRId64 "", frame.frameId, frame.translationalVelocity.x, frame.translationalVelocity.y, frame.translationalVelocity.z, frame.timestamp, frame.arrivalTimeStamp); status = gDevice->SendFrame(frame); if (status != Status::SUCCESS) From d7bec50d86a314a8eb3e3422f61b00c8eaabaab6 Mon Sep 17 00:00:00 2001 From: Phillip Schmidt Date: Mon, 11 Mar 2019 17:08:02 -0700 Subject: [PATCH 3/5] calibration_odometry: example file for fusion of translational velocity of robot base --- .../resources/calibration_odometry.json | 44 ++----------------- 1 file changed, 4 insertions(+), 40 deletions(-) diff --git a/unit-tests/resources/calibration_odometry.json b/unit-tests/resources/calibration_odometry.json index 673ed66d7a..3ccf7824c7 100644 --- a/unit-tests/resources/calibration_odometry.json +++ b/unit-tests/resources/calibration_odometry.json @@ -2,58 +2,22 @@ "velocimeters": [ { "scale_and_alignment": [ - 0.034, - 0.0000000000000000, - 0.0000000000000000, - 0.0000000000000000, 1.0, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000, - 1.0 - ], - "noise_variance": 0.0001511342857142857, - "extrinsics": { - "T": [ - 0.1037, - -0.0898, - -0.1631 - ], - "T_variance": [ - 9.999999974752427e-7, - 9.999999974752427e-7, - 9.999999974752427e-7 - ], - "W": [ - -1.1155, - -1.1690, - -1.2115 - ], - "W_variance": [ - 9.999999974752427e-5, - 9.999999974752427e-5, - 9.999999974752427e-5 - ] - } - }, - { - "scale_and_alignment": [ - 0.034, - 0.0000000000000000, - 0.0000000000000000, - 0.0000000000000000, 1.0, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000, 1.0 ], - "noise_variance": 0.0001511342857142857, + "noise_variance": 0.004445126050420168, "extrinsics": { "T": [ - -0.1461, - -0.1014, - -0.1631 + -0.5059, + -0.6294, + -0.6873 ], "T_variance": [ 9.999999974752427e-7, From 4969af44c22b26f9211f24aa0c2be8cb96d7a821 Mon Sep 17 00:00:00 2001 From: Phillip Schmidt Date: Tue, 19 Mar 2019 09:18:03 -0700 Subject: [PATCH 4/5] python: rename wheel odometry API (variable) --- wrappers/python/python.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrappers/python/python.cpp b/wrappers/python/python.cpp index 591bbd3607..aeb571403e 100644 --- a/wrappers/python/python.cpp +++ b/wrappers/python/python.cpp @@ -821,7 +821,7 @@ PYBIND11_MODULE(NAME, m) { .def("load_wheel_odometery_config", &rs2::wheel_odometer::load_wheel_odometery_config, "odometry_config_buf"_a, "Load Wheel odometer settings from host to device.") .def("send_wheel_odometry", &rs2::wheel_odometer::send_wheel_odometry, - "wo_sensor_id"_a, "frame_num"_a, "angular_velocity"_a, + "wo_sensor_id"_a, "frame_num"_a, "translational_velocity"_a, "Send wheel odometry data for each individual sensor (wheel)") .def("__nonzero__", &rs2::wheel_odometer::operator bool); From fcfe9c21537b3dcca5af0c20f35dc0fb41866219 Mon Sep 17 00:00:00 2001 From: Phillip Schmidt Date: Tue, 19 Mar 2019 09:43:46 -0700 Subject: [PATCH 5/5] c#: rename wheel odometry API (variable) --- wrappers/csharp/Intel.RealSense/NativeMethods.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrappers/csharp/Intel.RealSense/NativeMethods.cs b/wrappers/csharp/Intel.RealSense/NativeMethods.cs index 7ef53b5474..63b50d85c9 100644 --- a/wrappers/csharp/Intel.RealSense/NativeMethods.cs +++ b/wrappers/csharp/Intel.RealSense/NativeMethods.cs @@ -513,7 +513,7 @@ internal static extern int rs2_get_static_node(IntPtr sensor, [MarshalAs(Unmanag internal static extern int rs2_load_wheel_odometry_config(IntPtr sensor, IntPtr wheel_odometry_cfg_buf, uint size_of_buf, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] - internal static extern int rs2_send_wheel_odometry(IntPtr sensor, byte wo_sensor_id, uint frame_num, Math.Vector angular_velocity, + internal static extern int rs2_send_wheel_odometry(IntPtr sensor, byte wo_sensor_id, uint frame_num, Math.Vector translational_velocity, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)]