Add exposure control of the color camera

Author: saulthu

CMakeLists.txt

@@ -123,6 +123,7 @@ SET(SOURCES
   include/libfreenect2/frame_listener.hpp
   include/libfreenect2/frame_listener_impl.h
   include/libfreenect2/libfreenect2.hpp
+  include/libfreenect2/color_settings.h
   include/libfreenect2/packet_pipeline.h
   include/internal/libfreenect2/packet_processor.h
   include/libfreenect2/registration.h

include/internal/libfreenect2/protocol/command.h

@@ -28,6 +28,7 @@
 #define COMMAND_H_
 
 #include <stdint.h>
+#include <cstring>
 #include "libfreenect2/protocol/response.h"
 
 #define KCMD_READ_FIRMWARE_VERSIONS 0x02
@@ -40,6 +41,8 @@
 #define KCMD_SET_STREAMING 0x2B
 #define KCMD_SET_MODE 0x4B
 
+#define KCMD_RGB_SETTING 0x3E  // Command value for color camera settings
+
 #define KCMD_0x46 0x46
 #define KCMD_0x47 0x47
 
@@ -219,6 +222,39 @@ typedef CommandWith4Params<KCMD_SET_MODE, 0x00, 0x00> SetModeDisabledCommand;
 typedef CommandWith4Params<KCMD_SET_MODE, 0x00, 0x01> SetModeEnabledCommand;
 typedef CommandWith4Params<KCMD_SET_MODE, 0x00, 0x01, 0x00640064> SetModeEnabledWith0x00640064Command;
 typedef CommandWith4Params<KCMD_SET_MODE, 0x00, 0x01, 0x00500050> SetModeEnabledWith0x00500050Command;
+
+// The following information was found by using the library released by Microsoft under MIT license,
+// https://github.com/Microsoft/MixedRealityCompanionKit/tree/master/KinectIPD/NuiSensor
+// Command values come from headers, packet CommandId from stepping through assembly in NuiSensorLib.lib
+#define ColorSettingResponseSize sizeof(ColorSettingResponse)
+struct ColorSettingCommand : public Command<KCMD_RGB_SETTING, ColorSettingResponseSize, ColorSettingResponseSize, 4>
+{
+  ColorSettingCommand(ColorSettingCommandType cmd, uint32_t value = 0)
+    : Command<KCMD_RGB_SETTING, ColorSettingResponseSize, ColorSettingResponseSize, 4>(0)  // seq always zero
+  {
+    // Data parameters are elements of struct NUISENSOR_RGB_CHANGE_STREAM_SETTING
+    // which supports multiple settings for a single call.
+    this->data_.parameters[0] = 1;  // NumCommands = 1 for single command
+    this->data_.parameters[1] = 0;  // SequenceId = 0 for RGB commands
+    this->data_.parameters[2] = (uint32_t)cmd;  // Cmd with a value from NUISENSOR_RGB_COMMAND_*
+    this->data_.parameters[3] = value;  // Arg is int or float depending on Cmd, zero for GET_*
+  }
+
+  // Could overload ctor to ease usage for float-valued settings.
+  ColorSettingCommand(ColorSettingCommandType cmd, float value)
+    : ColorSettingCommand(cmd, float_bytes_to_uint32_(value))
+  {
+  }
+
+private:
+  static uint32_t float_bytes_to_uint32_(float value)
+  {
+    uint32_t out;
+    memcpy(&out, &value, sizeof(out));
+    return out;
+  }
+};
+
 } /* namespace protocol */
 } /* namespace libfreenect2 */
 #endif /* COMMAND_H_ */

include/internal/libfreenect2/protocol/response.h

@@ -323,6 +323,18 @@ LIBFREENECT2_PACK(struct P0TablesResponse
   uint8_t  unknownD[];
 });
 
+// RGB camera settings reply for a single setting change.
+// Equivalent of NUISENSOR_RGB_CHANGE_STREAM_SETTING_REPLY in NuiSensorLib.h
+LIBFREENECT2_PACK(struct ColorSettingResponse
+{
+  uint32_t NumStatus;
+  uint32_t CommandListStatus;
+  // Result of the first command -- we only send one at a time for now.
+  // Equivalent of a fixed-length array of NUISENSOR_RGB_CHANGE_STREAM_SETTING_REPLY_STATUS in NuiSensorLib.h
+  uint32_t Status;
+  uint32_t Data;
+});
+
 } /* namespace protocol */
 } /* namespace libfreenect2 */
 #endif /* RESPONSE_H_ */

include/libfreenect2/color_settings.h

