├── .gitignore
├── CMakeLists.txt
├── CalibTest.cpp
├── OpenNICapture.cpp
├── OpenNIEngine.cpp
├── OpenNIEngine.h
├── README.md
├── StereoCalib.cpp
├── chess.pdf
└── cmake
    └── FindOpenNI.cmake


/.gitignore:
--------------------------------------------------------------------------------
1 | build/
2 | *.user
3 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | project(RGBDCalib)
 3 | 
 4 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/cmake/")
 5 | 
 6 | #set(OPEN_NI_ROOT /home/carl/Work/3rdparty/OpenNI2/)
 7 | 
 8 | find_package(OpenCV REQUIRED)
 9 | find_package(OpenNI REQUIRED)
10 | 
11 | 
12 | IF(MSVC_IDE)
13 |   add_definitions(-D_CRT_SECURE_NO_WARNINGS)
14 |   add_definitions(-DUSING_CMAKE=1)
15 | ELSE(MSVC_IDE)
16 |   set(CFLAGS_WARN "-Wall -Wextra -Wno-unused-parameter -Wno-strict-aliasing")
17 |   set(CMAKE_CXX_FLAGS "-fPIC -O3 -march=native ${CFLAGS_WARN} ${CMAKE_CXX_FLAGS}")
18 | ENDIF(MSVC_IDE)
19 | 
20 | 
21 | include_directories("/home/carl/Work/3rdparty")
22 | include_directories("/home/carl/Work/3rdparty/Eigen3")
23 | include_directories(${OPENCV_INCLUDE_DIRS})
24 | include_directories(${OpenNI_INCLUDE_DIR})
25 | 
26 | add_library(OpenNIEngine
27 | OpenNIEngine.h
28 | OpenNIEngine.cpp
29 | )
30 | 
31 | add_executable(CalibCapture 
32 | OpenNICapture.cpp 
33 | )
34 | 
35 | add_executable(StereoCalib
36 | StereoCalib.cpp)
37 | 
38 | add_executable(TestCalibResult
39 | OpenNIEngine.h
40 | OpenNIEngine.cpp
41 | CalibTest.cpp)
42 | 
43 | target_link_libraries(OpenNIEngine
44 | ${OpenCV_LIBS} 
45 | ${OpenNI_LIBRARY}
46 | )
47 | 
48 | target_link_libraries(CalibCapture 
49 | OpenNIEngine
50 | ${OpenCV_LIBS} 
51 | ${OpenNI_LIBRARY}
52 | )
53 | 
54 | 
55 | target_link_libraries(StereoCalib 
56 | ${OpenCV_LIBS} 
57 | )
58 | 
59 | target_link_libraries(TestCalibResult 
60 | OpenNIEngine
61 | ${OpenCV_LIBS} 
62 | ${OpenNI_LIBRARY})
63 | 
64 | 


--------------------------------------------------------------------------------
/CalibTest.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #include<stdio.h>
  3 | #include<opencv2/opencv.hpp>
  4 | #include<fstream>
  5 | #include"OpenNIEngine.h"
  6 | 
  7 | using namespace std;
  8 | using namespace cv;
  9 | 
 10 | void 
 11 | mapDepthToColor(
 12 |         const Mat& rgb, 
 13 |         const Mat& raw_depth, 
 14 |         Matx44f M_d2rgb, 
 15 |         Matx33f K_depth,
 16 |         Matx33f K_rgb,
 17 |         Mat& out_rgb)
 18 | {
 19 |     cvtColor(rgb,out_rgb,CV_BGR2RGB);
 20 |     float max_depth=4000;
 21 |     
 22 | //    for (int i=0;i<rgb.rows;i++)
 23 | //        for (int j=0;j<rgb.cols;j++)
 24 | //        {
 25 | //            max_depth = max(max_depth,(float)raw_depth.at<unsigned short>(i,j));
 26 | //        }
 27 |     
 28 |     for (int i=0;i<rgb.rows;i++)
 29 |         for (int j=0;j<rgb.cols;j++)
 30 |         {
 31 |             unsigned short d = raw_depth.at<unsigned short>(i,j);
 32 |             
 33 |             if (d>=4000||d==0){
 34 |                 continue;
 35 |             }
 36 |             else
 37 |             {
 38 |                 unsigned char dColor =(1-(float)d/max_depth)*255;
 39 |                 float fd = (float)d/1000;
 40 |                 Vec3f pt_w = K_depth.inv()*Vec3f(j*fd, i*fd, fd);
 41 |                 Vec4f pt_cw = M_d2rgb * Vec4f(pt_w[0],pt_w[1],pt_w[2], 1);
 42 |                 Vec3f pt_color = K_rgb * Vec3f(pt_cw[0],pt_cw[1],pt_cw[2]);
 43 |                 
 44 |                 int x_w = pt_color[0] / pt_color[2];
 45 |                 int y_w = pt_color[1] / pt_color[2];
 46 |                 
 47 |                 if(x_w>=0 && x_w < rgb.cols && y_w>=0 && y_w < rgb.rows)
 48 |                 {
 49 |                     out_rgb.at<Vec3b>(y_w,x_w)[3] = dColor;   
 50 |                     out_rgb.at<Vec3b>(y_w,x_w)[2] = dColor;   
 51 |                 }
 52 |             }
 53 |         }
 54 | }
 55 | 
 56 | int main(int argc, char** argv){
 57 | 
 58 |     Mat R_d2rgb,T_d2rgb;
 59 |     Matx44d M_d2rgb = Matx44d::eye();
 60 |     Matx44d M_rgb2d = Matx44d::eye();
 61 |     Mat K_depth, K_rgb;
 62 |     
 63 |     FileStorage fs("intrinsics.yml",FileStorage::READ);
 64 |     fs["M1"]>>K_depth;
 65 |     fs["M2"]>>K_rgb;
 66 |     fs.release();
 67 |     
 68 |     fs.open("extrinsics.yml", FileStorage::READ);
 69 |     fs["R"]>>R_d2rgb;
 70 |     fs["T"]>>T_d2rgb;
 71 |     fs.release();
 72 |     
 73 |     Matx33d Kd(K_depth);
 74 |     Matx33d Kc(K_rgb);
 75 |     
 76 |     cout << Kd << endl;
 77 |     cout << Kc << endl;
 78 |     
 79 |     cout << R_d2rgb << endl<<endl;
 80 |     cout << T_d2rgb.t() << endl<<endl;
 81 | 
 82 |     for (int r=0;r<3;r++) {
 83 |         for (int c=0;c<3;c++){
 84 |             M_d2rgb(r,c) = R_d2rgb.at<double>(r,c);
 85 |         }
 86 |         M_d2rgb(r,3) = T_d2rgb.at<double>(r,0);
 87 |     }
 88 |     
 89 |     cout<<M_d2rgb<<endl;
 90 |     M_rgb2d = M_d2rgb.inv();    
 91 |     
 92 |     Mat rgb(480,640,CV_8UC3);
 93 |     Mat depth(480,640,CV_16UC1);
 94 |     Mat depth_show(480,640,CV_8UC1);
 95 |     Mat rgb_aligned(480,640,CV_8UC3);
 96 |     Size size(640,480);
 97 |     
 98 |     OpenNIEngine* openni_engine = new OpenNIEngine();
 99 | 
100 | 
101 | 
102 |     while(true)
103 |     {
104 |         openni_engine->getRGBDImages(rgb,depth);       
105 |         mapDepthToColor(rgb,depth,M_d2rgb,Kd,Kc,rgb_aligned);
106 |         imshow("aligned",rgb_aligned);
107 |         
108 | //        depth.convertTo(depth_show,CV_8UC1);
109 | //        imshow("depth",depth_show);
110 |         
111 |         char key = waitKey(1);        
112 |         if(key=='q') break;
113 |     }
114 |     
115 |     ofstream ofs("Calib_ITM.txt");
116 |     ofs<<640<<" "<<480<<endl;
117 |     ofs<<Kc(0,0)<<" "<<Kc(1,1)<<endl;
118 |     ofs<<Kc(0,2)<<" "<<Kc(1,2)<<endl;
119 | 
120 |     ofs<<endl;
121 |     
122 |     ofs<<640<<" "<<480<<endl;
123 |     ofs<<Kd(0,0)<<" "<<Kd(1,1)<<endl;
124 |     ofs<<Kd(0,2)<<" "<<Kd(1,2)<<endl;
125 |     
126 |     ofs<<endl;
127 |     
128 |     for (int r=0;r<3;r++) {
129 |         for (int c=0;c<4;c++){
130 |             ofs<<M_rgb2d(r,c)<<" ";
131 |         }
132 |         ofs<<endl;
133 |     }
134 |     
135 |     ofs<<endl;
136 | 
137 |     ofs<<0<<" "<<0;
138 |     
139 |     ofs.close();
140 |     
141 |     imwrite("testdepth.pgm",depth);
142 |     
143 |     delete openni_engine;
144 | 
145 |     return 0;
146 | }
147 | 


