Merge pull request #4650 from honpong/tm2-calibration-write-api

Tm2 calibration write api

include/librealsense2/h/rs_device.h

@@ -146,6 +146,21 @@ void rs2_connect_tm2_controller(const rs2_device* device, const unsigned char* m
 */
 void rs2_disconnect_tm2_controller(const rs2_device* device, int id, rs2_error** error);
 
+
+/** 
+* Reset device to factory calibration
+* \param[in] device       The RealSense device
+* \param[out] error       If non-null, receives any error that occurs during this call, otherwise, errors are ignored
+*/
+void rs2_reset_to_factory_calibration(const rs2_device* device, rs2_error** e);
+
+/**
+* Write calibration to device's EEPROM
+* \param[in] device       The RealSense device
+* \param[out] error       If non-null, receives any error that occurs during this call, otherwise, errors are ignored
+*/
+void rs2_write_calibration(const rs2_device* device, rs2_error** e);
+
 /**
 * Update device to the provided firmware, the device must be extendable to RS2_EXTENSION_UPDATABLE.
 * This call is executed on the caller's thread and it supports progress notifications via the optional callback.

include/librealsense2/h/rs_sensor.h

@@ -530,6 +530,36 @@ int rs2_load_wheel_odometry_config(const rs2_sensor* sensor, const unsigned char
 int rs2_send_wheel_odometry(const rs2_sensor* sensor, char wo_sensor_id, unsigned int frame_num,
     const rs2_vector translational_velocity, rs2_error** error);
 
+/**
+* Set intrinsics of a given sensor
+* \param[in] sensor       The RealSense device
+* \param[in] profile      Target stream profile
+* \param[in] intrinsics   Intrinsics value to be written to the device
+* \param[out] error       If non-null, receives any error that occurs during this call, otherwise, errors are ignored
+*/
+void rs2_set_intrinsics(const rs2_sensor* sensor, const rs2_stream_profile* profile , const rs2_intrinsics* intrinsics, rs2_error** error);
+
+/**
+ * Set extrinsics between two sensors
+ * \param[in]  from_sensor  Origin sensor
+ * \param[in]  from_profile Origin profile
+ * \param[in]  to_sensor    Target sensor
+ * \param[in]  to_profile   Target profile
+ * \param[out] extrinsics   Extrinsics from origin to target
+ * \param[out] error        If non-null, receives any error that occurs during this call, otherwise, errors are ignored
+ */
+void rs2_set_extrinsics(const rs2_sensor* from_sensor, const rs2_stream_profile* from_profile, rs2_sensor* to_sensor, const rs2_stream_profile* to_profile, const rs2_extrinsics* extrinsics, rs2_error** error);
+
+/**
+* Set motion device intrinsics
+* \param[in]  sensor       Motion sensor 
+* \param[in]  profile      Motion stream profile
+* \param[out] intrinsics   Pointer to the struct to store the data in
+* \param[out] error        If non-null, receives any error that occurs during this call, otherwise, errors are ignored
+*/
+void rs2_set_motion_device_intrinsics(const rs2_sensor* sensor, const rs2_stream_profile* profile, const rs2_motion_device_intrinsic* intrinsics, rs2_error** error);
+
+
 #ifdef __cplusplus
 }
 #endif

include/librealsense2/h/rs_types.h

@@ -170,6 +170,7 @@ typedef enum rs2_extension
     RS2_EXTENSION_GLOBAL_TIMER,
     RS2_EXTENSION_UPDATABLE,
     RS2_EXTENSION_UPDATE_DEVICE,
+    RS2_EXTENSION_TM2_SENSOR,
     RS2_EXTENSION_COUNT
 } rs2_extension;
 const char* rs2_extension_type_to_string(rs2_extension type);

include/librealsense2/hpp/rs_device.hpp

@@ -489,6 +489,85 @@ namespace rs2
             rs2_disconnect_tm2_controller(_dev.get(), id, &e);
             error::handle(e);
         }
