bool UvcDriver::Capture( hal::CameraMsg& vImages )
{
vImages.Clear();
uvc_frame_t* frame = NULL;
uvc_error_t err = uvc_get_frame(devh_, &frame, 0);
if(err!= UVC_SUCCESS) {
uvc_perror(err, "uvc_get_frame");
}else{
if(frame) {
hal::ImageMsg* pimg = vImages.add_image();
pimg->set_type( (hal::Type) pbtype );
pimg->set_format( (hal::Format) pbformat );
pimg->set_width(frame->width);
pimg->set_height(frame->height);
pimg->set_data(frame->data, width_ * height_);
return true;
}else{
std::cout << "No data..." << std::endl;
}
}
return true;
}
bool ProtoReaderDriver::ReadNextCameraMessage(hal::CameraMsg& msg) {
msg.Clear();
std::unique_ptr<hal::CameraMsg> readmsg = m_reader.ReadCameraMsg(m_camId);
if(readmsg) {
msg.Swap(readmsg.get());
return true;
}else{
return false;
}
}
bool JoinCameraDriver::Capture( hal::CameraMsg& vImages )
{
vImages.Clear();
const double time = Tic();
vImages.set_system_time(time);
vImages.set_device_time(time);
unsigned activeWorkerCount = 0;
std::vector<hal::CameraMsg>& results = m_WorkTeam.process();
int ixResult = 0;
for( hal::CameraMsg& result : results ) {
if(m_WorkTeam.m_bWorkerCaptureNotOver[ixResult]){
for( int i = 0; i < result.image_size(); i++ ) {
vImages.add_image()->Swap(result.mutable_image(i));
}
result.Clear();
}
ixResult++;
activeWorkerCount++;
}
return activeWorkerCount == results.size();
}
bool Freenect2Driver::Capture(hal::CameraMsg& vImages) {
vImages.Clear();
vImages.set_device_time(Tic());
libfreenect2::FrameMap frames;
libfreenect2::FrameMap::const_iterator fit;
for (size_t i = 0; i < m_devices.size(); ++i) {
m_devices[i].listener->waitForNewFrame(frames);
const double time = Tic();
bool save_rgb = false;
if ((fit = frames.find(libfreenect2::Frame::Color)) != frames.end()) {
hal::ImageMsg* pbImg = vImages.add_image();
pbImg->set_timestamp(time);
pbImg->set_width(m_nImgWidth);
pbImg->set_height(m_nImgHeight);
pbImg->set_serial_number(m_devices[i].serialNumber);
pbImg->set_type(hal::PB_UNSIGNED_BYTE);
if (m_bColor)
pbImg->set_format(hal::PB_BGR);
else
pbImg->set_format(hal::PB_LUMINANCE);
const libfreenect2::Frame* frame = fit->second;
cv::Mat rgbaTrg(frame->height, frame->width, CV_8UC4, frame->data);
//From http://fixall.online/well--basically-cvtcolor-only-works-with-a-fixed-set/2220695/
std::vector<cv::Mat1b> channels_rgba;
cv::split(rgbaTrg, channels_rgba);
// create matrix with first three channels only
std::vector<cv::Mat1b> channels_rgb(channels_rgba.begin(),
channels_rgba.begin()+3);
cv::Mat3b trg;
cv::merge(channels_rgb, trg);
if (frame->height != m_nImgHeight || frame->width != m_nImgWidth)
cv::resize(trg, trg, cv::Size(m_nImgWidth, m_nImgHeight));
cv::flip(trg, trg, 1);
cv::Mat1b gray;
if (!m_bColor)
{
cv::cvtColor(trg,gray, CV_BGR2GRAY,1);
//Only copy one gray plane
pbImg->set_data(gray.ptr<unsigned char>(),
gray.rows * gray.cols * 1);
}
else
{
pbImg->set_data(trg.ptr<unsigned char>(),
trg.rows * trg.cols * trg.channels());
}
save_rgb = true;
}
if ((fit = frames.find(libfreenect2::Frame::Ir)) != frames.end()) {
const libfreenect2::Frame* frame = fit->second;
hal::ImageMsg* pbImg = vImages.add_image();
pbImg->set_timestamp(time);
pbImg->set_width(frame->width);
pbImg->set_height(frame->height);
pbImg->set_serial_number(m_devices[i].serialNumber);
pbImg->set_type(hal::PB_FLOAT);
pbImg->set_format(hal::PB_LUMINANCE);
cv::Mat trg(frame->height, frame->width, CV_32F, frame->data);
cv::flip(trg, trg, 1);
pbImg->set_data(trg.ptr<float>(),
trg.rows * trg.cols * sizeof(float));
}
if ((fit = frames.find(libfreenect2::Frame::Depth)) != frames.end()) {
const libfreenect2::Frame* frame = fit->second;
cv::Mat trg = cv::Mat(frame->height, frame->width, CV_32FC1,
frame->data);
if (save_rgb && m_bAlign) {
if (m_nImgHeight != IR_IMAGE_HEIGHT
|| m_nImgWidth != IR_IMAGE_WIDTH)
m_devices[i].registration->depthToRGBResolution(trg, trg);
}
// change rgb and depth image
hal::ImageMsg* pbImg = vImages.add_image();
pbImg->set_timestamp(time);
pbImg->set_width(trg.cols);
pbImg->set_height(trg.rows);
pbImg->set_serial_number(m_devices[i].serialNumber);
pbImg->set_type(hal::PB_FLOAT);
pbImg->set_format(hal::PB_LUMINANCE);
cv::flip(trg, trg, 1);
pbImg->set_data(trg.ptr<float>(),
trg.rows * trg.cols * sizeof(float));
}
m_devices[i].listener->release(frames);
}
return true;
}
bool Freenect2Driver::Capture(hal::CameraMsg& vImages) {
vImages.Clear();
vImages.set_device_time(Tic());
libfreenect2::FrameMap frames;
libfreenect2::FrameMap::const_iterator fit;
for (size_t i = 0; i < m_devices.size(); ++i) {
m_devices[i].listener->waitForNewFrame(frames);
const double time = Tic();
bool save_rgb = false;
if ((fit = frames.find(libfreenect2::Frame::Color)) != frames.end()) {
hal::ImageMsg* pbImg = vImages.add_image();
pbImg->set_timestamp(time);
pbImg->set_width(m_nImgWidth);
pbImg->set_height(m_nImgHeight);
pbImg->set_serial_number(m_devices[i].serialNumber);
pbImg->set_type(hal::PB_UNSIGNED_BYTE);
if (m_bColor)
pbImg->set_format(hal::PB_BGR);
else
pbImg->set_format(hal::PB_LUMINANCE);
const libfreenect2::Frame* frame = fit->second;
cv::Mat trg(frame->height, frame->width, CV_8UC3, frame->data);
if (frame->height != m_nImgHeight || frame->width != m_nImgWidth)
cv::resize(trg, trg, cv::Size(m_nImgWidth, m_nImgHeight));
if (!m_bColor)
cv::cvtColor(trg, trg, CV_BGR2GRAY);
cv::flip(trg, trg, 1);
pbImg->set_data(trg.ptr<unsigned char>(),
trg.rows * trg.cols * trg.channels());
save_rgb = true;
}
if ((fit = frames.find(libfreenect2::Frame::Ir)) != frames.end()) {
const libfreenect2::Frame* frame = fit->second;
hal::ImageMsg* pbImg = vImages.add_image();
pbImg->set_timestamp(time);
pbImg->set_width(frame->width);
pbImg->set_height(frame->height);
pbImg->set_serial_number(m_devices[i].serialNumber);
pbImg->set_type(hal::PB_FLOAT);
pbImg->set_format(hal::PB_LUMINANCE);
cv::Mat trg(frame->height, frame->width, CV_32F, frame->data);
cv::flip(trg, trg, 1);
pbImg->set_data(trg.ptr<float>(),
trg.rows * trg.cols * sizeof(float));
}
if ((fit = frames.find(libfreenect2::Frame::Depth)) != frames.end()) {
const libfreenect2::Frame* frame = fit->second;
cv::Mat trg = cv::Mat(frame->height, frame->width, CV_32FC1,
frame->data);
if (save_rgb && m_bAlign) {
if (m_nImgHeight != IR_IMAGE_HEIGHT
|| m_nImgWidth != IR_IMAGE_WIDTH)
m_devices[i].registration->depthToRGBResolution(trg, trg);
}
// change rgb and depth image
hal::ImageMsg* pbImg = vImages.add_image();
pbImg->set_timestamp(time);
pbImg->set_width(trg.cols);
pbImg->set_height(trg.rows);
pbImg->set_serial_number(m_devices[i].serialNumber);
pbImg->set_type(hal::PB_FLOAT);
pbImg->set_format(hal::PB_LUMINANCE);
cv::flip(trg, trg, 1);
pbImg->set_data(trg.ptr<float>(),
trg.rows * trg.cols * sizeof(float));
}
m_devices[i].listener->release(frames);
}
return true;
}