--------------------------------------------------------------------------------
/OpenNICapture.cpp:
--------------------------------------------------------------------------------
 1 | #include<stdio.h>
 2 | #include<opencv2/opencv.hpp>
 3 | #include<fstream>
 4 | #include"OpenNIEngine.h"
 5 | 
 6 | using namespace std;
 7 | using namespace cv;
 8 | 
 9 | int main(int argc, char** argv){
10 | 
11 |     Mat rgb(480,640,CV_8UC3);
12 |     Mat depth(480,640,CV_16UC1);
13 |     Mat IR,gray;
14 |     
15 |     Size size(640,480);
16 |     
17 |     OpenNIEngine* openni_engine = new OpenNIEngine();
18 |     
19 |     ofstream ofs("image_lists.txt");
20 |     
21 |     int count = 0;
22 |     char out_name[200];
23 |     
24 |     while(true)
25 |     {
26 |         openni_engine->getRGBDImages(rgb,depth);
27 |         imshow("rgb",rgb);
28 |         
29 |         char key = waitKey(1);
30 |         
31 |         if(key == 's'){
32 |             openni_engine->shotGrayAndIRImages(gray,IR);
33 |             imshow("IR",IR);
34 |             imshow("gray",gray);
35 |             
36 |             sprintf(out_name,"%04d_left.jpg",count);
37 |             ofs<<out_name<<endl;
38 |             cout<<out_name<<"\t";
39 |            imwrite(out_name,IR);
40 |            
41 |             sprintf(out_name,"%04d_rignt.jpg",count);
42 |             ofs<<out_name<<endl;
43 |            cout<<out_name<<endl;
44 |            imwrite(out_name,gray);
45 | 
46 |             
47 |             count++;
48 |         }
49 |         
50 |         if(key=='q') break;
51 |     }
52 |     ofs.close();
53 |     
54 |     delete openni_engine;
55 | 
56 |     return 0;
57 | }
58 | 


