Merge pull request #3462 from schmidtp1/wheel-odometry

renaming of wheel odometry API: uses translational velocity (measurem…

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
 }

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;
         }

src/core/motion.h

@@ -38,7 +38,7 @@ namespace librealsense
     {
     public:
         virtual bool load_wheel_odometery_config(const std::vector<uint8_t>& 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);

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)

src/tm2/tm-device.cpp

@@ -1134,15 +1134,15 @@ 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");
 
         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)

src/tm2/tm-device.h

@@ -92,7 +92,7 @@ namespace librealsense
 
         // Wheel odometer
         bool load_wheel_odometery_config(const std::vector<uint8_t>& 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,

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            */
         };
     };
-}
+}

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;

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);

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;
 
     /**

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: <filename>\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)

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, 

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)]

wrappers/python/python.cpp

@@ -827,7 +827,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);