int main(int argc, char *argv[]) { CSimpleOpt s(argc, argv, argumentSpecifications); logger.setDefaultLogLevel(LOG_INFO); uint16_t vid = 0; Vector<uint16_t> pids; String serialNumber; String dumpFileName; String type = "raw"; int32_t frameCount = 1; char *endptr; while (s.Next()) { if (s.LastError() != SO_SUCCESS) { std::cout << s.GetLastErrorText(s.LastError()) << ": '" << s.OptionText() << "' (use -h to get command line help)" << std::endl; help(); return -1; } //std::cout << s.OptionId() << ": " << s.OptionArg() << std::endl; Vector<String> splits; switch (s.OptionId()) { case VENDOR_ID: vid = (uint16_t)strtol(s.OptionArg(), &endptr, 16); break; case PRODUCT_ID: split(s.OptionArg(), ',', splits); for(auto &s1: splits) pids.push_back((uint16_t)strtol(s1.c_str(), &endptr, 16)); break; case SERIAL_NUMBER: serialNumber = s.OptionArg(); break; case DUMP_FILE: dumpFileName = s.OptionArg(); break; case NUM_OF_FRAMES: frameCount = (int32_t)strtol(s.OptionArg(), &endptr, 10); break; case CAPTURE_TYPE: type = s.OptionArg(); break; default: help(); break; }; } if(vid == 0 || pids.size() == 0 || pids[0] == 0 || dumpFileName.size() == 0) { std::cerr << "Required argument missing." << std::endl; help(); return -1; } if (type != "raw" && type != "raw_processed" && type != "depth" && type != "pointcloud") { std::cerr << "Unknown type '" << type << "'" << std::endl; help(); return -1; } std::ofstream f(dumpFileName, std::ios::binary | std::ios::out); if(!f.good()) { std::cerr << "Failed to open '" << dumpFileName << "'" << std::endl; return -1; } CameraSystem sys; // Get all valid detected devices const Vector<DevicePtr> &devices = sys.scan(); DevicePtr toConnect; std::cout << "Detected devices: " << std::endl; for(auto &d: devices) { std::cout << d->id() << std::endl; if(d->interfaceID() == Device::USB) { USBDevice &usb = (USBDevice &)*d; if(usb.vendorID() == vid && (serialNumber.size() == 0 || usb.serialNumber() == serialNumber)) { for(auto pid: pids) if(usb.productID() == pid) toConnect = d; } } } if(!toConnect) { std::cerr << "No valid device found for the specified VID:PID:serialnumber" << std::endl; return -1; } DepthCameraPtr depthCamera = sys.connect(toConnect); if(!depthCamera) { std::cerr << "Could not load depth camera for device " << toConnect->id() << std::endl; return -1; } if(!depthCamera->isInitialized()) { std::cerr << "Depth camera not initialized for device " << toConnect->id() << std::endl; return -1; } std::cout << "Successfully loaded depth camera for device " << toConnect->id() << std::endl; int count = 0; TimeStampType lastTimeStamp = 0; if (type == "raw") { depthCamera->registerCallback(DepthCamera::FRAME_RAW_FRAME_UNPROCESSED, [&](DepthCamera &dc, const Frame &frame, DepthCamera::FrameType c) { const RawDataFrame *d = dynamic_cast<const RawDataFrame *>(&frame); if (!d) { std::cout << "Null frame captured? or not of type RawDataFrame" << std::endl; return; } std::cout << "Capture frame " << d->id << "@" << d->timestamp; if (lastTimeStamp != 0) std::cout << " (" << 1E6 / (d->timestamp - lastTimeStamp) << " fps)"; std::cout << std::endl; f.write((char *)&d->id, sizeof(d->id)); f.write((char *)&d->timestamp, sizeof(d->timestamp)); lastTimeStamp = d->timestamp; f.write((char *)d->data.data(), d->data.size()); count++; if (count >= frameCount) dc.stop(); }); } else if (type == "raw_processed") { depthCamera->registerCallback(DepthCamera::FRAME_RAW_FRAME_PROCESSED, [&](DepthCamera &dc, const Frame &frame, DepthCamera::FrameType c) { const ToFRawFrame *d = dynamic_cast<const ToFRawFrame *>(&frame); if (!d) { std::cout << "Null frame captured? or not of type ToFRawFrame" << std::endl; return; } std::cout << "Capture frame " << d->id << "@" << d->timestamp; if (lastTimeStamp != 0) std::cout << " (" << 1E6 / (d->timestamp - lastTimeStamp) << " fps)"; std::cout << std::endl; f.write((char *)&d->id, sizeof(d->id)); f.write((char *)&d->timestamp, sizeof(d->timestamp)); lastTimeStamp = d->timestamp; if (d->phase()) f.write((char *)d->phase(), d->phaseWordWidth()*d->size.width*d->size.height); if (d->amplitude()) f.write((char *)d->amplitude(), d->amplitudeWordWidth()*d->size.width*d->size.height); if (d->ambient()) f.write((char *)d->ambient(), d->ambientWordWidth()*d->size.width*d->size.height); if (d->flags()) f.write((char *)d->flags(), d->flagsWordWidth()*d->size.width*d->size.height); count++; if (count >= frameCount) dc.stop(); }); } else if (type == "depth") { depthCamera->registerCallback(DepthCamera::FRAME_DEPTH_FRAME, [&](DepthCamera &dc, const Frame &frame, DepthCamera::FrameType c) { const DepthFrame *d = dynamic_cast<const DepthFrame *>(&frame); if (!d) { std::cout << "Null frame captured? or not of type DepthFrame" << std::endl; return; } std::cout << "Capture frame " << d->id << "@" << d->timestamp; if (lastTimeStamp != 0) std::cout << " (" << 1E6 / (d->timestamp - lastTimeStamp) << " fps)"; std::cout << std::endl; f.write((char *)&d->id, sizeof(d->id)); f.write((char *)&d->timestamp, sizeof(d->timestamp)); lastTimeStamp = d->timestamp; f.write((char *)d->depth.data(), sizeof(float)*d->size.width*d->size.height); f.write((char *)d->amplitude.data(), sizeof(float)*d->size.width*d->size.height); count++; if (count >= frameCount) dc.stop(); }); } else if (type == "pointcloud") { depthCamera->registerCallback(DepthCamera::FRAME_XYZI_POINT_CLOUD_FRAME, [&](DepthCamera &dc, const Frame &frame, DepthCamera::FrameType c) { const XYZIPointCloudFrame *d = dynamic_cast<const XYZIPointCloudFrame *>(&frame); if (!d) { std::cout << "Null frame captured? or not of type XYZIPointCloudFrame" << std::endl; return; } std::cout << "Capture frame " << d->id << "@" << d->timestamp; if (lastTimeStamp != 0) std::cout << " (" << 1E6 / (d->timestamp - lastTimeStamp) << " fps)"; std::cout << std::endl; f.write((char *)&d->id, sizeof(d->id)); f.write((char *)&d->timestamp, sizeof(d->timestamp)); lastTimeStamp = d->timestamp; f.write((char *)d->points.data(), sizeof(IntensityPoint)*d->points.size()); count++; if (count >= frameCount) dc.stop(); }); } if(depthCamera->start()) { FrameRate r; if(depthCamera->getFrameRate(r)) logger(LOG_INFO) << "Capturing at a frame rate of " << r.getFrameRate() << " fps" << std::endl; depthCamera->wait(); } else std::cerr << "Could not start the depth camera " << depthCamera->id() << std::endl; return 0; }
UVCPrivate::UVCPrivate(DevicePtr usb) { if (usb->interfaceID() != Device::USB) return; USBSystem sys; String devicePath = sys.getDeviceNode((USBDevice &)*usb); if (!devicePath.size()) { logger(LOG_ERROR) << "UVC: Could not get device path for device '" << usb->id() << "'" << std::endl; return; } DWORD devInst; if (!sys.getUSBSystemPrivate().getDevInst(devicePath, devInst)) { logger(LOG_ERROR) << "UVC: Could not get devInst for device '" << usb->id() << "'" << std::endl; return; } HRESULT hr = S_OK; IMoniker *moniker; Ptr<ICreateDevEnum> devEnum; Ptr<IEnumMoniker> classEnum; void *p; CoInitializeEx(NULL, COINIT_MULTITHREADED); // Create the system device enumerator if ((hr = (CoCreateInstance(CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC, IID_ICreateDevEnum, (void **)&p))) != S_OK) { logger(LOG_ERROR) << "UVC: Failed to get ICreateDevEnum enumerator" << std::endl; return; } devEnum = Ptr<ICreateDevEnum>((ICreateDevEnum *)p, [](ICreateDevEnum *i) { i->Release(); }); // Create an enumerator for the video capture devices if ((hr = devEnum->CreateClassEnumerator(CLSID_VideoInputDeviceCategory, (IEnumMoniker **)&p, 0)) != S_OK) { logger(LOG_ERROR) << "UVC: Failed to get VideoInputDeviceCategory class enumerator" << std::endl; return; } // If there are no enumerators for the requested type, then // CreateClassEnumerator will succeed, but pClassEnum will be NULL. if (p == NULL) { logger(LOG_ERROR) << "UVC: Failed to get any video input devices in the enumerator" << std::endl; return; } classEnum = Ptr<IEnumMoniker>((IEnumMoniker *)p, [](IEnumMoniker *i) { i->Release(); }); while ((hr = classEnum->Next(1, &moniker, NULL)) == S_OK) { IPropertyBag *propBag; if ((hr = moniker->BindToStorage(0, 0, IID_IPropertyBag, (void**)&propBag)) != S_OK) { logger(LOG_WARNING) << "UVC: Could not get properties for current moniker" << std::endl; continue; } VARIANT varName; VariantInit(&varName); if ((hr = propBag->Read(L"DevicePath", &varName, 0)) != S_OK) { logger(LOG_WARNING) << "UVC: Could not get DevicePath for current moniker" << std::endl; continue; } _bstr_t b(varName.bstrVal); String devPath = b; VariantClear(&varName); DWORD dInst; DWORD parentDevInst; if (!sys.getUSBSystemPrivate().getDevInst(devPath, dInst, (LPGUID)&AM_KSCATEGORY_CAPTURE)) //AM_KSCATEGORY_VIDEO { logger(LOG_WARNING) << "UVC: Could not get DevInst for DevicePath = " << devPath << std::endl; continue; } if (CM_Get_Parent(&parentDevInst, dInst, 0) != CR_SUCCESS) { logger(LOG_WARNING) << "UVC: Could not get parent DevInst for DevicePath = " << devPath << std::endl; continue; } if (parentDevInst == devInst) // Check whether our USBDevice's DevInst matches with IMoniker's parent's DevInst matches { if (usb->channelID() >= 0) { auto x = devPath.find("&mi_"); if (x != String::npos) { char *endptr; int channel = strtol(devPath.c_str() + x + 4, &endptr, 10); if (channel != usb->channelID()) continue; } else { logger(LOG_ERROR) << "UVC: Could not find channel ID of current device '" << devPath << "'" << std::endl; continue; } } if ((hr = moniker->BindToObject(0, 0, IID_IBaseFilter, (void**)&p)) != S_OK) { logger(LOG_ERROR) << "UVC: Could not get the IBaseFilter for the current video input device" << std::endl; continue; } _captureFilter = Ptr<IBaseFilter>((IBaseFilter *)p, [](IBaseFilter *d) { d->Release(); }); _captureFilter->AddRef(); break; } } }