--------------------------------------------------------------------------------
/OpenNIEngine.cpp:
--------------------------------------------------------------------------------
  1 | #include "OpenNIEngine.h"
  2 | #include <cstdio>
  3 | #include <stdexcept>
  4 | 
  5 | using namespace cv;
  6 | using namespace std;
  7 | 
  8 | static openni::VideoMode 
  9 | findBestMode(
 10 |         const openni::SensorInfo *sensorInfo, 
 11 |         int requiredResolutionX = -1, 
 12 |         int requiredResolutionY = -1, 
 13 |         openni::PixelFormat requiredPixelFormat = (openni::PixelFormat)-1)
 14 | {
 15 | 	const openni::Array<openni::VideoMode> & modes = sensorInfo->getSupportedVideoModes();
 16 | 	openni::VideoMode bestMode = modes[0];
 17 | 	for (int m = 0; m < modes.getSize(); ++m) {
 18 | 		//fprintf(stderr, "mode %i: %ix%i, %i %i\n", m, modes[m].getResolutionX(), modes[m].getResolutionY(), modes[m].getFps(), modes[m].getPixelFormat());
 19 | 		const openni::VideoMode & curMode = modes[m];
 20 | 		if ((requiredPixelFormat != (openni::PixelFormat)-1)&&(curMode.getPixelFormat() != requiredPixelFormat)) continue;
 21 | 
 22 | 		bool acceptAsBest = false;
 23 | 		if ((curMode.getResolutionX() == bestMode.getResolutionX())&&
 24 | 		     (curMode.getFps() > bestMode.getFps())) {
 25 | 			acceptAsBest = true;
 26 | 		} else if ((requiredResolutionX <= 0)&&(requiredResolutionY <= 0)) {
 27 | 			if (curMode.getResolutionX() > bestMode.getResolutionX()) {
 28 | 				acceptAsBest = true;
 29 | 			}
 30 | 		} else {
 31 | 			int diffX_cur = abs(curMode.getResolutionX()-requiredResolutionX);
 32 | 			int diffX_best = abs(bestMode.getResolutionX()-requiredResolutionX);
 33 | 			int diffY_cur = abs(curMode.getResolutionY()-requiredResolutionY);
 34 | 			int diffY_best = abs(bestMode.getResolutionY()-requiredResolutionY);
 35 | 			if (requiredResolutionX > 0) {
 36 | 				if (diffX_cur < diffX_best) {
 37 | 					acceptAsBest = true;
 38 | 				}
 39 | 				if ((requiredResolutionY > 0)&&(diffX_cur == diffX_best)&&(diffY_cur < diffY_best)) {
 40 | 					acceptAsBest = true;
 41 | 				}
 42 | 			} else if (requiredResolutionY > 0) {
 43 | 				if (diffY_cur < diffY_best) {
 44 | 					acceptAsBest = true;
 45 | 				}
 46 | 			}
 47 | 		}
 48 | 		if (acceptAsBest) bestMode = curMode;
 49 | 	}
 50 | 	//fprintf(stderr, "=> best mode: %ix%i, %i %i\n", bestMode.getResolutionX(), bestMode.getResolutionY(), bestMode.getFps(), bestMode.getPixelFormat());
 51 | 	return bestMode;
 52 | }
 53 | 
 54 | 
 55 | OpenNIEngine::OpenNIEngine(const char *device_URI, 
 56 |         const bool use_internal_calibration, 
 57 |         Size requested_size_rgb, 
 58 |         Size requested_size_d)
 59 | {
 60 | 	if (device_URI==NULL) device_URI = openni::ANY_DEVICE;
 61 | 	openni::Status rc = openni::STATUS_OK;
 62 | 
 63 | 	rc = openni::OpenNI::initialize();
 64 | 	printf("OpenNI: Initialization ... \n%s\n", openni::OpenNI::getExtendedError());
 65 | 
 66 |     rc = openni_device_data.device.open(device_URI);
 67 | 	if (rc != openni::STATUS_OK)
 68 | 	{
 69 | 		std::string message("OpenNI: Device open failed!\n");
 70 | 		message += openni::OpenNI::getExtendedError();
 71 | 		openni::OpenNI::shutdown();
 72 | 		std::cout << message;
 73 | 		return;
 74 | 	}
 75 |     
 76 |     openni::PlaybackControl *control = openni_device_data.device.getPlaybackControl();
 77 | 	if (control != NULL) {
 78 | 		// this is a file! make sure we get every frame
 79 | 		control->setSpeed(-1.0f);
 80 | 		control->setRepeatEnabled(false);
 81 | 	}
 82 | 
 83 |     initRGBDStreams(use_internal_calibration, requested_size_d,requested_size_d);
 84 |     
 85 |     tmp_ir_image.create(image_size_depth,CV_16UC1);
 86 |     tmp_RGB_image.create(image_size_rgb,CV_8UC3);
 87 | }
 88 | 
 89 | 
 90 | void OpenNIEngine::initRGBDStreams(const bool use_internal_calibration, Size requested_size_rgb, Size requested_size_d)
 91 | {
 92 |     // create depth stream
 93 |     openni::Status rc = openni_device_data.depthStream.create(openni_device_data.device,openni::SENSOR_DEPTH);
 94 | 	if (rc == openni::STATUS_OK)
 95 | 	{
 96 | 		openni::VideoMode depthMode = findBestMode( openni_device_data.device.getSensorInfo(openni::SENSOR_DEPTH), requested_size_d.width, requested_size_d.height, openni::PIXEL_FORMAT_DEPTH_1_MM);
 97 | 		image_size_depth.width = depthMode.getResolutionX();
 98 | 		image_size_depth.height = depthMode.getResolutionY();
 99 | 		rc = openni_device_data.depthStream.setVideoMode(depthMode);
100 | 		if (rc != openni::STATUS_OK)
101 | 		{
102 | 			printf("OpenNI: Failed to set depth mode\n");
103 | 		}
104 |         openni_device_data.depthStream.setMirroringEnabled(false);
105 |         rc = openni_device_data.depthStream.start();
106 | 
107 | 
108 | 		if (use_internal_calibration) 
109 |             openni_device_data.device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
110 |         
111 | 		if (rc != openni::STATUS_OK)
112 | 		{
113 | 			printf("OpenNI: Couldn't start depthStream stream:\n%s\n", openni::OpenNI::getExtendedError());
114 |             openni_device_data.depthStream.destroy();
115 | 		}
116 | 		printf("Initialised OpenNI depth camera with resolution: %d x %d\n", image_size_depth.width,image_size_depth.height);
117 | 
118 | 		depth_available = true;
119 | 	}
120 | 	else
121 | 	{
122 | 		printf("OpenNI: Couldn't find depthStream stream:\n%s\n", openni::OpenNI::getExtendedError());
123 | 		depth_available = false;
124 | 	}
125 | 
126 |     
127 |     // create color stream
128 | 	rc = openni_device_data.colorStream.create(openni_device_data.device, openni::SENSOR_COLOR);
129 | 	if (rc == openni::STATUS_OK)
130 | 	{
131 | 		openni::VideoMode colourMode = findBestMode(openni_device_data.device.getSensorInfo(openni::SENSOR_COLOR), requested_size_rgb.width, requested_size_rgb.height);
132 | 		image_size_rgb.width = colourMode.getResolutionX();
133 | 		image_size_rgb.height = colourMode.getResolutionY();
134 | 		rc = openni_device_data.colorStream.setVideoMode(colourMode);
135 | 		if (rc != openni::STATUS_OK)
136 | 		{
137 | 			printf("OpenNI: Failed to set color mode\n");
138 | 		}
139 | 		openni_device_data.colorStream.setMirroringEnabled(false);
140 | 
141 | 		rc = openni_device_data.colorStream.start();
142 | 		if (rc != openni::STATUS_OK)
143 | 		{
144 | 			printf("OpenNI: Couldn't start colorStream stream:\n%s\n", openni::OpenNI::getExtendedError());
145 | 			openni_device_data.colorStream.destroy();
146 | 		}
147 | 
148 | 		printf("Initialised OpenNI color camera with resolution: %d x %d\n", image_size_rgb.width, image_size_rgb.height);
149 | 
150 | 		color_available = true;
151 | 	}
152 | 	else
153 | 	{
154 | 		printf("OpenNI: Couldn't find colorStream stream:\n%s\n", openni::OpenNI::getExtendedError());
155 | 		color_available = false;
156 | 	}
157 | 
158 | 	if (!depth_available)
159 | 	{
160 | 		openni::OpenNI::shutdown();
161 | 		std::cout << "OpenNI: No valid streams. Exiting." << std::endl;
162 | 		return;
163 | 	}
164 | 	
165 | 	openni_device_data.streams = new openni::VideoStream*[3];
166 | 	if (depth_available) openni_device_data.streams[0] = &openni_device_data.depthStream;
167 | 	if (color_available) openni_device_data.streams[1] = &openni_device_data.colorStream;
168 |      
169 |     rc  = openni_device_data.IRStream.create(openni_device_data.device,openni::SENSOR_IR);
170 |     openni::VideoMode IRMode = findBestMode(openni_device_data.device.getSensorInfo(openni::SENSOR_IR), requested_size_rgb.width, requested_size_rgb.height);
171 |     rc = openni_device_data.IRStream.setVideoMode(IRMode);
172 |     openni_device_data.IRStream.setMirroringEnabled(false);
173 |     
174 |     // rc = openni_device_data.IRStream.start();
175 |     
176 |     if(rc!=openni::STATUS_OK)
177 |     {
178 |         cout<<"can not create IR stream!"<<endl;
179 |     }
180 |    
181 |     
182 |     std::cout<<flush;
183 | }
184 | 
185 | OpenNIEngine::~OpenNIEngine()
186 | {
187 | 
188 | 		if (depth_available)
189 | 		{
190 | 			openni_device_data.depthStream.stop();
191 | 			openni_device_data.depthStream.destroy();
192 | 		}
193 | 		if (color_available)
194 | 		{
195 | 			openni_device_data.colorStream.stop();
196 | 			openni_device_data.colorStream.destroy();
197 | 		}
198 | 		openni_device_data.device.close();
199 | 
200 | 		delete[] openni_device_data.streams;
201 | 
202 | 	openni::OpenNI::shutdown();
203 | }
204 | 
205 | 
206 | void OpenNIEngine::getRGBDImages(cv::Mat& rgb, cv::Mat& raw_depth)
207 | {
208 | 	int changedIndex, waitStreamCount;
209 | 	if (depth_available && color_available) waitStreamCount = 2;
210 | 	else waitStreamCount = 1;
211 | 
212 | 	openni::Status rc = openni::OpenNI::waitForAnyStream(openni_device_data.streams, waitStreamCount, &changedIndex);
213 | 	if (rc != openni::STATUS_OK) { printf("OpenNI: Wait failed\n"); return /*false*/; }
214 | 
215 | 	if(depth_available) openni_device_data.depthStream.readFrame(&openni_device_data.depthFrame);
216 | 	if(color_available) openni_device_data.colorStream.readFrame(&openni_device_data.colorFrame);
217 | 
218 | 	if (depth_available && !openni_device_data.depthFrame.isValid()) return;
219 | 	if (color_available && !openni_device_data.colorFrame.isValid()) return;
220 | 
221 | 	if (color_available)
222 | 	{
223 | 		const openni::RGB888Pixel* colorImagePix = (const openni::RGB888Pixel*)openni_device_data.colorFrame.getData();
224 |         memcpy(rgb.data,colorImagePix,rgb.cols*rgb.rows*3*sizeof(unsigned char));        
225 | 	}
226 | 	else memset(rgb.data, 0, rgb.cols*rgb.rows*3*sizeof(unsigned char));
227 | 
228 | 	if (depth_available)
229 | 	{
230 | 		const openni::DepthPixel* depthImagePix = (const openni::DepthPixel*)openni_device_data.depthFrame.getData();
231 | 		memcpy(raw_depth.data, depthImagePix, raw_depth.cols*raw_depth.rows * sizeof(short));
232 | 	}
233 | 	else memset(raw_depth.data, 0, raw_depth.cols*raw_depth.rows * sizeof(short));
234 | 
235 | 	return /*true*/;
236 | }
237 | 
238 | 
239 | 
240 | void OpenNIEngine::shotGrayAndIRImages(Mat &gray, Mat &ir){
241 |     
242 |     openni_device_data.colorStream.readFrame(&openni_device_data.colorFrame);
243 |     if (openni_device_data.colorFrame.isValid())
244 |     {
245 |         const openni::RGB888Pixel* colorImagePix = (const openni::RGB888Pixel*)openni_device_data.colorFrame.getData();
246 |         memcpy(tmp_RGB_image.data,colorImagePix,tmp_RGB_image.cols*tmp_RGB_image.rows*3*sizeof(unsigned char));     
247 |         cvtColor(tmp_RGB_image,gray,CV_RGB2GRAY);
248 |     }
249 |     else
250 |     {
251 |         cerr<<"rgb frame wrong!"<<endl;
252 |     }
253 |     
254 |     openni_device_data.depthStream.stop();
255 |     openni_device_data.colorStream.stop();
256 |     openni_device_data.IRStream.start();
257 |     
258 |     openni_device_data.IRStream.readFrame(&openni_device_data.IRFrame);
259 |     if (openni_device_data.IRFrame.isValid())
260 |     {
261 |         const openni::Grayscale16Pixel* irImagePix = (const openni::Grayscale16Pixel*)openni_device_data.IRFrame.getData();
262 |         memcpy(tmp_ir_image.data, irImagePix, tmp_ir_image.cols*tmp_ir_image.rows * sizeof(openni::Grayscale16Pixel));
263 |         tmp_ir_image.convertTo(ir,CV_8UC1);
264 |     }
265 |     else
266 |     {
267 |         cerr<<"IR frame wrong!"<<endl;
268 |     }
269 |     
270 |     openni_device_data.IRStream.stop();
271 |     openni_device_data.depthStream.start();
272 |     openni_device_data.colorStream.start();
273 | 
274 | }
275 | 
276 | cv::Size OpenNIEngine::getDepthImageSize(void) {return image_size_depth;}
277 | cv::Size OpenNIEngine::getRGBImageSize(void){return image_size_rgb;}
278 | 
279 | 


