├── rgb-image.ppm
├── disparity-image.pgm
├── Q.xml
├── README.md
└── main.cpp


/rgb-image.ppm:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/bkornel/Reproject-Image-To-3D/HEAD/rgb-image.ppm


--------------------------------------------------------------------------------
/disparity-image.pgm:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/bkornel/Reproject-Image-To-3D/HEAD/disparity-image.pgm


--------------------------------------------------------------------------------
/Q.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <opencv_storage>
 3 | <Q type_id="opencv-matrix">
 4 |   <rows>4</rows>
 5 |   <cols>4</cols>
 6 |   <dt>d</dt>
 7 |   <data>
 8 |     1. 0. 0. -2.9615028381347656e+02 0. 1. 0. -2.3373317337036133e+02 0.
 9 |     0. 0. 5.6446880931501073e+02 0. 0. -1.1340974198400260e-01
10 |     4.1658568844268817e+00</data></Q>
11 | </opencv_storage>
12 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Reproject-Image-To-3D
 2 | Comparing a OpenCV's [`reprojectImageTo3D()`][1] to my own
 3 | 
 4 | 	// Reproject image to 3D
 5 | 	void customReproject(const cv::Mat& disparity, const cv::Mat& Q, cv::Mat& out3D)
 6 | 	{
 7 | 		CV_Assert(disparity.type() == CV_32F && !disparity.empty());
 8 | 		CV_Assert(Q.type() == CV_32F && Q.cols == 4 && Q.rows == 4);
 9 | 
10 | 		// 3-channel matrix for containing the reprojected 3D world coordinates
11 | 		out3D = cv::Mat::zeros(disparity.size(), CV_32FC3);
12 | 
13 | 		// Getting the interesting parameters from Q, everything else is zero or one
14 | 		float Q03 = Q.at<float>(0, 3);
15 | 		float Q13 = Q.at<float>(1, 3);
16 | 		float Q23 = Q.at<float>(2, 3);
17 | 		float Q32 = Q.at<float>(3, 2);
18 | 		float Q33 = Q.at<float>(3, 3);
19 | 
20 | 		// Transforming a single-channel disparity map to a 3-channel image representing a 3D surface
21 | 		for (int i = 0; i < disparity.rows; i++)
22 | 		{
23 | 			const float* disp_ptr = disparity.ptr<float>(i);
24 | 			cv::Vec3f* out3D_ptr = out3D.ptr<cv::Vec3f>(i);
25 | 
26 | 			for (int j = 0; j < disparity.cols; j++)
27 | 			{
28 | 				const float pw = 1.0f / (disp_ptr[j] * Q32 + Q33);
29 | 
30 | 				cv::Vec3f& point = out3D_ptr[j];
31 | 				point[0] = (static_cast<float>(j)+Q03) * pw;
32 | 				point[1] = (static_cast<float>(i)+Q13) * pw;
33 | 				point[2] = Q23 * pw;
34 | 			}
35 | 		}
36 | 	}
37 | 	
38 | 	
39 | Disparity map and Q matrix is from [Martin Peris' Blog][2].
40 | 
41 |   [1]: http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#reprojectimageto3d
42 |   [2]: http://blog.martinperis.com/2012/01/3d-reconstruction-with-opencv-and-point.html
43 |   
44 | 


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
  1 | #include <iostream>
  2 | #include <fstream>
  3 | #include <string>
  4 | 
  5 | #include <opencv2/core.hpp>
  6 | #include <opencv2/imgcodecs.hpp>
  7 | #include <opencv2/highgui.hpp>
  8 | #include <opencv2/calib3d.hpp>
  9 | 
 10 | void customReproject(const cv::Mat& disparity, const cv::Mat& Q, cv::Mat& out3D)
 11 | {
 12 | 	CV_Assert(disparity.type() == CV_32F && !disparity.empty());
 13 | 	CV_Assert(Q.type() == CV_32F && Q.cols == 4 && Q.rows == 4);
 14 | 
 15 | 	out3D = cv::Mat::zeros(disparity.size(), CV_32FC3);
 16 | 
 17 | 	float Q03 = Q.at<float>(0, 3);
 18 | 	float Q13 = Q.at<float>(1, 3);
 19 | 	float Q23 = Q.at<float>(2, 3);
 20 | 	float Q32 = Q.at<float>(3, 2);
 21 | 	float Q33 = Q.at<float>(3, 3);
 22 | 
 23 | 	for (int i = 0; i < disparity.rows; i++)
 24 | 	{
 25 | 		const float* disp_ptr = disparity.ptr<float>(i);
 26 | 		cv::Vec3f* out3D_ptr = out3D.ptr<cv::Vec3f>(i);
 27 | 
 28 | 		for (int j = 0; j < disparity.cols; j++)
 29 | 		{
 30 | 			const float pw = 1.0f / (disp_ptr[j] * Q32 + Q33);
 31 | 
 32 | 			cv::Vec3f& point = out3D_ptr[j];
 33 | 			point[0] = (static_cast<float>(j)+Q03) * pw;
 34 | 			point[1] = (static_cast<float>(i)+Q13) * pw;
 35 | 			point[2] = Q23 * pw;
 36 | 		}
 37 | 	}
 38 | }
 39 | 
 40 | void save(const cv::Mat& image3D, const std::string& fileName)
 41 | {
 42 | 	CV_Assert(image3D.type() == CV_32FC3 && !image3D.empty());
 43 | 	CV_Assert(!fileName.empty());
 44 | 
 45 | 	std::ofstream outFile(fileName);
 46 | 
 47 | 	if (!outFile.is_open())
 48 | 	{
 49 | 		std::cerr << "ERROR: Could not open " << fileName << std::endl;
 50 | 		return;
 51 | 	}
 52 | 
 53 | 	for (int i = 0; i < image3D.rows; i++)
 54 | 	{
 55 | 		const cv::Vec3f* image3D_ptr = image3D.ptr<cv::Vec3f>(i);
 56 | 
 57 | 		for (int j = 0; j < image3D.cols; j++)
 58 | 		{
 59 | 			outFile << image3D_ptr[j][0] << " " << image3D_ptr[j][1] << " " << image3D_ptr[j][2] << std::endl;
 60 | 		}
 61 | 	}
 62 | 
 63 | 	outFile.close();
 64 | }
 65 | 
 66 | int main(int argc, char* argv[])
 67 | {
 68 | 	// Loading Q Matrix
 69 | 	cv::FileStorage fs("Q.xml", cv::FileStorage::READ);
 70 | 
 71 | 	if (!fs.isOpened())
 72 | 	{
 73 | 		std::cerr << "ERROR: Could not read Q.xml" << std::endl;
 74 | 		return 1;
 75 | 	}
 76 | 
 77 | 	cv::Mat Q;
 78 | 
 79 | 	fs["Q"] >> Q;
 80 | 	Q.convertTo(Q, CV_32F);
 81 | 	fs.release();
 82 | 
 83 | 	// If size of Q is not 4x4 exit
 84 | 	if (Q.cols != 4 || Q.rows != 4)
 85 | 	{
 86 | 		std::cerr << "ERROR: Q is not 4x4)" << std::endl;
 87 | 		return 1;
 88 | 	}
 89 | 
 90 | 	// Loading disparity image
 91 | 	cv::Mat disparity = cv::imread("disparity-image.pgm", cv::IMREAD_GRAYSCALE);
 92 | 	if (disparity.empty())
 93 | 	{
 94 | 		std::cerr << "ERROR: Could not read disparity-image.pgm" << std::endl;
 95 | 		return 1;
 96 | 	}
 97 | 
 98 | 	// Conversion of the disparity map to 32F before reprojecting to 3D
 99 | 	// NOTE: also take care to do not scale twice the disparity
100 | 	disparity.convertTo(disparity, CV_32F, 1.0 / 16.0);
101 | 
102 | 	// Reproject image to 3D by OpenCV
103 | 	cv::Mat image3DOCV;
104 | 	cv::reprojectImageTo3D(disparity, image3DOCV, Q, false, CV_32F);
105 | 
106 | 	// Reproject image to 3D by our own method
107 | 	cv::Mat image3D;
108 | 	customReproject(disparity, Q, image3D);
109 | 
110 | 	save(image3D, "custom.xyz");
111 | 	save(image3D, "opencv.xyz");
112 | 
113 | 	return 0;
114 | }
115 | 


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