@@ -0,0 +1,118 @@
+/*
+ * This file is part of the OpenKinect Project. http://www.openkinect.org
+ *
+ * Copyright (c) 2011 individual OpenKinect contributors. See the CONTRIB file
+ * for details.
+ *
+ * This code is licensed to you under the terms of the Apache License, version
+ * 2.0, or, at your option, the terms of the GNU General Public License,
+ * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
+ * or the following URLs:
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * http://www.gnu.org/licenses/gpl-2.0.txt
+ *
+ * If you redistribute this file in source form, modified or unmodified, you
+ * may:
+ *   1) Leave this header intact and distribute it under the same terms,
+ *      accompanying it with the APACHE20 and GPL20 files, or
+ *   2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
+ *   3) Delete the GPL v2 clause and accompany it with the APACHE20 file
+ * In all cases you must keep the copyright notice intact and include a copy
+ * of the CONTRIB file.
+ *
+ * Binary distributions must follow the binary distribution requirements of
+ * either License.
+ */
+
+#ifndef COLOR_SETTINGS_H_
+#define COLOR_SETTINGS_H_
+
+namespace libfreenect2
+{
+
+// The following information was found by using the library released by Microsoft under MIT license,
+// https://github.com/Microsoft/MixedRealityCompanionKit/tree/master/KinectIPD/NuiSensor
+enum ColorSettingCommandType
+{
+  COLOR_SETTING_SET_EXPOSURE_MODE = 0,
+  COLOR_SETTING_SET_INTEGRATION_TIME = 1,
+  COLOR_SETTING_GET_INTEGRATION_TIME = 2,
+  COLOR_SETTING_SET_WHITE_BALANCE_MODE = 10,
+  COLOR_SETTING_SET_RED_CHANNEL_GAIN = 11,
+  COLOR_SETTING_SET_GREEN_CHANNEL_GAIN = 12,
+  COLOR_SETTING_SET_BLUE_CHANNEL_GAIN = 13,
+  COLOR_SETTING_GET_RED_CHANNEL_GAIN = 14,
+  COLOR_SETTING_GET_GREEN_CHANNEL_GAIN = 15,
+  COLOR_SETTING_GET_BLUE_CHANNEL_GAIN = 16,
+  COLOR_SETTING_SET_EXPOSURE_TIME_MS = 17,
+  COLOR_SETTING_GET_EXPOSURE_TIME_MS = 18,
+  COLOR_SETTING_SET_DIGITAL_GAIN = 19,
+  COLOR_SETTING_GET_DIGITAL_GAIN = 20,
+  COLOR_SETTING_SET_ANALOG_GAIN = 21,
+  COLOR_SETTING_GET_ANALOG_GAIN = 22,
+  COLOR_SETTING_SET_EXPOSURE_COMPENSATION = 23,
+  COLOR_SETTING_GET_EXPOSURE_COMPENSATION = 24,
+  COLOR_SETTING_SET_ACS = 25,
+  COLOR_SETTING_GET_ACS = 26,
+  COLOR_SETTING_SET_EXPOSURE_METERING_MODE = 27,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONES = 28,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_0_WEIGHT = 29,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_1_WEIGHT = 30,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_2_WEIGHT = 31,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_3_WEIGHT = 32,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_4_WEIGHT = 33,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_5_WEIGHT = 34,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_6_WEIGHT = 35,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_7_WEIGHT = 36,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_8_WEIGHT = 37,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_9_WEIGHT = 38,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_10_WEIGHT = 39,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_11_WEIGHT = 40,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_12_WEIGHT = 41,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_13_WEIGHT = 42,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_14_WEIGHT = 43,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_15_WEIGHT = 44,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_16_WEIGHT = 45,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_17_WEIGHT = 46,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_18_WEIGHT = 47,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_19_WEIGHT = 48,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_20_WEIGHT = 49,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_21_WEIGHT = 50,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_22_WEIGHT = 51,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_23_WEIGHT = 52,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_24_WEIGHT = 53,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_25_WEIGHT = 54,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_26_WEIGHT = 55,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_27_WEIGHT = 56,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_28_WEIGHT = 57,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_29_WEIGHT = 58,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_30_WEIGHT = 59,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_31_WEIGHT = 60,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_32_WEIGHT = 61,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_33_WEIGHT = 62,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_34_WEIGHT = 63,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_35_WEIGHT = 64,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_36_WEIGHT = 65,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_37_WEIGHT = 66,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_38_WEIGHT = 67,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_39_WEIGHT = 68,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_40_WEIGHT = 69,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_41_WEIGHT = 70,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_42_WEIGHT = 71,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_43_WEIGHT = 72,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_44_WEIGHT = 73,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_45_WEIGHT = 74,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_46_WEIGHT = 75,
+  COLOR_SETTING_SET_EXPOSURE_METERING_ZONE_47_WEIGHT = 76,
+  COLOR_SETTING_SET_MAX_ANALOG_GAIN_CAP = 77,
+  COLOR_SETTING_SET_MAX_DIGITAL_GAIN_CAP = 78,
+  COLOR_SETTING_SET_FLICKER_FREE_FREQUENCY = 79,
+  COLOR_SETTING_GET_EXPOSURE_MODE = 80,
+  COLOR_SETTING_GET_WHITE_BALANCE_MODE = 81,
+  COLOR_SETTING_SET_FRAME_RATE = 82,
+  COLOR_SETTING_GET_FRAME_RATE = 83,
+};
+
+} /* namespace libfreenect2 */
+
+#endif /* COLOR_SETTINGS_H_ */