+
+        /**
+        * Set tm2 camera intrinsics
+        * \param[in] fisheye_senor_id The ID of the fisheye sensor
+        * \param[in] intrinsics       value to be written to the device
+        */
+        void set_intrinsics(int fisheye_sensor_id, const rs2_intrinsics& intrinsics)
+        {
+            rs2_error* e = nullptr;
+            auto fisheye_sensor = get_sensor_profile(RS2_STREAM_FISHEYE, fisheye_sensor_id);
+            rs2_set_intrinsics(fisheye_sensor.first.get().get(), fisheye_sensor.second.get(), &intrinsics, &e);
+            error::handle(e);
+        }
+
+        /**
+        * Set tm2 camera extrinsics
+        * \param[in] from_stream     only support RS2_STREAM_FISHEYE
+        * \param[in] from_id         only support left fisheye = 1
+        * \param[in] to_stream       only support RS2_STREAM_FISHEYE
+        * \param[in] to_id           only support left fisheye = 2
+        * \param[in] extrinsics      extrinsics value to be written to the device
+        */
+        void set_extrinsics(rs2_stream from_stream, int from_id, rs2_stream to_stream, int to_id, rs2_extrinsics& extrinsics)
+        {
+            rs2_error* e = nullptr;
+            auto from_sensor = get_sensor_profile(from_stream, from_id);
+            auto to_sensor   = get_sensor_profile(to_stream, to_id);
+            rs2_set_extrinsics(from_sensor.first.get().get(), from_sensor.second.get(), to_sensor.first.get().get(), to_sensor.second.get(), &extrinsics, &e);
+            error::handle(e);
+        }
+
+        /** 
+        * Set tm2 motion device intrinsics
+        * \param[in] stream_type       stream type of the motion device
+        * \param[in] motion_intriniscs intrinsics value to be written to the device
+        */
+        void set_motion_device_intrinsics(rs2_stream stream_type, const rs2_motion_device_intrinsic& motion_intriniscs)
+        {
+            rs2_error* e = nullptr;
+            auto motion_sensor = get_sensor_profile(stream_type, 0);
+            rs2_set_motion_device_intrinsics(motion_sensor.first.get().get(), motion_sensor.second.get(), &motion_intriniscs, &e);
+            error::handle(e);
+        }
+
+        /**
+        * Reset tm2 to factory calibration
+        * \param[in] device       tm2 device
+        * \param[out] error       If non-null, receives any error that occurs during this call, otherwise, errors are ignored
+        */
+        void reset_to_factory_calibration()
+        {
+            rs2_error* e = nullptr;
+            rs2_reset_to_factory_calibration(_dev.get(), &e);
+            error::handle(e);
+        }
+
+        /**
+        * Write calibration to tm2 device's EEPROM
+        * \param[in] device       tm2 device
+        * \param[out] error       If non-null, receives any error that occurs during this call, otherwise, errors are ignored
+        */
+        void write_calibration()
+        {
+            rs2_error* e = nullptr;
+            rs2_write_calibration(_dev.get(), &e);
+            error::handle(e);
+        }
+
+    private:
+
+        std::pair<sensor, stream_profile> get_sensor_profile(rs2_stream stream_type, int stream_index) {
+            for (auto s : query_sensors()) {
+                for (auto p : s.get_stream_profiles()) {
+                    if (p.stream_type() == stream_type && p.stream_index() == stream_index)
+                        return std::pair<sensor, stream_profile>(s, p);
+                }
+            }
+            return std::pair<sensor, stream_profile>();
+         }
     };
 }
 #endif // LIBREALSENSE_RS2_DEVICE_HPP

src/core/motion.h

@@ -43,7 +43,7 @@ namespace librealsense
     };
     MAP_EXTENSION(RS2_EXTENSION_WHEEL_ODOMETER, librealsense::wheel_odometry_interface);
 
-     class tm2_extensions
+    class tm2_extensions
     {
     public:
         virtual void enable_loopback(const std::string& input) = 0;
@@ -54,4 +54,15 @@ namespace librealsense
         virtual ~tm2_extensions() = default;
     };
     MAP_EXTENSION(RS2_EXTENSION_TM2, librealsense::tm2_extensions);
+
+    class tm2_sensor_interface
+    {
+    public:
+        virtual void set_intrinsics(const stream_profile_interface& stream_profile, const rs2_intrinsics& intr) = 0;
+        virtual void set_extrinsics(const stream_profile_interface& from_profile, const stream_profile_interface& to_profile, const rs2_extrinsics& extr) = 0;
+        virtual void set_motion_device_intrinsics(const stream_profile_interface& stream_profile, const rs2_motion_device_intrinsic& intr) = 0;
+        virtual void reset_to_factory_calibration() = 0;
+        virtual void write_calibration() = 0;
+    };
+    MAP_EXTENSION(RS2_EXTENSION_TM2_SENSOR, librealsense::tm2_sensor_interface);
 }

src/realsense.def

@@ -299,6 +299,11 @@ EXPORTS
     rs2_loopback_is_enabled
     rs2_connect_tm2_controller
     rs2_disconnect_tm2_controller
+    rs2_set_intrinsics
+    rs2_set_extrinsics
+    rs2_set_motion_device_intrinsics
+    rs2_reset_to_factory_calibration
+    rs2_write_calibration
     rs2_import_localization_map
     rs2_export_localization_map
     rs2_set_static_node

src/rs.cpp

@@ -1111,6 +1111,7 @@ int rs2_is_sensor_extendable_to(const rs2_sensor* sensor, rs2_extension extensio
     case RS2_EXTENSION_SOFTWARE_SENSOR     : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::software_sensor)        != nullptr;
     case RS2_EXTENSION_POSE_SENSOR         : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::pose_sensor_interface)  != nullptr;
     case RS2_EXTENSION_WHEEL_ODOMETER      : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::wheel_odometry_interface)!= nullptr;
+    case RS2_EXTENSION_TM2_SENSOR          : return VALIDATE_INTERFACE_NO_THROW(sensor->sensor, librealsense::tm2_sensor_interface) != nullptr;
 
     default:
         return false;
@@ -2202,6 +2203,63 @@ void rs2_disconnect_tm2_controller(const rs2_device* device, int id, rs2_error**
 }
 HANDLE_EXCEPTIONS_AND_RETURN(, device)
 
+void rs2_set_intrinsics(const rs2_sensor* sensor, const rs2_stream_profile* profile, const rs2_intrinsics* intrinsics, rs2_error** error) BEGIN_API_CALL
+{
+    VALIDATE_NOT_NULL(sensor);
+    VALIDATE_NOT_NULL(profile);
+    VALIDATE_NOT_NULL(intrinsics);
+
+    auto tm2 = VALIDATE_INTERFACE(sensor->sensor, librealsense::tm2_sensor_interface);
+    tm2->set_intrinsics(*profile->profile, *intrinsics);
+}
+HANDLE_EXCEPTIONS_AND_RETURN(, sensor, profile, intrinsics)
+
+void rs2_set_extrinsics(const rs2_sensor* from_sensor, const rs2_stream_profile* from_profile, rs2_sensor* to_sensor, const rs2_stream_profile* to_profile, const rs2_extrinsics* extrinsics, rs2_error** error) BEGIN_API_CALL
+{
+    VALIDATE_NOT_NULL(from_sensor);
+    VALIDATE_NOT_NULL(from_profile);
+    VALIDATE_NOT_NULL(to_sensor);
+    VALIDATE_NOT_NULL(to_profile);
+    VALIDATE_NOT_NULL(extrinsics);
+
+    if (from_sensor->parent.device != to_sensor->parent.device)
+    {
+        LOG_ERROR("Cannot set extrinsics of two different devices \n");
+        return;
+    }
+
+    auto tm2 = VALIDATE_INTERFACE(from_sensor->sensor, librealsense::tm2_sensor_interface);
+    tm2->set_extrinsics(*from_profile->profile, *to_profile->profile, *extrinsics);
+}
+HANDLE_EXCEPTIONS_AND_RETURN(, from_sensor, from_profile, to_sensor, to_profile, extrinsics)
+
+void rs2_set_motion_device_intrinsics(const rs2_sensor* sensor, const rs2_stream_profile* profile, const rs2_motion_device_intrinsic* intrinsics, rs2_error** error) BEGIN_API_CALL
+{
+    VALIDATE_NOT_NULL(sensor);
+    VALIDATE_NOT_NULL(profile);
+    VALIDATE_NOT_NULL(intrinsics);
+
+    auto tm2 = VALIDATE_INTERFACE(sensor->sensor, librealsense::tm2_sensor_interface);
+    tm2->set_motion_device_intrinsics(*profile->profile, *intrinsics);
+}
+HANDLE_EXCEPTIONS_AND_RETURN(, sensor, profile, intrinsics)
+
+void rs2_reset_to_factory_calibration(const rs2_device* device, rs2_error** error) BEGIN_API_CALL
+{
+    VALIDATE_NOT_NULL(device);
+    auto tm2 = VALIDATE_INTERFACE(&device->device->get_sensor(0), librealsense::tm2_sensor_interface);
+    tm2->reset_to_factory_calibration();
+}
+HANDLE_EXCEPTIONS_AND_RETURN(, device)
+
+void rs2_write_calibration(const rs2_device* device, rs2_error** error) BEGIN_API_CALL
+{
+    VALIDATE_NOT_NULL(device);
+    auto tm2 = VALIDATE_INTERFACE(&device->device->get_sensor(0), librealsense::tm2_sensor_interface);
+    tm2->write_calibration();
+}
+HANDLE_EXCEPTIONS_AND_RETURN(, device)
+
 rs2_processing_block_list* rs2_get_recommended_processing_blocks(rs2_sensor* sensor, rs2_error** error) BEGIN_API_CALL
 {
     VALIDATE_NOT_NULL(sensor);