Now, i'm using real sense D435 camera.
I installed sdk 2.0 full package and upgraded camera version 5.1 to 5.9(latest version).
I want to code to get color image and depth image using visual studio 2015.
so i coded
#include <iostream>
#include "pxcsession.h"
#include "pxcprojection.h"
#include "pxcsensemanager.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Windows.h>
#pragma comment(lib, "winmm.lib")
using namespace cv;
using namespace std;
class RealSenseAsenseManager
{
public:
~RealSenseAsenseManager()
{
if (senseManager != 0) {
senseManager->Release();
}
}
void initialize()
{
senseManager = PXCSenseManager::CreateInstance();
if (senseManager == nullptr) {
throw std::runtime_error("SenseManager failed");
}
pxcStatus sts = senseManager->EnableStream(
PXCCapture::StreamType::STREAM_TYPE_DEPTH,
DEPTH_WIDTH, DEPTH_HEIGHT, DEPTH_FPS);
if (sts < PXC_STATUS_NO_ERROR) {
throw std::runtime_error("Depth stream activation failed");
}
sts = senseManager->Init();
if (sts < PXC_STATUS_NO_ERROR) {
throw std::runtime_error("Pipeline Initialzation failed");
}
senseManager->QueryCaptureManager()->QueryDevice()->SetMirrorMode(
PXCCapture::Device::MirrorMode::MIRROR_MODE_HORIZONTAL);
}
void run()
{
while (1) {
updateFrame();
auto ret = showImage();
if (!ret) {
break;
}
}
}
private:
void updateFrame()
{
pxcStatus sts = senseManager->AcquireFrame(false);
if (sts < PXC_STATUS_NO_ERROR) {
return;
}
const PXCCapture::Sample *sample = senseManager->QuerySample();
if (sample) {
updateDepthImage(sample->depth);
}
senseManager->ReleaseFrame();
}
void updateDepthImage(PXCImage* depthFrame)
{
if (depthFrame == 0) {
return;
}
PXCImage::ImageData data;
pxcStatus sts = depthFrame->AcquireAccess(
PXCImage::Access::ACCESS_READ,
PXCImage::PixelFormat::PIXEL_FORMAT_RGB32, &data);
if (sts < PXC_STATUS_NO_ERROR) {
throw std::runtime_error("Taking Depth image failed");
}
PXCImage::ImageInfo info = depthFrame->QueryInfo();
depthImage = cv::Mat(info.height, info.width, CV_8UC4);
memcpy(depthImage.data, data.planes[0], data.pitches[0] * info.height);
depthFrame->ReleaseAccess(&data);
}
bool showImage()
{
if (depthImage.rows == 0 || (depthImage.cols == 0)) {
return true;
}
cv::imshow("Depth Image", depthImage);
int c = cv::waitKey(10);
if ((c == 27) || (c == 'q') || (c == 'Q')) {
// ESC|q|Q for Exit
return false;
}
return true;
}
private:
cv::Mat depthImage;
PXCSenseManager *senseManager = 0;
const int DEPTH_WIDTH = 640;
const int DEPTH_HEIGHT = 480;
const int DEPTH_FPS = 30.0f;
};
void main()
{
try {
RealSenseAsenseManager deep;
deep.initialize();
deep.run();
}
catch (std::exception& ex) {
std::cout << ex.what() << std::endl;
}
}
But this error appears.
sts = senseManager->Init();
if (sts < PXC_STATUS_NO_ERROR) {
throw std::runtime_error("Pipeline Initialzation failed");
}
Pipeline Initialization failed <-
I don't know how to solve this problem.
The depth camera connection is not likely to be wrong.
The color image is displayed. Only depth video is not available.
How I can solve this problem??
Thank you for reading my question.
The D400 series cameras aren't compatible with the old Realsense SDK, only the new librealsense SDK, available here: https://github.com/IntelRealSense/librealsense.
A sample showing how to get the colour and depth images streaming is here: https://github.com/IntelRealSense/librealsense/tree/master/examples/capture
You can start by using one of the provided examples.
The code below configures the camera and renders Depth & RGB data:
(the example.hpp header is located in the main repo /examples dir)
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include "example.hpp" // Include short list of convenience functions for rendering
// Capture Example demonstrates how to
// capture depth and color video streams and render them to the screen
int main(int argc, char * argv[]) try
{
rs2::log_to_console(RS2_LOG_SEVERITY_ERROR);
// Create a simple OpenGL window for rendering:
window app(1280, 720, "RealSense Capture Example");
// Declare two textures on the GPU, one for color and one for depth
texture depth_image, color_image;
// Declare depth colorizer for pretty visualization of depth data
rs2::colorizer color_map;
// Declare RealSense pipeline, encapsulating the actual device and sensors
rs2::pipeline pipe;
// Start streaming with default recommended configuration
pipe.start();
while(app) // Application still alive?
{
rs2::frameset data = pipe.wait_for_frames(); // Wait for next set of frames from the camera
rs2::frame depth = color_map(data.get_depth_frame()); // Find and colorize the depth data
rs2::frame color = data.get_color_frame(); // Find the color data
// For cameras that don't have RGB sensor, we'll render infrared frames instead of color
if (!color)
color = data.get_infrared_frame();
// Render depth on to the first half of the screen and color on to the second
depth_image.render(depth, { 0, 0, app.width() / 2, app.height() });
color_image.render(color, { app.width() / 2, 0, app.width() / 2, app.height() });
}
return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
return EXIT_FAILURE;
}
catch (const std::exception& e)
{
std::cerr << e.what() << std::endl;
return EXIT_FAILURE;
}
Related
I am trying to make a program that uses servos connected to an arduino that follows your face. I'm using visual c++ in visual studio 2017 with opencv 4.1 to do the facial recognition, then sending the location of the identified face to an arduino via serial connection.
I'm new to opencv so I've been going through many tutorials to try to make a code that works.
The plan is to do the facial recognition then calculate the location of the face in visual studio. Then combine the x and y locations to a single string and send it to the arduino with serial. The arduino then splits the coordinates, separated by a colon, using strtok(). It then will move servos accordingly to keep the tracked face in center screen.
I have tested the serial comm in a separate c++ project and it seems to work fine with the current arduino code (not anywhere near finished because I ran into a hiccup wiyh serial comm) except when I send the location string, the servo moves to the desired spot then returns to its starting location.
When I try to implement the serial communication in the project with opencv, it sends the location once, then appears to stop sending serial commands. I've tried debugging by manually, by calling the sendSerial function in other locations, to see if I can get it to send. I've tried looking around for solutions but haven't found any definite solutions other than it may be the waitKey(10) function. If this is so, is there a way around this?
Thanks.
###############SerialPort.h##############
#ifndef SERIALPORT_H
#define SERIALPORT_H
#define ARDUINO_WAIT_TIME 2000
#define MAX_DATA_LENGTH 255
#include <windows.h>
#include <stdio.h>
#include <stdlib.h>
class SerialPort
{
private:
HANDLE handler;
bool connected;
COMSTAT status;
DWORD errors;
public:
SerialPort(char *portName);
~SerialPort();
int readSerialPort(char *buffer, unsigned int buf_size);
bool writeSerialPort(char *buffer, unsigned int buf_size);
bool isConnected();
};
#endif // SERIALPORT_H
#################SerialSource.cpp##################
#include "SerialPort.h"
SerialPort::SerialPort(char *portName)
{
this->connected = false;
this->handler = CreateFileA(static_cast<LPCSTR>(portName),
GENERIC_READ | GENERIC_WRITE,
0,
NULL,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
NULL);
if (this->handler == INVALID_HANDLE_VALUE) {
if (GetLastError() == ERROR_FILE_NOT_FOUND) {
printf("ERROR: Handle was not attached. Reason: %s not available\n", portName);
}
else
{
printf("ERROR!!!");
}
}
else {
DCB dcbSerialParameters = { 0 };
if (!GetCommState(this->handler, &dcbSerialParameters)) {
printf("failed to get current serial parameters");
}
else {
dcbSerialParameters.BaudRate = CBR_9600;
dcbSerialParameters.ByteSize = 8;
dcbSerialParameters.StopBits = ONESTOPBIT;
dcbSerialParameters.Parity = NOPARITY;
dcbSerialParameters.fDtrControl = DTR_CONTROL_ENABLE;
if (!SetCommState(handler, &dcbSerialParameters))
{
printf("ALERT: could not set Serial port parameters\n");
}
else {
this->connected = true;
PurgeComm(this->handler, PURGE_RXCLEAR | PURGE_TXCLEAR);
Sleep(ARDUINO_WAIT_TIME);
}
}
}
}
SerialPort::~SerialPort()
{
if (this->connected) {
this->connected = false;
CloseHandle(this->handler);
}
}
int SerialPort::readSerialPort(char *buffer, unsigned int buf_size)
{
DWORD bytesRead;
unsigned int toRead = 0;
ClearCommError(this->handler, &this->errors, &this->status);
if (this->status.cbInQue > 0) {
if (this->status.cbInQue > buf_size) {
toRead = buf_size;
}
else toRead = this->status.cbInQue;
}
if (ReadFile(this->handler, buffer, toRead, &bytesRead, NULL)) return bytesRead;
return 0;
}
bool SerialPort::writeSerialPort(char *buffer, unsigned int buf_size)
{
DWORD bytesSend;
if (!WriteFile(this->handler, (void*)buffer, buf_size, &bytesSend, 0)) {
ClearCommError(this->handler, &this->errors, &this->status);
return false;
}
else return true;
}
bool SerialPort::isConnected()
{
return this->connected;
}
###################faceDetect.cpp################
// CPP program to detects face in a video
// Include required header files from OpenCV directory
#include <opencv2/objdetect.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <string>
#include <stdlib.h>
#include "SerialPort.h"
#include <sstream>
#include <iomanip>
using namespace std;
using namespace cv;
//Set up serial comm
char output[MAX_DATA_LENGTH];
char port[] = "\\\\.\\COM3";
char incoming[MAX_DATA_LENGTH];
// Function for Face Detection
void detectAndDraw(Mat& img, CascadeClassifier& cascade, double scale, SerialPort arduino);
string cascadeName;
// Function for sending locations to arduino
void sendSerial(string locations, SerialPort arduino);
int main(int argc, const char** argv)
{
//Establish connection to serial
SerialPort arduino(port);
if (arduino.isConnected()) {
cout << "COnnection Established" << endl;
}
else {
cout << "Error in port name" << endl;
}
// VideoCapture class for playing video for which faces to be detected
VideoCapture capture;
Mat frame, image;
// PreDefined trained XML classifiers with facial features
CascadeClassifier cascade;
double scale = 1;
// Change path before execution
cascade.load("C:/opencv/sources/data/haarcascades/haarcascade_frontalface_default.xml");
// Start Video..1) 0 for WebCam 2) "Path to Video" for a Local Video
capture.open(CAP_MSMF);
//sendSerial("400:100", arduino);
if (capture.isOpened())
{
// Capture frames from video and detect faces
cout << "Face Detection Started...." << endl;
while (1)
{
capture >> frame;
if (frame.empty())
break;
Mat frame1 = frame.clone();
detectAndDraw(frame1, cascade, scale, arduino);
char c = (char)waitKey(10);
// Press q to exit from window
if (c == 27 || c == 'q' || c == 'Q')
break;
}
}
else
cout << "Could not Open Camera";
return 0;
}
void sendSerial(string locations, SerialPort arduino) {
//string command;
//command = to_string(xloc);
cout << locations << endl;
char *charArray = new char[locations.size() + 1];
copy(locations.begin(), locations.end(), charArray);
charArray[locations.size()] = '\n';
arduino.writeSerialPort(charArray, MAX_DATA_LENGTH);
//arduino.readSerialPort(output, MAX_DATA_LENGTH);
//cout << output;
delete[] charArray;
//
//command = to_string(yloc);
//copy(command.begin(), command.end(), charArray);
//charArray[command.size()] = '\n';
//arduino.writeSerialPort(charArray, MAX_DATA_LENGTH);
////arduino.readSerialPort(output, MAX_DATA_LENGTH);
////cout << output;
//delete[] charArray;
}
void detectAndDraw(Mat& img, CascadeClassifier& cascade,
double scale, SerialPort arduino)
{
vector<Rect> faces;
Mat gray, smallImg;
cvtColor(img, gray, COLOR_BGR2GRAY); // Convert to Gray Scale
double fx = 1 / scale;
// Resize the Grayscale Image
resize(gray, smallImg, Size(), fx, fx, INTER_LINEAR);
equalizeHist(smallImg, smallImg);
// Detect faces of different sizes using cascade classifier
cascade.detectMultiScale(smallImg, faces, 1.1,
2, 0 | CASCADE_SCALE_IMAGE, Size(30, 30));
// Draw circles around the faces
for (size_t i = 0; i < faces.size(); i++)
{
Rect r = faces[i];
Mat smallImgROI;
int x = faces[i].x;
int y = faces[i].y;
int h = y + faces[i].height;
int w = x + faces[i].width;
int centerX = x + (.5* faces[i].width);
int centerY = y + (.5* faces[i].height);
if (abs(320 - centerX) <= 50) {
if (abs(240 - centerY) <= 50) {
rectangle(img,
Point(x, y),
Point(w, h),
Scalar(0, 0, 275),
2,
8,
0);
}
}
else {
rectangle(img,
Point(x, y),
Point(w, h),
Scalar(275, 275, 275),
2,
8,
0);
}
stringstream stringX;
stringstream stringY;
stringX << std::setw(3) << std::setfill('0') << centerX;
stringY << std::setw(3) << std::setfill('0') << centerY;
std::stringstream ss;
//ss << std::setw(3) << std::setfill('0') << centerX << ":"<< centerY;
//std::string s = ss.str();
std::string s = stringX.str() + ":" + stringY.str();
//cout << s << endl;
sendSerial(s, arduino);
smallImgROI = smallImg(r);
if (arduino.isConnected()) {
cout << "COnnection Established" << endl;
//sendSerial("400:100", arduino);
}
}
// Show Processed Image with detected faces
imshow("Face Detection", img);
}
#####################arduino code################
#include <Servo.h>
String input;
char array[6];
char *strings[3];
char *ptr = NULL;
int xloc;
int yloc;
int hServoPin = 9;
Servo hServo;
int ledPin = 13;
void setup() {
//set up servos
hServo.attach(hServoPin);
//start serial connection
Serial.begin(9600);
//***** delete later *****
pinMode(ledPin, OUTPUT);
}
void loop() {
if(Serial.available()){
//grab "xloc:yloc" and convert to char array
input = Serial.readStringUntil('\n');
//delete later
//Serial.print("input; ");
//Serial.println(input);
for(int i = 0; i<6; i++){
array[i] = input.charAt(i);
//Serial.print(array[i]);
}
//split char array into two entities
byte index = 0;
ptr = strtok(array, ":;"); // takes a list of delimiters
while(ptr != NULL)
{
strings[index] = ptr;
index++;
ptr = strtok(NULL, ":;"); // takes a list of delimiters
//Serial.println("loop");
}
//set xloc and yloc respectively
xloc = atoi(strings[0]);
yloc = atoi(strings[1]);
}
if((xloc < 214)){
hServo.write(0);
delay(100);
}
else if((xloc > 214) && (xloc < 328)){
hServo.write(90);
delay(100);
}
else if((xloc > 328)){
hServo.write(180);
delay(100);
}
}
How do I take images with Sapera SDK and transfer the image data from the SapBuffer object to vector?
To handle images taken by camera using Sapera, you should make specialization of SapProcessing class, which is used to process buffers. Otherwise the buffer is cleared automatically after each frame, and you lose the data.
The imaging process goes as follows:
You call Grab() on the camera object to start imaging
After each frame has been taken, transfer callback is called. Here you request your SapProcessing object to process the next frame.
Run() function of your SapProcessing object is called. Here you can read data from the buffer.
After Run() function, processing callback is called.
When you have received enough frames, call Freeze() to stop imaging.
This example code takes images using default settings on the camera (monochrome 8-bit pixel format).
#include <string>
#include <vector>
#include <memory>
#include <stdexcept>
#include <iostream>
#include <iomanip>
#include <atomic>
#include "SapClassBasic.h"
// Helper function to find the camera by its serial number
SapAcqDevice getDeviceBySN(const std::string& sn)
{
char serverName[CORSERVER_MAX_STRLEN];
char serialNumberName[2048];
const int serverCount = SapManager::GetServerCount();
for (int i = 0; i < serverCount; i++) {
if (SapManager::GetResourceCount(i, SapManager::ResourceAcqDevice) != 0)
{
SapManager::GetServerName(i, serverName, sizeof(serverName));
SapAcqDevice camera(serverName);
if (!camera.Create()) {
throw std::runtime_error("Failed to create camera object.");
}
int featureCount;
if (camera.GetFeatureCount(&featureCount) && featureCount > 0)
{
if (camera.GetFeatureValue("DeviceID", serialNumberName, sizeof(serialNumberName))
&& serialNumberName == sn)
{
return camera;
}
}
camera.Destroy();
}
}
const auto errorStr = "Camera \"" + sn + "\" was not found.";
throw std::runtime_error(errorStr.c_str());
}
class SapMyProcessing : public SapProcessing
{
public:
SapMyProcessing(SapBuffer* pBuffers, SapProCallback pCallback, void* pContext);
virtual ~SapMyProcessing();
protected:
virtual BOOL Run();
};
SapMyProcessing::SapMyProcessing(SapBuffer* pBuffers, SapProCallback pCallback, void* pContext)
: SapProcessing(pBuffers, pCallback, pContext)
{}
SapMyProcessing::~SapMyProcessing()
{
if (m_bInitOK) Destroy();
}
BOOL SapMyProcessing::Run()
{
// Get the current buffer index
const int proIndex = GetIndex();
// If this is not true, buffer has overflown
SapBuffer::State state;
bool goodContent = m_pBuffers->GetState(proIndex, &state)
&& state == SapBuffer::StateFull;
if (goodContent) {
void *inAddress = nullptr;
m_pBuffers->GetAddress(proIndex, &inAddress);
int inSize = 0;
m_pBuffers->GetSpaceUsed(proIndex, &inSize);
// Width, height and pixel format are received from the camera
const int width = m_pBuffers->GetWidth();
const int height = m_pBuffers->GetHeight();
const auto format = m_pBuffers->GetFormat();
const int outSize = width * height;
// Skip unexpected pixel format or incomplete frame
goodContent = format == SapFormatMono8
&& inSize == outSize;
if (goodContent) {
// Copy data to vector
std::vector<uint8_t> outBuffer(outSize);
std::copy((uint8_t*)inAddress, (uint8_t*)(inAddress) + outSize, outBuffer.begin());
// Print the first line
for (int i = 0; i < width; i++) {
std::cout << std::hex << int(outBuffer[i]);
}
std::cout << std::endl << std::endl;
}
}
return TRUE;
}
// Information to pass to callbacks
struct TransferContext {
std::atomic_int frameGrabCount = 0, frameProcessingCount = 0;
std::shared_ptr<SapMyProcessing> processing;
};
void transferCallback(SapXferCallbackInfo *info)
{
auto context = (TransferContext*)info->GetContext();
context->frameGrabCount++;
if (!info->IsTrash()) {
// Execute Run() for this frame
context->processing->ExecuteNext();
}
}
// Processing callback is called after Run()
void processingCallback(SapProCallbackInfo* info)
{
auto context = (TransferContext*)info->GetContext();
// Processing has finished
context->frameProcessingCount++;
}
// The main imaging function
void grab(const std::string& serialNumber)
{
// Number of frames to receive from the camera
const int maxFrameCount = 10;
TransferContext context;
auto camera = getDeviceBySN(serialNumber);
std::unique_ptr<SapBuffer> buffer
= std::make_unique<SapBufferWithTrash>(maxFrameCount, &camera);
std::unique_ptr<SapTransfer> transfer
= std::make_unique<SapAcqDeviceToBuf>(&camera, buffer.get(), transferCallback, &context);
context.processing = std::make_shared<SapMyProcessing>(buffer.get(), processingCallback, &context);
auto cleanup = [&]() {
if (context.processing) context.processing->Destroy();
if (transfer) transfer->Destroy();
if (buffer) buffer->Destroy();
camera.Destroy();
};
try {
if (!buffer->Create()) {
throw std::runtime_error("Failed to create buffer object.");
}
if (!transfer->Create()) {
throw std::runtime_error("Failed to create transfer object.");
}
if (!context.processing->Create()) {
throw std::runtime_error("Failed to create processing object.");
}
transfer->SetAutoEmpty(false);
context.processing->SetAutoEmpty(true);
context.processing->Init();
transfer->Grab();
// Wait for the camera to grab all frames
while (context.frameGrabCount < maxFrameCount);
transfer->Freeze();
if (!transfer->Wait(5000)) {
throw std::runtime_error("Failed to stop grab.");
}
// Wait for processing to complete
while (context.frameProcessingCount < maxFrameCount);
cleanup();
}
catch (...) {
cleanup();
throw;
}
}
In a project I'm working on, I'd like to use stb_vorbis to stream audio from an ogg file. However, after implementing the processes to do this, I found the audio was crackly. I feel the problem may be similar to this question, but I can't see where a problem could be.
Here is my code:
#include <SDL2/SDL.h>
#include <stb/vorbis.h>
#include <iostream>
void sdlAudioCallback(void* userData, Uint8* stream, int len)
{
stb_vorbis* myVorbis = (stb_vorbis*) userData;
SDL_memset(stream, 0, len);
stb_vorbis_get_frame_short_interleaved(myVorbis, 2, (short*) stream, len/sizeof(short));
}
int main()
{
if (SDL_Init(SDL_INIT_AUDIO) != 0)
return -1;
int error = 0;
stb_vorbis_alloc* alloc;
stb_vorbis* vorbis = stb_vorbis_open_filename("res/thingy.ogg", &error, alloc);
if (error != 0)
return -2;
stb_vorbis_info info = stb_vorbis_get_info(vorbis);
SDL_AudioSpec spec;
spec.freq = info.sample_rate;
spec.format = AUDIO_S16;
spec.channels = 2;
spec.samples = 1024;
spec.callback = sdlAudioCallback;
spec.userdata = vorbis;
if (SDL_OpenAudio(&spec, NULL) < 0) {
std::cout << "failed to open audio device: " << SDL_GetError() << std::endl;
SDL_Quit();
return -3;
}
SDL_PauseAudio(0);
SDL_Delay(5000);
}
More information:
thingy.ogg is from Sean Barrett's samples
building with g++ on an OSX machine
// Sample_PCL.cpp : Defines the entry point for the console application.
//
#include "stdafx.h"
#define NOMINMAX
#include <Windows.h>
#include <Kinect.h> // Kinectを使用するためのヘッダファイル
#include <pcl/visualization/cloud_viewer.h> // PCLを使用して表示するためのヘッダファイル
#include <pcl/io/pcd_io.h> // 点群データを保存するためのヘッダファイル(.pcd, .ply)
//#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <iostream>
template<class Interface>
inline void SafeRelease(Interface *& pInterfaceToRelease)
{
if (pInterfaceToRelease != NULL) {
pInterfaceToRelease->Release();
}
}
int main()
{
// Create Sensor Instance
IKinectSensor* pSensor;
HRESULT hResult = S_OK;
hResult = GetDefaultKinectSensor(&pSensor);
if (FAILED(hResult)) {
std::cerr << "Error : GetDefaultKinectSensor" << std::endl;
return -1;
}
printf("GetDfaultKinectSensor is OK\n");
// Open Sensor
hResult = pSensor->Open();
if (FAILED(hResult)) {
std::cerr << "Error : IKinectSensor::Open()" << std::endl;
return -1;
}
printf("IKinectSensor::Open is OK\n");
// Retrieved Coordinate Mapper
ICoordinateMapper* pCoordinateMapper;
hResult = pSensor->get_CoordinateMapper(&pCoordinateMapper);
if (FAILED(hResult)) {
std::cerr << "Error : IKinectSensor::get_CoordinateMapper()" << std::endl;
return -1;
}
printf("IKinectSensor::get_CoordinateMapper is OK\n");
// Retrieved Color Frame Source
IColorFrameSource* pColorSource;
hResult = pSensor->get_ColorFrameSource(&pColorSource);
if (FAILED(hResult)) {
std::cerr << "Error : IKinectSensor::get_ColorFrameSource()" << std::endl;
return -1;
}
printf("IKinectSensor::get_ColorFrameSource is OK\n");
// Open Color Frame Reader
IColorFrameReader* pColorReader;
hResult = pColorSource->OpenReader(&pColorReader);
if (FAILED(hResult)) {
std::cerr << "Error : IColorFrameSource::OpenReader()" << std::endl;
return -1;
}
printf("IColorFrameSource::OpenReader is OK\n");
// Retrieved Depth Frame Source
IDepthFrameSource* pDepthSource;
hResult = pSensor->get_DepthFrameSource(&pDepthSource);
if (FAILED(hResult)) {
std::cerr << "Error : IKinectSensor::get_DepthFrameSource()" << std::endl;
return -1;
}
printf("IKinectSensor::get_DepthFrameSource is OK\n");
// Open Depth Frame Reader
IDepthFrameReader* pDepthReader;
hResult = pDepthSource->OpenReader(&pDepthReader);
if (FAILED(hResult)) {
std::cerr << "Error : IDepthFrameSource::OpenReader()" << std::endl;
return -1;
}
printf("IDepthFrameSource::OpenReader is OK\n");
// Retrieved Color Frame Size
IFrameDescription* pColorDescription;
hResult = pColorSource->get_FrameDescription(&pColorDescription);
if (FAILED(hResult)) {
std::cerr << "Error : IColorFrameSource::get_FrameDescription()" << std::endl;
return -1;
}
printf("IColorFrameSource::get_FrameDescription is OK\n");
int colorWidth = 0;
int colorHeight = 0;
pColorDescription->get_Width(&colorWidth); // 1920
pColorDescription->get_Height(&colorHeight); // 1080
// To Reserve Color Frame Buffer
std::vector<RGBQUAD> colorBuffer(colorWidth * colorHeight);
// Retrieved Depth Frame Size
IFrameDescription* pDepthDescription;
hResult = pDepthSource->get_FrameDescription(&pDepthDescription);
if (FAILED(hResult)) {
std::cerr << "Error : IDepthFrameSource::get_FrameDescription()" << std::endl;
return -1;
}
printf("IDepthFrameSource::get_FrameDescription is OK\n");
int depthWidth = 0;
int depthHeight = 0;
pDepthDescription->get_Width(&depthWidth); // 512
pDepthDescription->get_Height(&depthHeight); // 424
// To Reserve Depth Frame Buffer
std::vector<UINT16> depthBuffer(depthWidth * depthHeight);
printf("Display Point Cloud\n");
// Create Cloud Viewer
pcl::visualization::CloudViewer viewer("Point Cloud Viewer"); // 点群のウィンドウ表示
while (!viewer.wasStopped()) {
// Acquire Latest Color Frame
IColorFrame* pColorFrame = nullptr;
hResult = pColorReader->AcquireLatestFrame(&pColorFrame);
if (SUCCEEDED(hResult)) {
// Retrieved Color Data
hResult = pColorFrame->CopyConvertedFrameDataToArray(colorBuffer.size() * sizeof(RGBQUAD), reinterpret_cast<BYTE*>(&colorBuffer[0]), ColorImageFormat::ColorImageFormat_Bgra);
if (FAILED(hResult)) {
std::cerr << "Error : IColorFrame::CopyConvertedFrameDataToArray()" << std::endl;
}
}
SafeRelease(pColorFrame);
// Acquire Latest Depth Frame
IDepthFrame* pDepthFrame = nullptr;
hResult = pDepthReader->AcquireLatestFrame(&pDepthFrame);
if (SUCCEEDED(hResult)) {
// Retrieved Depth Data
hResult = pDepthFrame->CopyFrameDataToArray(depthBuffer.size(), &depthBuffer[0]);
if (FAILED(hResult)) {
std::cerr << "Error : IDepthFrame::CopyFrameDataToArray()" << std::endl;
}
}
SafeRelease(pDepthFrame);
// Point Cloud Libraryの設定
// Create Point Cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>()); // PCLの構造体
pointcloud->width = static_cast<uint32_t>(depthWidth); // 点群の数
pointcloud->height = static_cast<uint32_t>(depthHeight);
pointcloud->is_dense = false;
for (int y = 0; y < depthHeight; y++) {
for (int x = 0; x < depthWidth; x++) {
pcl::PointXYZRGB point; // PCLで使用する点群情報
DepthSpacePoint depthSpacePoint = { static_cast<float>(x), static_cast<float>(y) };
UINT16 depth = depthBuffer[y * depthWidth + x];
// Coordinate Mapping Depth to Color Space, and Setting PointCloud RGB
ColorSpacePoint colorSpacePoint = { 0.0f, 0.0f };
pCoordinateMapper->MapDepthPointToColorSpace(depthSpacePoint, depth, &colorSpacePoint); // 色の座標系
int colorX = static_cast<int>(std::floor(colorSpacePoint.X + 0.5f));
int colorY = static_cast<int>(std::floor(colorSpacePoint.Y + 0.5f));
if ((0 <= colorX) && (colorX < colorWidth) && (0 <= colorY) && (colorY < colorHeight)) {
RGBQUAD color = colorBuffer[colorY * colorWidth + colorX];
// 色の情報を格納する
point.b = color.rgbBlue;
point.g = color.rgbGreen;
point.r = color.rgbRed;
}
// Coordinate Mapping Depth to Camera Space, and Setting PointCloud XYZ
CameraSpacePoint cameraSpacePoint = { 0.0f, 0.0f, 0.0f }; // カメラ空間
pCoordinateMapper->MapDepthPointToCameraSpace(depthSpacePoint, depth, &cameraSpacePoint);
if ((0 <= colorX) && (colorX < colorWidth) && (0 <= colorY) && (colorY < colorHeight)) {
// 直交座標系の情報を格納する
point.x = cameraSpacePoint.X;
point.y = cameraSpacePoint.Y;
point.z = cameraSpacePoint.Z;
}
pointcloud->push_back(point);
}
}
// Show Point Cloud on Cloud Viewer
viewer.showCloud(pointcloud);
// Input Key ( Exit ESC key )
if (GetKeyState(VK_ESCAPE) < 0) {
pcl::io::savePCDFile("PointCloud.pcd", *pointcloud);
//pcl::io::savePLYFile("test_pcd2.ply", *pointcloud); // 最後に取得した点群を保存
printf("Save Point Cloud Data\n");
//break;
}
}
// End Processing
SafeRelease(pColorSource);
SafeRelease(pDepthSource);
SafeRelease(pColorReader);
SafeRelease(pDepthReader);
SafeRelease(pColorDescription);
SafeRelease(pDepthDescription);
SafeRelease(pCoordinateMapper);
if (pSensor) {
pSensor->Close();
}
SafeRelease(pSensor);
printf("Disconnect Kinect Sensor\n");
return 0;
}
The previous code is a code taken from a tutorial from the Point Cloud Library Website uses the Kinect to display a Point Cloud of what the Kinect sees in real time. Therefore The Point Cloud is constantly changing. That is why I would like to get just a frame, in other words, I would like the point cloud to freeze instead of constantly capturing new frames.
and here is my modification.
// Sample_PCL.cpp : Defines the entry point for the console application.
//
#include "stdafx.h"
#define NOMINMAX
#include <Windows.h>
#include <Kinect.h> // Kinectを使用するためのヘッダファイル
#include <pcl/visualization/cloud_viewer.h> // PCLを使用して表示するためのヘッダファイル
#include <pcl/io/pcd_io.h> // 点群データを保存するためのヘッダファイル(.pcd, .ply)
//#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <iostream>
template<class Interface>
inline void SafeRelease(Interface *& pInterfaceToRelease)
{
if (pInterfaceToRelease != NULL) {
pInterfaceToRelease->Release();
}
}
int main()
{
// Create Sensor Instance
IKinectSensor* pSensor;
HRESULT hResult = S_OK;
hResult = GetDefaultKinectSensor(&pSensor);
if (FAILED(hResult)) {
std::cerr << "Error : GetDefaultKinectSensor" << std::endl;
return -1;
}
printf("GetDfaultKinectSensor is OK\n");
// Open Sensor
hResult = pSensor->Open();
if (FAILED(hResult)) {
std::cerr << "Error : IKinectSensor::Open()" << std::endl;
return -1;
}
printf("IKinectSensor::Open is OK\n");
// Retrieved Coordinate Mapper
ICoordinateMapper* pCoordinateMapper;
hResult = pSensor->get_CoordinateMapper(&pCoordinateMapper);
if (FAILED(hResult)) {
std::cerr << "Error : IKinectSensor::get_CoordinateMapper()" << std::endl;
return -1;
}
printf("IKinectSensor::get_CoordinateMapper is OK\n");
// Retrieved Color Frame Source
IColorFrameSource* pColorSource;
hResult = pSensor->get_ColorFrameSource(&pColorSource);
if (FAILED(hResult)) {
std::cerr << "Error : IKinectSensor::get_ColorFrameSource()" << std::endl;
return -1;
}
printf("IKinectSensor::get_ColorFrameSource is OK\n");
// Open Color Frame Reader
IColorFrameReader* pColorReader;
hResult = pColorSource->OpenReader(&pColorReader);
if (FAILED(hResult)) {
std::cerr << "Error : IColorFrameSource::OpenReader()" << std::endl;
return -1;
}
printf("IColorFrameSource::OpenReader is OK\n");
// Retrieved Depth Frame Source
IDepthFrameSource* pDepthSource;
hResult = pSensor->get_DepthFrameSource(&pDepthSource);
if (FAILED(hResult)) {
std::cerr << "Error : IKinectSensor::get_DepthFrameSource()" << std::endl;
return -1;
}
printf("IKinectSensor::get_DepthFrameSource is OK\n");
// Open Depth Frame Reader
IDepthFrameReader* pDepthReader;
hResult = pDepthSource->OpenReader(&pDepthReader);
if (FAILED(hResult)) {
std::cerr << "Error : IDepthFrameSource::OpenReader()" << std::endl;
return -1;
}
printf("IDepthFrameSource::OpenReader is OK\n");
// Retrieved Color Frame Size
IFrameDescription* pColorDescription;
hResult = pColorSource->get_FrameDescription(&pColorDescription);
if (FAILED(hResult)) {
std::cerr << "Error : IColorFrameSource::get_FrameDescription()" << std::endl;
return -1;
}
printf("IColorFrameSource::get_FrameDescription is OK\n");
int colorWidth = 0;
int colorHeight = 0;
pColorDescription->get_Width(&colorWidth); // 1920
pColorDescription->get_Height(&colorHeight); // 1080
// To Reserve Color Frame Buffer
std::vector<RGBQUAD> colorBuffer(colorWidth * colorHeight);
// Retrieved Depth Frame Size
IFrameDescription* pDepthDescription;
hResult = pDepthSource->get_FrameDescription(&pDepthDescription);
if (FAILED(hResult)) {
std::cerr << "Error : IDepthFrameSource::get_FrameDescription()" << std::endl;
return -1;
}
printf("IDepthFrameSource::get_FrameDescription is OK\n");
int depthWidth = 0;
int depthHeight = 0;
pDepthDescription->get_Width(&depthWidth); // 512
pDepthDescription->get_Height(&depthHeight); // 424
// To Reserve Depth Frame Buffer
std::vector<UINT16> depthBuffer(depthWidth * depthHeight);
printf("Display Point Cloud\n");
// Create Cloud Viewer
pcl::visualization::CloudViewer viewer("Point Cloud Viewer"); // 点群のウィンドウ表示
//while (!viewer.wasStopped()) {
// Acquire Latest Color Frame
IColorFrame* pColorFrame = nullptr;
hResult = pColorReader->AcquireLatestFrame(&pColorFrame);
if (SUCCEEDED(hResult)) {
// Retrieved Color Data
hResult = pColorFrame->CopyConvertedFrameDataToArray(colorBuffer.size() * sizeof(RGBQUAD), reinterpret_cast<BYTE*>(&colorBuffer[0]), ColorImageFormat::ColorImageFormat_Bgra);
if (FAILED(hResult)) {
std::cerr << "Error : IColorFrame::CopyConvertedFrameDataToArray()" << std::endl;
}
}
SafeRelease(pColorFrame);
// Acquire Latest Depth Frame
IDepthFrame* pDepthFrame = nullptr;
hResult = pDepthReader->AcquireLatestFrame(&pDepthFrame);
if (SUCCEEDED(hResult)) {
// Retrieved Depth Data
hResult = pDepthFrame->CopyFrameDataToArray(depthBuffer.size(), &depthBuffer[0]);
if (FAILED(hResult)) {
std::cerr << "Error : IDepthFrame::CopyFrameDataToArray()" << std::endl;
}
}
SafeRelease(pDepthFrame);
// Point Cloud Libraryの設定
// Create Point Cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>()); // PCLの構造体
pointcloud->width = static_cast<uint32_t>(depthWidth); // 点群の数
pointcloud->height = static_cast<uint32_t>(depthHeight);
pointcloud->is_dense = false;
for (int y = 0; y < depthHeight; y++) {
for (int x = 0; x < depthWidth; x++) {
//printf("scann\n");
pcl::PointXYZRGB point; // PCLで使用する点群情報
DepthSpacePoint depthSpacePoint = { static_cast<float>(x), static_cast<float>(y) };
UINT16 depth = depthBuffer[y * depthWidth + x];
// Coordinate Mapping Depth to Color Space, and Setting PointCloud RGB
ColorSpacePoint colorSpacePoint = { 0.0f, 0.0f };
pCoordinateMapper->MapDepthPointToColorSpace(depthSpacePoint, depth, &colorSpacePoint); // 色の座標系
int colorX = static_cast<int>(std::floor(colorSpacePoint.X + 0.5f));
int colorY = static_cast<int>(std::floor(colorSpacePoint.Y + 0.5f));
if ((0 <= colorX) && (colorX < colorWidth) && (0 <= colorY) && (colorY < colorHeight)) {
RGBQUAD color = colorBuffer[colorY * colorWidth + colorX];
// 色の情報を格納する
point.b = color.rgbBlue;
point.g = color.rgbGreen;
point.r = color.rgbRed;
}
// Coordinate Mapping Depth to Camera Space, and Setting PointCloud XYZ
CameraSpacePoint cameraSpacePoint = { 0.0f, 0.0f, 0.0f }; // カメラ空間
pCoordinateMapper->MapDepthPointToCameraSpace(depthSpacePoint, depth, &cameraSpacePoint);
if ((0 <= colorX) && (colorX < colorWidth) && (0 <= colorY) && (colorY < colorHeight)) {
// 直交座標系の情報を格納する
point.x = cameraSpacePoint.X;
point.y = cameraSpacePoint.Y;
point.z = cameraSpacePoint.Z;
}
pointcloud->push_back(point);
}
//}
viewer.showCloud(pointcloud);
while (!viewer.wasStopped())
{
}
/*pcl::io::savePCDFile("PointCloud.pcd", *pointcloud);
printf("Saved Point Cloud Data\n");
// Show Point Cloud on Cloud Viewer
printf("Open viewer\n");
viewer.showCloud(pointcloud);
while (!viewer.wasStopped()) {
}*/
// Input Key ( Exit ESC key )
if (GetKeyState(VK_ESCAPE) < 0) {
pcl::io::savePCDFile("PointCloud.pcd", *pointcloud);
//pcl::io::savePLYFile("test_pcd2.ply", *pointcloud); // 最後に取得した点群を保存
printf("Save Point Cloud Data\n");
//break;
}
}
// End Processing
SafeRelease(pColorSource);
SafeRelease(pDepthSource);
SafeRelease(pColorReader);
SafeRelease(pDepthReader);
SafeRelease(pColorDescription);
SafeRelease(pDepthDescription);
SafeRelease(pCoordinateMapper);
if (pSensor) {
pSensor->Close();
}
SafeRelease(pSensor);
printf("Disconnect Kinect Sensor\n");
return 0;
}
The modification mainly consists of removing the loop that updates the point cloud constantly which is: You can see it commented in the second code.
while (!viewer.wasStopped())
The problem is that the the Point cloud viewer does not display any data received by the Kinect, and I would like to know the reason why it could not be displayed.
Your code only seems to show the very first frame it receives from the Kinect, which might be empty or invalid. Did you check if the points in the cloud you're taking are sane?
However, you might want to approach the problem differently:
Leave the visualization loop as it is.
Register a key handler using registerKeyboardCallback (doc).
When a specific key is pressed, set a boolean to true.
In the visualization loop, if that boolean variable is true, skip the frame grabbing from the Kinect. It should retain the previously set cloud.
I need to make small video player with OpenCV, which have to support the following functionality. Key 'p' on keyboard - pause/unpause, 'q' - exit, left and right arrow keys - play video frame by frame straight and reverse when it is paused. So the problem is when I try to show video with high quality and I hold arrow key for several seconds it does not run, but freeze and then jump to current frame after I release key. I tried to fix this with adding this_thread::sleep after cv::imshow() to give time to draw, but it did not help at all. So here is the code. Also, I have some reasons to use boost instead of C++11, so it is ok.
main.cpp
#include "VideoPlayer.hpp"
#include <iostream>
int main(int argc, char *argv[])
{
if (argc < 2) {
std::cerr << "Video file full name required as argument." << std::endl;
}
VideoPlayer vp(argv[1]);
if (!vp.play())
return 1;
return 0;
}
VideoPlayer.hpp
#pragma once
#include <opencv/cxcore.hpp>
#include <opencv/highgui.h>
#include <string>
class VideoPlayer
{
public:
VideoPlayer(const std::string &video, const std::string &windowName = "Output window",
unsigned int delay = 30);
bool play();
private:
cv::VideoCapture videoCapture_;
std::string windowName_;
unsigned int delay_;
private:
bool processKey(int key);
void setFrame(int frameNum);
};
VideoPlayer.cpp
#include "VideoPlayer.hpp"
#include <iostream>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread/thread.hpp>
VideoPlayer::VideoPlayer(const std::string &video, const std::string &windowName,
unsigned int delay)
: videoCapture_(video)
, windowName_(windowName)
, delay_(delay)
{}
bool VideoPlayer::play()
{
if (!videoCapture_.isOpened()) {
std::cerr << "Unable to open video." << std::endl;
return false;
}
cv::namedWindow(windowName_);
for (;;) {
cv::Mat frame;
videoCapture_ >> frame;
cv::imshow(windowName_, frame);
int key = cv::waitKey(delay_);
if (processKey(key))
break;
}
return true;
}
bool VideoPlayer::processKey(int key)
{
if (key == 'p') {
for (;;) {
int pausedKey = cv::waitKey(0);
if (pausedKey == 'p') {
break;
} else if (pausedKey == 'q') {
return true;
} else if (pausedKey == 65363) {
int frameNum = videoCapture_.get(CV_CAP_PROP_POS_FRAMES);
setFrame(frameNum);
} else if (pausedKey == 65361) {
int frameNum = videoCapture_.get(CV_CAP_PROP_POS_FRAMES);
setFrame(frameNum - 2);
}
}
} else if (key == 'q') {
return true;
}
return false;
}
void VideoPlayer::setFrame(int frameNum)
{
if (frameNum > 0 && frameNum < videoCapture_.get(CV_CAP_PROP_FRAME_COUNT)) {
std::cerr << frameNum << std::endl;
videoCapture_.set(CV_CAP_PROP_POS_FRAMES, frameNum);
cv::Mat frame;
videoCapture_ >> frame;
cv::imshow(windowName_, frame);
boost::this_thread::sleep(boost::posix_time::milliseconds(10));
}
}
I also created a multithreading implementation with buffer based on std::queue with lock, but it didn't solve the problem. And I tried to use boost::lockfree::queue, but I could not manage to finish it because of some strange behavior. I will share this code later, if it is necessary.
So, if somebody knows some good practice, how to avoid this problem, help me please.
EDIT:
Replacing boost::this_thread::sleep(boost::posix_time::milliseconds(10)); with cv::waitKey(0) is bad, because is makes me to do two short presses on arrow key to change one frame, and it does not help, because holding key skips it very fast. So the following code helped, but it is too strange and it is necessary to select times each for each video.
void VideoPlayer::setFrame(int frameNum)
{
if (frameNum > 0 && frameNum < videoCapture_.get(CV_CAP_PROP_FRAME_COUNT)) {
std::cerr << frameNum << std::endl;
videoCapture_.set(CV_CAP_PROP_POS_FRAMES, frameNum);
cv::Mat frame;
videoCapture_ >> frame;
cv::imshow(windowName_, frame);
int times = 7;
for (int i = 0; i < times; i++)
cv::waitKey(10);
}
}
Also, I'm unable to use Qt or something else, only C++03 with boost and OpenCV.
I think I need some trick to make cv::waitKey(time) wait fortime whether any key pressed or not.