--------------------------------------------------------------------------------
/OpenNIEngine.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | #include<opencv2/opencv.hpp>
 3 | #include <OpenNI.h>
 4 | 
 5 | class OpenNIEngine
 6 | {
 7 |     struct DeviceData{
 8 |         openni::Device device;
 9 |         
10 |         openni::VideoStream depthStream;
11 |         openni::VideoStream colorStream;
12 |         openni::VideoStream IRStream;
13 |         
14 |         openni::VideoFrameRef depthFrame;
15 |         openni::VideoFrameRef colorFrame;
16 |         openni::VideoFrameRef IRFrame;
17 |         
18 |         openni::VideoStream **streams;
19 |     };
20 |     
21 | private:
22 |     
23 |     DeviceData openni_device_data;
24 |     
25 |     cv::Size image_size_rgb;
26 |     cv::Size image_size_depth;    
27 |     
28 |     bool color_available;
29 |     bool depth_available;
30 |     bool ir_available;    
31 |     
32 |     cv::Mat tmp_ir_image;
33 |     cv::Mat tmp_RGB_image;
34 |     
35 |     void initRGBDStreams( const bool use_internal_calibration, 
36 |                           cv::Size requested_size_rgb, 
37 |                           cv::Size requested_size_d);
38 |     
39 | public:
40 |     OpenNIEngine(
41 |             const char *device_URI = NULL, 
42 |             const bool use_internal_calibration = false,
43 |             cv::Size requested_size_rgb = cv::Size(640,480), 
44 |             cv::Size requested_size_d = cv::Size(640,480));
45 |     
46 |     ~OpenNIEngine();
47 | 
48 |     void shotGrayAndIRImages(cv::Mat& gray, cv::Mat& ir);
49 |     void getRGBDImages(cv::Mat& rgb, cv::Mat& raw_depth);
50 |     
51 |     cv::Size getDepthImageSize(void);
52 |     cv::Size getRGBImageSize(void);
53 | };
54 | 
55 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Calibration Tool for OpenNI based RGB-D sensors
 2 | 
 3 | ## Build:
 4 | 
 5 | ```
 6 | mkdir build
 7 | cd build
 8 | cmake .. -DOPEN_NI_ROOT=/your-path-to-OpenNI2/ 
 9 | make -j$(nproc)
10 | ```
11 | 
12 | ## Dependency
13 | 
14 | - OpenCV3.0 <http://opencv.org/> -- REQUIRED  
15 | - OpenNI2 <http://structure.io/openni>  -- REQUIRED  
16 | 
17 | ## How to Calibrate a OpenNI sensor
18 | 
19 | - Print the pattern in chess.pdf and stick the two page together side by side, making a 7x5 pattern.  
20 |   Don't scale the print, since each rectango should be 38mmx38mm.
21 | - Plug in your OpenNI sensor, run `./CalibCapture` program. When you see the color frame window, use a small piece of glue tape to cover the infrad projector of your sensor, so that you will have less noise on the IR image. Point the sensor to the printed pattern, leave the sensor still, then press `s` on the keyboard to save a capture. Captured frame will be shown on separte windows. 
22 | - **DO NOT MOVE THE SENSOR WHEN YOU CAPTURE THE FRAMES!** (this is because OpenNI2 don't give you access to the color and the IR stream of the sensor at the same time, the program is switching the color stream and IR stream on and off, and this takes time.)
23 | - Take around 40 captures from different viewport. **press `q` to exit**
24 | - Run `./StereoCalib`, the program will autometically calibrate the camera and save calibration results to  
25 |   `intrinsics.yml` and `extrinsics.yml`
26 | 
27 | ## Verify result and generate [InfiniTAM](https://github.com/victorprad/InfiniTAM) readable calib file
28 | 
29 | Run `./TestCalibResult` to visualize a overlay between the color and the depth frame using the calibration result. press `q` to quite the program and a `Calib_ITM.txt` file will be written to the `build/` folder. This file can be directly read from [InfiniTAM](https://github.com/victorprad/InfiniTAM) 
30 |   
31 | 


--------------------------------------------------------------------------------
/StereoCalib.cpp:
--------------------------------------------------------------------------------
  1 | /* This is sample from the OpenCV book. The copyright notice is below */
  2 | 
  3 | /* *************** License:**************************
  4 |    Oct. 3, 2008
  5 |    Right to use this code in any way you want without warranty, support or any guarantee of it working.
  6 | 
  7 |    BOOK: It would be nice if you cited it:
  8 |    Learning OpenCV: Computer Vision with the OpenCV Library
  9 |      by Gary Bradski and Adrian Kaehler
 10 |      Published by O'Reilly Media, October 3, 2008
 11 | 
 12 |    AVAILABLE AT:
 13 |      http://www.amazon.com/Learning-OpenCV-Computer-Vision-Library/dp/0596516134
 14 |      Or: http://oreilly.com/catalog/9780596516130/
 15 |      ISBN-10: 0596516134 or: ISBN-13: 978-0596516130
 16 | 
 17 |    OPENCV WEBSITES:
 18 |      Homepage:      http://opencv.org
 19 |      Online docs:   http://docs.opencv.org
 20 |      Q&A forum:     http://answers.opencv.org
 21 |      Issue tracker: http://code.opencv.org
 22 |      GitHub:        https://github.com/Itseez/opencv/
 23 |    ************************************************** */
 24 | 
 25 | #include "opencv2/calib3d/calib3d.hpp"
 26 | #include "opencv2/imgcodecs.hpp"
 27 | #include "opencv2/highgui/highgui.hpp"
 28 | #include "opencv2/imgproc/imgproc.hpp"
 29 | 
 30 | #include <vector>
 31 | #include <string>
 32 | #include <algorithm>
 33 | #include <iostream>
 34 | #include <iterator>
 35 | #include <stdio.h>
 36 | #include <stdlib.h>
 37 | #include <ctype.h>
 38 | #include <fstream>
 39 | 
 40 | using namespace cv;
 41 | using namespace std;
 42 | 
 43 | static int print_help()
 44 | {
 45 |     cout <<
 46 |             " Given a list of chessboard images, the number of corners (nx, ny)\n"
 47 |             " on the chessboards, and a flag: useCalibrated for \n"
 48 |             "   calibrated (0) or\n"
 49 |             "   uncalibrated \n"
 50 |             "     (1: use cvStereoCalibrate(), 2: compute fundamental\n"
 51 |             "         matrix separately) stereo. \n"
 52 |             " Calibrate the cameras and display the\n"
 53 |             " rectified results along with the computed disparity images.   \n" << endl;
 54 |     cout << "Usage:\n ./stereo_calib -w board_width -h board_height [-nr /*dot not view results*/] <image list XML/YML file>\n" << endl;
 55 |     return 0;
 56 | }
 57 | 
 58 | 
 59 | static void
 60 | StereoCalib(const vector<string>& imagelist, Size boardSize, bool useCalibrated=true, bool showRectified=true)
 61 | {
 62 |     if( imagelist.size() % 2 != 0 )
 63 |     {
 64 |         cout << "Error: the image list contains odd (non-even) number of elements\n";
 65 |         return;
 66 |     }
 67 | 
 68 |     bool displayCorners = true;//true;
 69 |     const int maxScale = 2;
 70 |     const float squareSize = 0.038f;  // Set this to your actual square size
 71 |     // ARRAY AND VECTOR STORAGE:
 72 | 
 73 |     vector<vector<Point2f> > imagePoints[2];
 74 |     vector<vector<Point3f> > objectPoints;
 75 |     Size imageSize;
 76 | 
 77 |     int i, j, k, nimages = (int)imagelist.size()/2;
 78 | 
 79 |     imagePoints[0].resize(nimages);
 80 |     imagePoints[1].resize(nimages);
 81 |     vector<string> goodImageList;
 82 | 
 83 |     for( i = j = 0; i < nimages; i++ )
 84 |     {
 85 |         for( k = 0; k < 2; k++ )
 86 |         {
 87 |             const string& filename = imagelist[i*2+k];
 88 |             Mat img = imread(filename, 0);
 89 |             if(img.empty())
 90 |                 break;
 91 |             if( imageSize == Size() )
 92 |                 imageSize = img.size();
 93 |             else if( img.size() != imageSize )
 94 |             {
 95 |                 cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n";
 96 |                 break;
 97 |             }
 98 |             bool found = false;
 99 |             vector<Point2f>& corners = imagePoints[k][j];
100 |             for( int scale = 1; scale <= maxScale; scale++ )
101 |             {
102 |                 Mat timg;
103 |                 if( scale == 1 )
104 |                     timg = img;
105 |                 else
106 |                     resize(img, timg, Size(), scale, scale);
107 |                 found = findChessboardCorners(timg, boardSize, corners,
108 |                     CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);
109 |                 if( found )
110 |                 {
111 |                     if( scale > 1 )
112 |                     {
113 |                         Mat cornersMat(corners);
114 |                         cornersMat *= 1./scale;
115 |                     }
116 |                     break;
117 |                 }
118 |             }
119 |             if( displayCorners )
120 |             {
121 |                 cout << filename << endl;
122 |                 Mat cimg, cimg1;
123 |                 cvtColor(img, cimg, COLOR_GRAY2BGR);
124 |                 drawChessboardCorners(cimg, boardSize, corners, found);
125 |                 double sf = 640./MAX(img.rows, img.cols);
126 |                 resize(cimg, cimg1, Size(), sf, sf);
127 |                 imshow("corners", cimg1);
128 |                 char c = (char)waitKey(500);
129 |                 if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit
130 |                     exit(-1);
131 |             }
132 |             else
133 |                 putchar('.');
134 |             if( !found )
135 |                 break;
136 |             cornerSubPix(img, corners, Size(11,11), Size(-1,-1),
137 |                          TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,
138 |                                       30, 0.01));
139 |         }
140 |         if( k == 2 )
141 |         {
142 |             goodImageList.push_back(imagelist[i*2]);
143 |             goodImageList.push_back(imagelist[i*2+1]);
144 |             j++;
145 |         }
146 |     }
147 |     cout << j << " pairs have been successfully detected.\n";
148 |     nimages = j;
149 |     if( nimages < 2 )
150 |     {
151 |         cout << "Error: too little pairs to run the calibration\n";
152 |         return;
153 |     }
154 | 
155 |     imagePoints[0].resize(nimages);
156 |     imagePoints[1].resize(nimages);
157 |     objectPoints.resize(nimages);
158 | 
159 |     cout<<"board size:"<<boardSize<<endl;
160 |     cout << "square size:"<<squareSize<<endl;
161 |     
162 |     for( i = 0; i < nimages; i++ )
163 |     {
164 |         for( j = 0; j < boardSize.height; j++ )
165 |             for( k = 0; k < boardSize.width; k++ )
166 |                 objectPoints[i].push_back(Point3f(k*squareSize, j*squareSize, 0));
167 |     }
168 | 
169 |     cout << "Running stereo calibration ..."<<endl;
170 | 
171 |     Mat cameraMatrix[2], distCoeffs[2];
172 |     cameraMatrix[0] = Mat::eye(3, 3, CV_64F);
173 |     cameraMatrix[1] = Mat::eye(3, 3, CV_64F);
174 |     Mat R, T, E, F;
175 | 
176 |     double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
177 |                     cameraMatrix[0], distCoeffs[0],
178 |                     cameraMatrix[1], distCoeffs[1],
179 |                     imageSize, R, T, E, F,
180 |                     CALIB_ZERO_TANGENT_DIST +
181 |                     CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5,
182 |                     TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-5) );
183 |     cout << "done with RMS error=" << rms << endl;
184 | 
185 | // CALIBRATION QUALITY CHECK
186 | // because the output fundamental matrix implicitly
187 | // includes all the output information,
188 | // we can check the quality of calibration using the
189 | // epipolar geometry constraint: m2^t*F*m1=0
190 |     double err = 0;
191 |     int npoints = 0;
192 |     vector<Vec3f> lines[2];
193 |     for( i = 0; i < nimages; i++ )
194 |     {
195 |         int npt = (int)imagePoints[0][i].size();
196 |         Mat imgpt[2];
197 |         for( k = 0; k < 2; k++ )
198 |         {
199 |             imgpt[k] = Mat(imagePoints[k][i]);
200 |             undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]);
201 |             computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
202 |         }
203 |         for( j = 0; j < npt; j++ )
204 |         {
205 |             double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] +
206 |                                 imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
207 |                            fabs(imagePoints[1][i][j].x*lines[0][j][0] +
208 |                                 imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
209 |             err += errij;
210 |         }
211 |         npoints += npt;
212 |     }
213 |     cout << "average reprojection err = " <<  err/npoints << endl;
214 | 
215 |     // save intrinsic parameters
216 |     FileStorage fs("intrinsics.yml", FileStorage::WRITE);
217 |     if( fs.isOpened() )
218 |     {
219 |         fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] <<
220 |             "M2" << cameraMatrix[1] << "D2" << distCoeffs[1];
221 |         fs.release();
222 |     }
223 |     else
224 |         cout << "Error: can not save the intrinsic parameters\n";
225 | 
226 |     Mat R1, R2, P1, P2, Q;
227 |     Rect validRoi[2];
228 | 
229 |     stereoRectify(cameraMatrix[0], distCoeffs[0],
230 |                   cameraMatrix[1], distCoeffs[1],
231 |                   imageSize, R, T, R1, R2, P1, P2, Q,
232 |                   CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]);
233 | 
234 |     fs.open("extrinsics.yml", FileStorage::WRITE);
235 |     if( fs.isOpened() )
236 |     {
237 |         fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q;
238 |         fs.release();
239 |     }
240 |     else
241 |         cout << "Error: can not save the extrinsic parameters\n";
242 | 
243 |     // OpenCV can handle left-right
244 |     // or up-down camera arrangements
245 |     bool isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3));
246 | 
247 | // COMPUTE AND DISPLAY RECTIFICATION
248 |     if( !showRectified )
249 |         return;
250 | 
251 |     Mat rmap[2][2];
252 | // IF BY CALIBRATED (BOUGUET'S METHOD)
253 |     if( useCalibrated )
254 |     {
255 |         // we already computed everything
256 |     }
257 | // OR ELSE HARTLEY'S METHOD
258 |     else
259 |  // use intrinsic parameters of each camera, but
260 |  // compute the rectification transformation directly
261 |  // from the fundamental matrix
262 |     {
263 |         vector<Point2f> allimgpt[2];
264 |         for( k = 0; k < 2; k++ )
265 |         {
266 |             for( i = 0; i < nimages; i++ )
267 |                 std::copy(imagePoints[k][i].begin(), imagePoints[k][i].end(), back_inserter(allimgpt[k]));
268 |         }
269 |         F = findFundamentalMat(Mat(allimgpt[0]), Mat(allimgpt[1]), FM_8POINT, 0, 0);
270 |         Mat H1, H2;
271 |         stereoRectifyUncalibrated(Mat(allimgpt[0]), Mat(allimgpt[1]), F, imageSize, H1, H2, 3);
272 | 
273 |         R1 = cameraMatrix[0].inv()*H1*cameraMatrix[0];
274 |         R2 = cameraMatrix[1].inv()*H2*cameraMatrix[1];
275 |         P1 = cameraMatrix[0];
276 |         P2 = cameraMatrix[1];
277 |     }
278 | 
279 |     //Precompute maps for cv::remap()
280 |     initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
281 |     initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);
282 | 
283 |     Mat canvas;
284 |     double sf;
285 |     int w, h;
286 |     if( !isVerticalStereo )
287 |     {
288 |         sf = 600./MAX(imageSize.width, imageSize.height);
289 |         w = cvRound(imageSize.width*sf);
290 |         h = cvRound(imageSize.height*sf);
291 |         canvas.create(h, w*2, CV_8UC3);
292 |     }
293 |     else
294 |     {
295 |         sf = 300./MAX(imageSize.width, imageSize.height);
296 |         w = cvRound(imageSize.width*sf);
297 |         h = cvRound(imageSize.height*sf);
298 |         canvas.create(h*2, w, CV_8UC3);
299 |     }
300 | 
301 |     for( i = 0; i < nimages; i++ )
302 |     {
303 |         for( k = 0; k < 2; k++ )
304 |         {
305 |             Mat img = imread(goodImageList[i*2+k], 0), rimg, cimg;
306 |             remap(img, rimg, rmap[k][0], rmap[k][1], INTER_LINEAR);
307 |             cvtColor(rimg, cimg, COLOR_GRAY2BGR);
308 |             Mat canvasPart = !isVerticalStereo ? canvas(Rect(w*k, 0, w, h)) : canvas(Rect(0, h*k, w, h));
309 |             resize(cimg, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);
310 |             if( useCalibrated )
311 |             {
312 |                 Rect vroi(cvRound(validRoi[k].x*sf), cvRound(validRoi[k].y*sf),
313 |                           cvRound(validRoi[k].width*sf), cvRound(validRoi[k].height*sf));
314 |                 rectangle(canvasPart, vroi, Scalar(0,0,255), 3, 8);
315 |             }
316 |         }
317 | 
318 |         if( !isVerticalStereo )
319 |             for( j = 0; j < canvas.rows; j += 16 )
320 |                 line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8);
321 |         else
322 |             for( j = 0; j < canvas.cols; j += 16 )
323 |                 line(canvas, Point(j, 0), Point(j, canvas.rows), Scalar(0, 255, 0), 1, 8);
324 |         imshow("rectified", canvas);
325 |         char c = (char)waitKey();
326 |         if( c == 27 || c == 'q' || c == 'Q' )
327 |             break;
328 |     }
329 | }
330 | 
331 | 
332 | static bool readStringList( const string& filename, vector<string>& l )
333 | {
334 |     ifstream ifs(filename.c_str());
335 |     string tmp_str;
336 |     while(ifs>>tmp_str)  l.push_back(tmp_str);
337 |     return true;
338 | }
339 | 
340 | int main(int argc, char** argv)
341 | {
342 |     Size boardSize;
343 |     string imagelistfn;
344 |     bool showRectified = true;
345 | 
346 |     for( int i = 1; i < argc; i++ )
347 |     {
348 |         if( string(argv[i]) == "-w" )
349 |         {
350 |             if( sscanf(argv[++i], "%d", &boardSize.width) != 1 || boardSize.width <= 0 )
351 |             {
352 |                 cout << "invalid board width" << endl;
353 |                 return print_help();
354 |             }
355 |         }
356 |         else if( string(argv[i]) == "-h" )
357 |         {
358 |             if( sscanf(argv[++i], "%d", &boardSize.height) != 1 || boardSize.height <= 0 )
359 |             {
360 |                 cout << "invalid board height" << endl;
361 |                 return print_help();
362 |             }
363 |         }
364 |         else if( string(argv[i]) == "-nr" )
365 |             showRectified = false;
366 |         else if( string(argv[i]) == "--help" )
367 |             return print_help();
368 |         else if( argv[i][0] == '-' )
369 |         {
370 |             cout << "invalid option " << argv[i] << endl;
371 |             return 0;
372 |         }
373 |         else
374 |             imagelistfn = argv[i];
375 |     }
376 | 
377 |     if( imagelistfn == "" )
378 |     {
379 |         imagelistfn = "image_lists.txt";
380 |         boardSize = Size(7, 5);
381 |     }
382 |     else if( boardSize.width <= 0 || boardSize.height <= 0 )
383 |     {
384 |         cout << "if you specified XML file with chessboards, you should also specify the board width and height (-w and -h options)" << endl;
385 |         return 0;
386 |     }
387 | 
388 |     vector<string> imagelist;
389 |     bool ok = readStringList(imagelistfn, imagelist);
390 |     if(!ok || imagelist.empty())
391 |     {
392 |         cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
393 |         return print_help();
394 |     }
395 | 
396 |     StereoCalib(imagelist, boardSize, true, showRectified);
397 |     return 0;
398 | }
399 | 


