Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- SR300Camera::SR300Camera(bool use_live_sensor)
- {
- if (!use_live_sensor) {
- return;
- }
- int res;
- std::cout << "Trying to open SR300 camera\n";
- if (!pp) {
- wprintf_s(L"Unable to create the SenseManager\n");
- }
- cm = pp->QueryCaptureManager();
- //Status sts= STATUS_NO_ERROR;
- Status sts = STATUS_DATA_UNAVAILABLE; //mona is this the correct initialization?
- //do {
- bool revert = false;
- pp->EnableStream(PXCCapture::StreamType::STREAM_TYPE_DEPTH,
- X_DIMENSION, Y_DIMENSION, depth_fps);
- revert = true;
- sts = pp->Init();
- if (sts < Status::STATUS_NO_ERROR) {
- if (revert) {
- pp->Close();
- pp->EnableStream(PXCCapture::STREAM_TYPE_DEPTH);
- sts = pp->Init();
- if (sts < Status::STATUS_NO_ERROR) {
- pp->Close();
- //pp->EnableStream(Capture::STREAM_TYPE_COLOR);
- sts = pp->Init();
- }
- }
- if (sts < Status::STATUS_NO_ERROR) {
- wprintf_s(L"Failed to locate any video stream(s)\n");
- pp->Release();
- }
- }
- device = pp->QueryCaptureManager()->QueryDevice();
- //device->ResetProperties(PXCCapture::STREAM_TYPE_ANY);
- //mona mona for (int nframes = 0; ; nframes++) {
- sts = pp->AcquireFrame(false);
- if (sts < Status::STATUS_NO_ERROR) {
- if (sts == Status::STATUS_STREAM_CONFIG_CHANGED) {
- wprintf_s(L"Stream configuration was changed, re-initilizing\n");
- pp->Close();
- }
- //mona mona break;
- }
- sample = pp->QuerySample();
- if (sample) {
- cv::Mat img;
- Converter::ConvertPXCImageToOpenCVMat(sample->depth, &img);
- cv::imshow("Depth Image by OpenARK", Visualizer::visualizeDepthMap(img));
- //mona mona if (sample->depth && !renderd.RenderFrame(sample->depth)) break;
- renderd.RenderFrame(sample->depth);
- PXCImage::ImageData depthImage;
- PXCImage *depthMap = sample->depth;
- depthMap->AcquireAccess(PXCImage::ACCESS_READ, &depthImage);
- //depthMap->AcquireAccess(PXCImage::ACCESS_WRITE, &depthImage); //mona is this the correct flag?
- PXCImage::ImageInfo imgInfo = depthMap->QueryInfo();
- int depth_stride = depthImage.pitches[0] / sizeof(pxcU16);
- PXCProjection * projection = device->CreateProjection();
- cout << "projection is: " << projection;
- cout << "device is: " << device;
- pxcU16 *dpixels = (pxcU16*)depthImage.planes[0];
- unsigned int dpitch = depthImage.pitches[0] / sizeof(pxcU16);
- PXCPoint3DF32 *pos3D = new PXCPoint3DF32[num_pixels];
- projection->QueryVertices(depthMap, pos3D);
- for (int k = 0; k < num_pixels; k++) {
- /*if (pos3D[k].x != 0) {
- cout << " xx is: " << pos3D[k].x << endl;
- }
- if (pos3D[k].y != 0) {
- cout << " yy is: " << pos3D[k].y << endl;
- }
- if (pos3D[k].z != 0) {
- cout << " zz is: " << pos3D[k].z << endl;
- }*/
- //xyzBuffer.push_back(cv::Point3f(pos3D[k].x, pos3D[k].y, pos3D[k].z));
- xyzBuffer.emplace_back(cv::Point3f(pos3D[k].x, pos3D[k].y, pos3D[k].z));
- }
- //xyzMap = cv::Mat(xyzBuffer);
- cv::Mat xyzMap{ xyzBuffer };
- }
- printf("opened sensor\n");
- pp->ReleaseFrame();
- printf("acquired image\n");
- //cout << "xyzMap = " << endl << " " << xyzMap << endl << endl;
- //mona mona }
- //} while (sts == Status::STATUS_STREAM_CONFIG_CHANGED);
- //numRows = width and numCol = height? ?
- //numPixels = dd.img.numRows * dd.img.numColumns; // Number of pixels in camera
- numPixels = depth_width * depth_height;
- dists = new float[3 * numPixels]; // Dists contains XYZ values. needs to be 3x the size of numPixels
- amps = new float[numPixels];
- //frame = cvCreateImage(cvSize(dd.img.numColumns, dd.img.numRows), 8, 3); // Create the frame
- frame = cvCreateImage(cvSize(depth_width, depth_height), 8, 3); // Create the frame
- KF.init(6, 3, 0);
- KF.transitionMatrix = (cv::Mat_<float>(6, 6) << 1, 0, 0, 1, 0, 0,
- 0, 1, 0, 0, 1, 0,
- 0, 0, 1, 0, 0, 1,
- 0, 0, 0, 1, 0, 0,
- 0, 0, 0, 0, 1, 0,
- 0, 0, 0, 0, 0, 1);
- measurement = cv::Mat_<float>::zeros(3, 1);
- //Initaite Kalman
- KF.statePre.setTo(0);
- setIdentity(KF.measurementMatrix);
- setIdentity(KF.processNoiseCov, cv::Scalar::all(.001)); // Adjust this for faster convergence - but higher noise
- setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-1));
- setIdentity(KF.errorCovPost, cv::Scalar::all(.1));
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement