v01

thirdparty/Pangolin/src/video/drivers/depthsense.cpp

@@ -0,0 +1,656 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <pangolin/video/drivers/depthsense.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+#include <iomanip>
+
+namespace pangolin
+{
+
+const size_t ROGUE_ADDR = 0x01;
+const double MAX_DELTA_TIME = 20000.0; //u_s
+
+DepthSenseContext& DepthSenseContext::I()
+{
+    static DepthSenseContext s;
+    return s;
+}
+
+DepthSense::Context& DepthSenseContext::Context()
+{
+    return g_context;
+}
+
+void DepthSenseContext::NewDeviceRunning()
+{
+    running_devices++;
+    if(running_devices == 1) {
+        StartNodes();
+    }
+}
+
+void DepthSenseContext::DeviceClosing()
+{
+    running_devices--;
+    if(running_devices == 0) {
+        StopNodes();
+
+        // Force destruction of current context
+        g_context = DepthSense::Context();
+    }
+}
+
+DepthSenseVideo* DepthSenseContext::GetDepthSenseVideo(size_t device_num, DepthSenseSensorType s1, DepthSenseSensorType s2, ImageDim dim1, ImageDim dim2, unsigned int fps1, unsigned int fps2, const Uri& uri)
+{
+    if(running_devices == 0) {
+        // Initialise SDK
+        g_context = DepthSense::Context::create("localhost");
+    }
+
+    // Get the list of currently connected devices
+    std::vector<DepthSense::Device> da = g_context.getDevices();
+
+    if( da.size() > device_num )
+    {
+        return new DepthSenseVideo(da[device_num], s1, s2, dim1, dim2, fps1, fps2, uri);
+    }
+
+    throw VideoException("DepthSense device not connected.");
+}
+
+DepthSenseContext::DepthSenseContext()
+    : is_running(false), running_devices(0)
+{
+}
+
+DepthSenseContext::~DepthSenseContext()
+{
+    StopNodes();
+}
+
+
+void DepthSenseContext::StartNodes()
+{
+    if(!is_running) {
+        // Launch EventLoop thread
+        event_thread = std::thread(&DepthSenseContext::EventLoop, this );
+    }
+}
+
+void DepthSenseContext::StopNodes()
+{
+    if(is_running && event_thread.joinable()) {
+        g_context.quit();
+        event_thread.join();
+    }
+}
+
+void DepthSenseContext::EventLoop()
+{
+    is_running = true;
+    g_context.startNodes();
+    g_context.run();
+    g_context.stopNodes();
+    is_running = false;
+}
+
+DepthSenseVideo::DepthSenseVideo(DepthSense::Device device, DepthSenseSensorType s1, DepthSenseSensorType s2, ImageDim dim1, ImageDim dim2, unsigned int fps1, unsigned int fps2, const Uri& uri)
+    : device(device), fill_image(0), depthmap_stream(-1), rgb_stream(-1), gotDepth(0), gotColor(0),
+      enableDepth(false), enableColor(false), depthTs(0.0), colorTs(0.0), size_bytes(0)
+{
+    streams_properties = &frame_properties["streams"];
+    *streams_properties = picojson::value(picojson::array_type, false);
+    streams_properties->get<picojson::array>().resize(2);
+
+    sensorConfig[0] = {s1, dim1, fps1};
+    sensorConfig[1] = {s2, dim2, fps2};
+    ConfigureNodes(uri);
+
+    DepthSenseContext::I().NewDeviceRunning();
+}
+
+DepthSenseVideo::~DepthSenseVideo()
+{
+    if (g_cnode.isSet()) DepthSenseContext::I().Context().unregisterNode(g_cnode);
+    if (g_dnode.isSet()) DepthSenseContext::I().Context().unregisterNode(g_dnode);
+
+    fill_image = (unsigned char*)ROGUE_ADDR;
+    cond_image_requested.notify_all();
+
+    DepthSenseContext::I().DeviceClosing();
+}
+
+picojson::value Json(DepthSense::IntrinsicParameters& p)
+{
+    picojson::value js;
+    js["model"] = "polynomial";
+    js["width"] = p.width;
+    js["height"] = p.height;
+    js["RDF"] = "[1,0,0; 0,1,0; 0,0,1]";
+
+    js["fx"] = p.fx;
+    js["fy"] = p.fy;
+    js["u0"] = p.cx;
+    js["v0"] = p.cy;
+    js["k1"] = p.k1;
+    js["k2"] = p.k2;
+    js["k3"] = p.k3;
+    js["p1"] = p.p1;
+    js["p2"] = p.p2;
+
+    return js;
+}
+
+picojson::value Json(DepthSense::ExtrinsicParameters& p)
+{
+    picojson::value js;
+    js["rows"] = "3";
+    js["cols"] = "4";
+
+    std::ostringstream oss;
+    oss << std::setprecision(17);
+    oss << "[" << p.r11 << "," << p.r12 << "," << p.r13 << "," << p.t1 << ";";
+    oss        << p.r21 << "," << p.r22 << "," << p.r23 << "," << p.t2 << ";";
+    oss        << p.r31 << "," << p.r32 << "," << p.r33 << "," << p.t3 << "]";
+
+    js["data"] = oss.str();
+    return js;
+}
+
+void DepthSenseVideo::ConfigureNodes(const Uri& uri)
+{
+    std::vector<DepthSense::Node> nodes = device.getNodes();
+
+    for (int i = 0; i<2; ++i)
+    {
+        switch (sensorConfig[i].type)
+        {
+        case DepthSenseDepth:
+        {
+            for (int n = 0; n < (int)nodes.size(); n++)
+            {
+                DepthSense::Node node = nodes[n];
+                if ((node.is<DepthSense::DepthNode>()) && (!g_dnode.isSet()))
+                {
+                    depthmap_stream = i;
+                    g_dnode = node.as<DepthSense::DepthNode>();
+                    ConfigureDepthNode(sensorConfig[i], uri);
+                    DepthSenseContext::I().Context().registerNode(node);
+                }
+            }
+            break;
+        }
+        case DepthSenseRgb:
+        {
+            for (int n = 0; n < (int)nodes.size(); n++)
+            {
+                DepthSense::Node node = nodes[n];
+                if ((node.is<DepthSense::ColorNode>()) && (!g_cnode.isSet()))
+                {
+                    rgb_stream = i;
+                    g_cnode = node.as<DepthSense::ColorNode>();
+                    ConfigureColorNode(sensorConfig[i], uri);
+                    DepthSenseContext::I().Context().registerNode(node);
+                }
+            }
+            break;
+        }
+        default:
+            continue;
+        }
+    }
+
+    DepthSense::StereoCameraParameters scp = device.getStereoCameraParameters();
+
+    //Set json device properties for intrinsics and extrinsics
+    picojson::value& jsintrinsics = device_properties["intrinsics"];
+    if (jsintrinsics.is<picojson::null>()) {
+        jsintrinsics = picojson::value(picojson::array_type, false);
+        jsintrinsics.get<picojson::array>().resize(streams.size());
+        if (depthmap_stream >= 0) jsintrinsics[depthmap_stream] = Json(scp.depthIntrinsics);
+        if (rgb_stream >= 0) jsintrinsics[rgb_stream] = Json(scp.colorIntrinsics);
+    }
+
+    picojson::value& jsextrinsics = device_properties["extrinsics"];
+    if(jsextrinsics.is<picojson::null>()){
+        jsextrinsics = Json(scp.extrinsics);
+    }
+}
+
+inline DepthSense::FrameFormat ImageDim2FrameFormat(const ImageDim& dim)
+{
+    DepthSense::FrameFormat retVal = DepthSense::FRAME_FORMAT_UNKNOWN;
+    if(dim.x == 160 && dim.y == 120)
+    {
+        retVal = DepthSense::FRAME_FORMAT_QQVGA;
+    }
+    else if(dim.x == 176 && dim.y == 144)
+    {
+        retVal = DepthSense::FRAME_FORMAT_QCIF;
+    }
+    else if(dim.x == 240 && dim.y == 160)
+    {
+        retVal = DepthSense::FRAME_FORMAT_HQVGA;
+    }
+    else if(dim.x == 320 && dim.y == 240)
+    {
+        retVal = DepthSense::FRAME_FORMAT_QVGA;
+    }
+    else if(dim.x == 352 && dim.y == 288)
+    {
+        retVal = DepthSense::FRAME_FORMAT_CIF;
+    }
+    else if(dim.x == 480 && dim.y == 320)
+    {
+        retVal = DepthSense::FRAME_FORMAT_HVGA;
+    }
+    else if(dim.x == 640 && dim.y == 480)
+    {
+        retVal = DepthSense::FRAME_FORMAT_VGA;
+    }
+    else if(dim.x == 1280 && dim.y == 720)
+    {
+        retVal = DepthSense::FRAME_FORMAT_WXGA_H;
+    }
+    else if(dim.x == 320 && dim.y == 120)
+    {
+        retVal = DepthSense::FRAME_FORMAT_DS311;
+    }
+    else if(dim.x == 1024 && dim.y == 768)
+    {
+        retVal = DepthSense::FRAME_FORMAT_XGA;
+    }
+    else if(dim.x == 800 && dim.y == 600)
+    {
+        retVal = DepthSense::FRAME_FORMAT_SVGA;
+    }
+    else if(dim.x == 636 && dim.y == 480)
+    {
+        retVal = DepthSense::FRAME_FORMAT_OVVGA;
+    }
+    else if(dim.x == 640 && dim.y == 240)
+    {
+        retVal = DepthSense::FRAME_FORMAT_WHVGA;
+    }
+    else if(dim.x == 640 && dim.y == 360)
+    {
+        retVal = DepthSense::FRAME_FORMAT_NHD;
+    }
+    return retVal;
+}
+
+void DepthSenseVideo::UpdateParameters(const DepthSense::Node& node, const Uri& uri)
+{
+    DepthSense::Type type = node.getType();
+    picojson::value& jsnode = device_properties[type.name()];
+
+    std::vector<DepthSense::PropertyBase> properties = type.getProperties();
+    for(std::vector<DepthSense::PropertyBase>::const_iterator it = properties.begin(); it != properties.end(); ++it) {
+        const DepthSense::PropertyBase& prop = *it;
+
+        if (prop.is<DepthSense::Property<int32_t> >()) {
+            DepthSense::Property<int32_t> tprop = prop.as<DepthSense::Property<int32_t> >();
+            if (uri.Contains(prop.name())) {
+                if (!prop.isReadOnly()) {
+                    tprop.setValue(node, uri.Get<int32_t>(prop.name(), tprop.getValue(node)));
+                } else {
+                    pango_print_warn("DepthSense property '%s' is read-only\n", prop.name().c_str() );
+                }
+            }
+            jsnode[prop.name()] = tprop.getValue(node);
+        } else if (prop.is<DepthSense::Property<float> >()) {
+            DepthSense::Property<float> tprop = prop.as<DepthSense::Property<float> >();
+            if (uri.Contains(prop.name())) {
+                if (!prop.isReadOnly()) {
+                    tprop.setValue(node, uri.Get<float>(prop.name(), tprop.getValue(node)));
+                } else {
+                    pango_print_warn("DepthSense property '%s' is read-only\n", prop.name().c_str() );
+                }
+            }
+            jsnode[prop.name()] = tprop.getValue(node);
+        } else if (prop.is<DepthSense::Property<bool> >()) {
+            DepthSense::Property<bool> tprop = prop.as<DepthSense::Property<bool> >();
+            if (uri.Contains(prop.name())) {
+                if (!prop.isReadOnly()) {
+                    tprop.setValue(node, uri.Get<bool>(prop.name(), tprop.getValue(node)));
+                } else {
+                    pango_print_warn("DepthSense property '%s' is read-only\n", prop.name().c_str() );
+                }
+            }
+            jsnode[prop.name()] = tprop.getValue(node);
+        } else if (prop.is<DepthSense::Property<std::string> >()){
+            DepthSense::Property<std::string> tprop = prop.as<DepthSense::Property<std::string> >();
+            if (uri.Contains(prop.name())) {
+                if (!prop.isReadOnly()) {
+                    tprop.setValue(node, uri.Get<std::string>(prop.name(), tprop.getValue(node)).c_str() );
+                } else {
+                    pango_print_warn("DepthSense property '%s' is read-only\n", prop.name().c_str() );
+                }
+            }
+            jsnode[prop.name()] = tprop.getValue(node);
+        }
+    }
+}
+
+void DepthSenseVideo::ConfigureDepthNode(const SensorConfig& sensorConfig, const Uri& uri)
+{
+    g_dnode.newSampleReceivedEvent().connect(this, &DepthSenseVideo::onNewDepthSample);
+
+    DepthSense::DepthNode::Configuration config = g_dnode.getConfiguration();
+
+    config.frameFormat = ImageDim2FrameFormat(sensorConfig.dim);
+    config.framerate = sensorConfig.fps;
+    config.mode = DepthSense::DepthNode::CAMERA_MODE_CLOSE_MODE;
+    config.saturation = true;
+
+    try {
+        DepthSenseContext::I().Context().requestControl(g_dnode, 0);
+        g_dnode.setConfiguration(config);
+        g_dnode.setEnableDepthMap(true);
+    } catch (DepthSense::Exception& e) {
+        throw pangolin::VideoException("DepthSense exception whilst configuring node", e.what());
+    }
+
+    //Set pangolin stream for this channel
+    const int w = sensorConfig.dim.x;
+    const int h = sensorConfig.dim.y;
+
+    const PixelFormat pfmt = PixelFormatFromString("GRAY16LE");
+
+    const StreamInfo stream_info(pfmt, w, h, (w*pfmt.bpp) / 8, (unsigned char*)0);
+    streams.push_back(stream_info);
+
+    size_bytes += stream_info.SizeBytes();
+
+    enableDepth = true;
+
+    UpdateParameters(g_dnode, uri);
+}
+
+void DepthSenseVideo::ConfigureColorNode(const SensorConfig& sensorConfig, const Uri& uri)
+{
+    // connect new color sample handler
+    g_cnode.newSampleReceivedEvent().connect(this, &DepthSenseVideo::onNewColorSample);
+
+    DepthSense::ColorNode::Configuration config = g_cnode.getConfiguration();
+    config.frameFormat = ImageDim2FrameFormat(sensorConfig.dim);
+    config.compression = DepthSense::COMPRESSION_TYPE_MJPEG;
+    config.powerLineFrequency = DepthSense::POWER_LINE_FREQUENCY_50HZ;
+    config.framerate = sensorConfig.fps;
+
+    try {
+        DepthSenseContext::I().Context().requestControl(g_cnode,0);
+        g_cnode.setConfiguration(config);
+        g_cnode.setEnableColorMap(true);
+        UpdateParameters(g_cnode, uri);
+    } catch (DepthSense::Exception& e) {
+        throw pangolin::VideoException("DepthSense exception whilst configuring node", e.what());
+    }
+
+    //Set pangolin stream for this channel
+    const int w = sensorConfig.dim.x;
+    const int h = sensorConfig.dim.y;
+
+    const PixelFormat pfmt = PixelFormatFromString("BGR24");
+
+    const StreamInfo stream_info(pfmt, w, h, (w*pfmt.bpp) / 8, (unsigned char*)0 + size_bytes);
+    streams.push_back(stream_info);
+
+    size_bytes += stream_info.SizeBytes();
+
+    enableColor = true;
+}
+
+void DepthSenseVideo::onNewColorSample(DepthSense::ColorNode node, DepthSense::ColorNode::NewSampleReceivedData data)
+{
+    {
+        std::unique_lock<std::mutex> lock(update_mutex);
+
+        // Wait for fill request
+        while (!fill_image) {
+            cond_image_requested.wait(lock);
+        }
+
+        // Update per-frame parameters
+        //printf("Color delta: %.1f\n", fabs(colorTs - data.timeOfCapture));
+        colorTs = data.timeOfCapture;
+        picojson::value& jsstream = frame_properties["streams"][rgb_stream];
+        jsstream["time_us"] = data.timeOfCapture;
+
+        if (fill_image != (unsigned char*)ROGUE_ADDR) {
+            // Fill with data
+            unsigned char* imagePtr = fill_image;
+            bool copied = false;
+            for (int i = 0; !copied && i < 2; ++i)
+            {
+                switch (sensorConfig[i].type)
+                {
+                case DepthSenseDepth:
+                {
+                    imagePtr += streams[i].SizeBytes();
+                    break;
+                }
+                case DepthSenseRgb:
+                {
+                    // Leave as BGR
+                    std::memcpy(imagePtr, data.colorMap, streams[i].SizeBytes());
+                    copied = true;
+                    break;
+                }
+                default:
+                    continue;
+                }
+            }
+            gotColor++;
+        }
+
+        //printf("Got color at: %.1f\n", colorTs);
+
+        if(gotDepth)
+        {
+            double delta = fabs(GetDeltaTime());
+            if(delta > MAX_DELTA_TIME)
+            {
+                //printf("**** Waiting for another depth, delta: %.1f ****\n", delta);
+                gotDepth = 0;
+                return;
+            }
+        }
+    }
+
+    cond_image_filled.notify_one();
+}
+
+void DepthSenseVideo::onNewDepthSample(DepthSense::DepthNode node, DepthSense::DepthNode::NewSampleReceivedData data)
+{
+    {
+        std::unique_lock<std::mutex> lock(update_mutex);
+
+        // Wait for fill request
+        while(!fill_image) {
+            cond_image_requested.wait(lock);
+        }
+
+        // Update per-frame parameters
+        //printf("Depth delta: %.1f\n", fabs(depthTs - data.timeOfCapture));
+        depthTs = data.timeOfCapture;
+
+        picojson::value& jsstream = frame_properties["streams"][depthmap_stream];
+        jsstream["time_us"] = depthTs;
+
+        if(fill_image != (unsigned char*)ROGUE_ADDR) {
+            // Fill with data
+            unsigned char* imagePtr = fill_image;
+            bool copied = false;
+            for (int i = 0; i < 2; ++i)
+            {
+                switch (sensorConfig[i].type)
+                {
+                case DepthSenseDepth:
+                {
+                    memcpy(imagePtr, data.depthMap, streams[i].SizeBytes());
+                    copied = true;
+                    break;
+                }
+                case DepthSenseRgb:
+                {
+                    imagePtr += streams[i].SizeBytes();
+                    break;
+                }
+                default:
+                    continue;
+                }
+                if(copied)
+                {
+                    break;
+                }
+            }
+            gotDepth++;
+        }
+
+        //printf("Got depth at: %.1f\n", depthTs);
+
+        if(gotColor)
+        {
+            double delta = fabs(GetDeltaTime());
+            if(delta > MAX_DELTA_TIME)
+            {
+                //printf("**** Waiting for another color, delta: %.1f ****\n", delta);
+                gotColor = 0;
+                return;
+            }
+        }
+    }
+
+    cond_image_filled.notify_one();
+}
+
+void DepthSenseVideo::Start()
+{
+}
+
+void DepthSenseVideo::Stop()
+{
+}
+
+size_t DepthSenseVideo::SizeBytes() const
+{
+    return size_bytes;
+}
+
+const std::vector<StreamInfo>& DepthSenseVideo::Streams() const
+{
+    return streams;
+}
+
+bool DepthSenseVideo::GrabNext( unsigned char* image, bool /*wait*/ )
+{
+    if(fill_image) {
+        throw std::runtime_error("GrabNext Cannot be called concurrently");
+    }
+
+    //printf("#### Grab Next ####\n");
+
+    // Request that image is filled with data
+    fill_image = image;
+    cond_image_requested.notify_one();
+
+    // Wait until it has been filled successfully.
+    {
+        std::unique_lock<std::mutex> lock(update_mutex);
+        while ((enableDepth && !gotDepth) || (enableColor && !gotColor))
+        {
+            cond_image_filled.wait(lock);
+        }
+
+        if (gotDepth)
+        {
+            gotDepth = 0;
+        }
+        if (gotColor)
+        {
+            gotColor = 0;
+        }
+        fill_image = 0;
+    }
+
+    //printf("Delta time: %.1f\n", fabs(GetDeltaTime()));
+
+    return true;
+}
+
+bool DepthSenseVideo::GrabNewest( unsigned char* image, bool wait )
+{
+    return GrabNext(image,wait);
+}
+
+double DepthSenseVideo::GetDeltaTime() const
+{
+    return depthTs - colorTs;
+}
+
+DepthSenseSensorType depthsense_sensor(const std::string& str)
+{
+    if (!str.compare("rgb")) {
+        return DepthSenseRgb;
+    }
+    else if (!str.compare("depth")) {
+        return DepthSenseDepth;
+    }
+    else if (str.empty()) {
+        return DepthSenseUnassigned;
+    }
+    else{
+        throw pangolin::VideoException("Unknown DepthSense sensor", str);
+    }
+}
+
+PANGOLIN_REGISTER_FACTORY(DepthSenseVideo)
+{
+    struct DepthSenseVideoFactory final : public FactoryInterface<VideoInterface> {
+        std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+            DepthSenseSensorType img1 = depthsense_sensor(uri.Get<std::string>("img1", "depth"));
+            DepthSenseSensorType img2 = depthsense_sensor(uri.Get<std::string>("img2", ""));
+
+            const ImageDim dim1 = uri.Get<ImageDim>("size1", img1 == DepthSenseDepth ? ImageDim(320, 240) : ImageDim(640, 480) );
+            const ImageDim dim2 = uri.Get<ImageDim>("size2", img2 == DepthSenseDepth ? ImageDim(320, 240) : ImageDim(640, 480) );
+
+            const unsigned int fps1 = uri.Get<unsigned int>("fps1", 30);
+            const unsigned int fps2 = uri.Get<unsigned int>("fps2", 30);
+
+            return std::unique_ptr<VideoInterface>(
+                DepthSenseContext::I().GetDepthSenseVideo(0, img1, img2, dim1, dim2, fps1, fps2, uri)
+            );
+        }
+    };
+
+    FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<DepthSenseVideoFactory>(), 10, "depthsense");
+}
+
+}