Swissranger 4000 and OpenCV and Point Cloud Library
Dennis GuseI am back on working with Depth Cameras and hand gesture recognition - now we are using a Swissranger SR4000 - a TOF camera - instead a Kinect. Here is demo code to get the camera data to OpenCV as well as Point Cloud Library.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
/**
* Demo program for the SR4k that shows the output (DepthImage and PCL) and can export these as images.
* Use SPACE to take a picture and ESC for end.
* @author Dennis Guse
*/
#include <stdio.h>
#include <iostream>
#include <string>
#include <time.h>
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/imgproc/imgproc_c.h"
//SR4k
#include "libMesaSR.h"
#include "definesSR.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl/visualization/cloud_viewer.h"
using namespace cv;
using namespace std;
using namespace pcl;
#define SR_ROWS 176
#define SR_COLS 144
/**
* Takes a picture with the SR4k and returns the depthimage as well as the point cloud!
*/
cv::Mat takePicture(SRCAM srCam, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
SR_Acquire(srCam);
cv::Mat depthImage(SR_ROWS, SR_COLS, SR_CV_PIXELTYPE, (unsigned short*)SR_GetImage(srCam, 0)); //0:DepthImage; 1:Amplitude; 2:ConfidenceMap
float x [SR_ROWS * SR_COLS];
float y [SR_ROWS * SR_COLS];
float z [SR_ROWS * SR_COLS];
SR_CoordTrfFlt(srCam, x, y, z, sizeof(float), sizeof(float), sizeof(float));
for(int i=0; i<SR_ROWS * SR_COLS; i++) {
point.x=x[i];
point.y=y[i];
point.z=z[i];
point->points.push_back(point);
}
return depthImage;
}
int main(int argc, char **argv) {
SRCAM srCam;
SR_OpenETH(&srCam, SR_IP_ADDR); //Add error handling
SR_SetMode(srCam, AM_COR_FIX_PTRN|AM_CONV_GRAY|AM_DENOISE_ANF|AM_CONF_MAP);
pcl::visualization::CloudViewer viewer ("PCLViewer");
while(true) {
pcl::PointCloud<pcl::pointxyz>::Ptr cloud(new pcl::PointCloud<pcl::pointxyz>);
cv::Mat depthImage = takePicture(srCam, cloud);
cv::imshow("Depth", depthIamge);
viewer.showCloud(cloud, "Cloud");
int key = waitKey(1);
if (key == KEY_ESC) break;
if (key != -1) saveDepthImageAndCloud(depthImage, cloud);
}
}