include/libfreenect2/libfreenect2.hpp

@@ -32,6 +32,7 @@
 #include <libfreenect2/config.h>
 #include <libfreenect2/frame_listener.hpp>
 #include <libfreenect2/packet_pipeline.h>
+#include <libfreenect2/color_settings.h>
 #include <string>
 #include <vector>
 
@@ -160,6 +161,45 @@ class LIBFREENECT2_API Freenect2Device
   /** Provide your listener to receive IR and depth frames. */
   virtual void setIrAndDepthFrameListener(FrameListener* ir_frame_listener) = 0;
 
+  /** Sets the RGB camera to fully automatic exposure setting.
+   * Exposure compensation: negative value gives an underexposed image,
+   * positive gives an overexposed image.
+   * 
+   * @param exposure_compensation Exposure compensation, range [-2.0, 2.0]
+   */
+  virtual void setColorAutoExposure(float exposure_compensation = 0) = 0;
+
+  /** Sets a flicker-free exposure time of the RGB camera in pseudo-ms, value in range [0.0, 640] ms.
+   * The actual frame integration time is set to a multiple of fluorescent light period
+   * that is shorter than the requested time e.g. requesting 16 ms will set 10 ms
+   * in Australia (100Hz light flicker), 8.33 ms in USA (120Hz light flicker).
+   * The gain is automatically set to compensate for the reduced integration time,
+   * as if the gain was set to 1.0 and the integration time was the requested value.
+   *
+   * Requesting less than a single fluorescent light period will set the integration time
+   * to the requested value, so the image brightness will flicker.
+   *
+   * To set the shortest non-flickering integration period for any country, simply set
+   * a pseudo-exposure time of between (10.0, 16.667) ms, which will automatically drop
+   * the integration time to 10 or 8.3 ms depending on country, while setting the analog
+   * gain control to a brighter value.
+   * 
+   * @param pseudo_exposure_time_ms Pseudo-exposure time in milliseconds, range (0.0, 66.0+]
+   */
+  virtual void setColorSemiAutoExposure(float pseudo_exposure_time_ms) = 0;
+
+  /** Manually set true exposure time and analog gain of the RGB camera.
+   * @param integration_time_ms True shutter time in milliseconds, range (0.0, 66.0]
+   * @param analog_gain Analog gain, range [1.0, 4.0]
+   */
+  virtual void setColorManualExposure(float integration_time_ms, float analog_gain) = 0;
+
+  /** Set/get an individual setting value of the RGB camera. */
+  virtual void setColorSetting(ColorSettingCommandType cmd, uint32_t value) = 0;
+  virtual void setColorSetting(ColorSettingCommandType cmd, float value) = 0;
+  virtual uint32_t getColorSetting(ColorSettingCommandType cmd) = 0;
+  virtual float getColorSettingFloat(ColorSettingCommandType cmd) = 0;
+
   /** Start data processing with both RGB and depth streams.
    * All above configuration must only be called before start() or after stop().
    *

src/libfreenect2.cpp

@@ -33,6 +33,7 @@
 #include <limits>
 #include <cmath>
 #include <cstdlib>
+#include <cstring>
 #include <fstream>
 
 #define WRITE_LIBUSB_ERROR(__RESULT) libusb_error_name(__RESULT) << " " << libusb_strerror((libusb_error)__RESULT)
@@ -255,6 +256,13 @@ class Freenect2DeviceImpl : public Freenect2Device
 
   virtual void setColorFrameListener(libfreenect2::FrameListener* rgb_frame_listener);
   virtual void setIrAndDepthFrameListener(libfreenect2::FrameListener* ir_frame_listener);
+  virtual void setColorAutoExposure(float exposure_compensation = 0);
+  virtual void setColorSemiAutoExposure(float pseudo_exposure_time_ms);
+  virtual void setColorManualExposure(float integration_time_ms, float analog_gain);
+  virtual void setColorSetting(ColorSettingCommandType cmd, uint32_t value);
+  virtual void setColorSetting(ColorSettingCommandType cmd, float value);
+  virtual uint32_t getColorSetting(ColorSettingCommandType cmd);
+  virtual float getColorSettingFloat(ColorSettingCommandType cmd);
   virtual bool start();
   virtual bool startStreams(bool rgb, bool depth);
   virtual bool stop();
@@ -279,6 +287,14 @@ class Freenect2ReplayDevice : public Freenect2Device
   virtual void setColorFrameListener(FrameListener* listener);
   virtual void setIrAndDepthFrameListener(FrameListener* listener);
 
+  virtual void setColorAutoExposure(float exposure_compensation) {}
+  virtual void setColorSemiAutoExposure(float pseudo_exposure_time_ms) {}
+  virtual void setColorManualExposure(float integration_time_ms, float analog_gain) {}
+  virtual void setColorSetting(ColorSettingCommandType cmd, uint32_t value) {}
+  virtual void setColorSetting(ColorSettingCommandType cmd, float value) {}
+  virtual uint32_t getColorSetting(ColorSettingCommandType cmd) { return 0u; }
+  virtual float getColorSettingFloat(ColorSettingCommandType cmd) { return 0.0f; }
+
   bool open();
 
   virtual bool start();
@@ -736,6 +752,64 @@ void Freenect2DeviceImpl::setIrAndDepthFrameListener(libfreenect2::FrameListener
     pipeline_->getDepthPacketProcessor()->setFrameListener(ir_frame_listener);
 }
 
+void Freenect2DeviceImpl::setColorAutoExposure(float exposure_compensation)
+{
+  CommandTransaction::Result result;
+  command_tx_.execute(ColorSettingCommand(COLOR_SETTING_SET_ACS, 0u), result);
+  command_tx_.execute(ColorSettingCommand(COLOR_SETTING_SET_EXPOSURE_MODE, 0u), result);  // 0 == Fully auto
+  command_tx_.execute(ColorSettingCommand(COLOR_SETTING_SET_EXPOSURE_COMPENSATION, exposure_compensation), result);
+}
+
+void Freenect2DeviceImpl::setColorSemiAutoExposure(float pseudo_exposure_time_ms)
+{
+  CommandTransaction::Result result;
+  command_tx_.execute(ColorSettingCommand(COLOR_SETTING_SET_ACS, 0u), result);
+  command_tx_.execute(ColorSettingCommand(COLOR_SETTING_SET_EXPOSURE_MODE, 3u), result);  // 3 == Semi-auto
+  command_tx_.execute(ColorSettingCommand(COLOR_SETTING_SET_EXPOSURE_TIME_MS, pseudo_exposure_time_ms), result);
+}
+
+void Freenect2DeviceImpl::setColorManualExposure(float integration_time_ms, float analog_gain)
+{
+  CommandTransaction::Result result;
+  command_tx_.execute(ColorSettingCommand(COLOR_SETTING_SET_ACS, 0u), result);
+  command_tx_.execute(ColorSettingCommand(COLOR_SETTING_SET_EXPOSURE_MODE, 4u), result);  // 4 == Fully manual
+  command_tx_.execute(ColorSettingCommand(COLOR_SETTING_SET_INTEGRATION_TIME, integration_time_ms), result);
+  command_tx_.execute(ColorSettingCommand(COLOR_SETTING_SET_ANALOG_GAIN, analog_gain), result);
+}
+
+void Freenect2DeviceImpl::setColorSetting(ColorSettingCommandType cmd, uint32_t value)
+{
+  CommandTransaction::Result result;
+  command_tx_.execute(ColorSettingCommand(cmd, value), result);
+}
+
+void Freenect2DeviceImpl::setColorSetting(ColorSettingCommandType cmd, float value)
+{
+  CommandTransaction::Result result;
+  command_tx_.execute(ColorSettingCommand(cmd, value), result);
+}
+
+uint32_t Freenect2DeviceImpl::getColorSetting(ColorSettingCommandType cmd)
+{
+  CommandTransaction::Result result;
+  command_tx_.execute(ColorSettingCommand(cmd), result);
+  if (result.size() < sizeof(ColorSettingResponse))
+  {
+    LOG_WARNING << "failed to get color setting, response size " << result.size() << " too small, expected " << sizeof(ColorSettingResponse);
+    return 0u;
+  }
+  ColorSettingResponse const* data = reinterpret_cast<ColorSettingResponse const*>(&result[0]);
+  return data->Data;
+}
+
+float Freenect2DeviceImpl::getColorSettingFloat(ColorSettingCommandType cmd)
+{
+  uint32_t data = getColorSetting(cmd);
+  float out;
+  memcpy(&out, &data, sizeof(out));
+  return out;
+}
+
 bool Freenect2DeviceImpl::open()
 {
   LOG_INFO << "opening...";