--------------------------------------------------------------------------------
/chess.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/carlren/OpenNICalibTool/1a40309667773fa5f443cd92d2f441903b6a675f/chess.pdf


--------------------------------------------------------------------------------
/cmake/FindOpenNI.cmake:
--------------------------------------------------------------------------------
 1 | # - Find OpenNI2
 2 | # This module defines
 3 | #  OpenNI_INCLUDE_DIR, where to find OpenNI include files
 4 | #  OpenNI_LIBRARIES, the libraries needed to use OpenNI
 5 | #  OpenNI_FOUND, If false, do not try to use OpenNI.
 6 | # also defined, but not for general use are
 7 | #  OpenNI_LIBRARY, where to find the OpenNI library.
 8 | 
 9 | message(STATUS ${OPEN_NI_ROOT})
10 | 
11 | find_path(OpenNI_INCLUDE_DIR OpenNI.h PATH "${OPEN_NI_ROOT}/Include")
12 | find_library(OpenNI_LIBRARY OpenNI2 PATH "${OPEN_NI_ROOT}/Bin/x64-Release/")
13 | 
14 | # handle the QUIETLY and REQUIRED arguments and set JPEG_FOUND to TRUE if
15 | # all listed variables are TRUE
16 | #include(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake)
17 | #include(${CMAKE_MODULE_PATH}/FindPackageHandleStandardArgs.cmake)
18 | find_package_handle_standard_args(OpenNI DEFAULT_MSG OpenNI_LIBRARY OpenNI_INCLUDE_DIR)
19 | 
20 | if(OPENNI_FOUND)
21 |   set(OpenNI_LIBRARIES ${OpenNI_LIBRARY})
22 | endif()
23 | 
24 | mark_as_advanced(OpenNI_LIBRARY OpenNI_INCLUDE_DIR)
25 | 
26 | 


--------------------------------------------------------------------------------