WebSVN – seema-scanner – Diff – /src/SMReconstructionWorker.cpp


#include "SMReconstructionWorker.h"
 
#include "CodecGrayCode.h"
#include "CodecPhaseShift.h"
 
#include <QCoreApplication>
#include <QSettings>
 
#include <iostream>
#include <opencv2/opencv.hpp>
 
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/pcd_io.h>
 
 
void SMReconstructionWorker::setup(){
 
    QSettings settings;
 
    // Get current calibration
    calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();
 
    // Create decoder
    dir = (CodecDir)settings.value("pattern/direction", CodecDirHorizontal).toInt();
    if(dir == CodecDirNone)
        std::cerr << "SMCaptureWorker: invalid coding direction " << std::endl;
 
    QString codec = settings.value("codec", "GrayCode").toString();
    if(codec == "PhaseShift")
        decoder = new DecoderPhaseShift(dir);
    else if(codec == "GrayCode")
        decoder = new DecoderGrayCode(dir);
    else
        std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;
 
 
    // Precompute lens correction maps
    cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap0Horz, lensMap0Vert);
    cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1Horz, lensMap1Vert);
 
    //cv::Mat mapHorz, mapVert;
    //cv::normalize(lensMap0Horz, mapHorz, 0, 255, cv::NORM_MINMAX, CV_8U);
    //cv::normalize(lensMap0Vert, mapVert, 0, 255, cv::NORM_MINMAX, CV_8U);
    //cv::imwrite("mapHorz.png", mapHorz);
    //cv::imwrite("mapVert.png", mapVert);
}
 
void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){
 
    time.start();
 
    // Decode frames
    cv::Mat up0, vp0, up1, vp1, shading0, mask0, shading1, mask1;
    decoder->decodeFrames(frameSequence.frames0, up0, vp0, mask0, shading0);
    decoder->decodeFrames(frameSequence.frames1, up1, vp1, mask1, shading1);
 
    // Triangulate
    cv::Mat pointCloud;
    if(dir == CodecDirBoth)
        triangulateFromUpVp(up0, vp0, mask0, up1, vp1, mask1, pointCloud);
    else if(dir == CodecDirHorizontal)
        triangulateFromUp(up0, mask0, up1, mask1, pointCloud);
    else if(dir == CodecDirVertical)
        triangulateFromVp(vp0, mask0, vp1, mask1, pointCloud);
 
    // Simply use shading information from camera 0 (for now)
    cv::Mat shading = shading0;
 
    // Convert point cloud to PCL format
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
 
    // Interprete as organized point cloud
    pointCloudPCL->width = pointCloud.cols;
    pointCloudPCL->height = pointCloud.rows;
    pointCloudPCL->is_dense = false;
 
    pointCloudPCL->points.resize(pointCloud.rows*pointCloud.cols);
 
    for(int row=0; row<pointCloud.rows; row++){
        int offset = row * pointCloudPCL->width;
        for(int col=0; col<pointCloud.cols; col++){
            const cv::Vec3f pnt = pointCloud.at<cv::Vec3f>(row,col);
            unsigned char shade = shading.at<unsigned short>(row,col) >> 8;
            pcl::PointXYZRGB point;
            point.x = pnt[0]; point.y = pnt[1]; point.z = pnt[2];
            point.r = shade; point.g = shade; point.b = shade;
            pointCloudPCL->points[offset + col] = point;
        }
    }
 
/*    // stack xyz data
    std::vector<cv::Mat> xyz;
    cv::split(pointCloud, xyz);
    std::vector<cv::Mat> pointCloudChannels;
    pointCloudChannels.push_back(xyz[0]);
    pointCloudChannels.push_back(xyz[1]);
    pointCloudChannels.push_back(xyz[2]);
 
    // 4 byte padding
    pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
 
    // triple uchar color information
    std::vector<cv::Mat> rgb;
    rgb.push_back(shading);
    rgb.push_back(shading);
    rgb.push_back(shading);
    rgb.push_back(cv::Mat::zeros(shading.size(), CV_8U));
 
    cv::Mat rgb8UC4;
    cv::merge(rgb, rgb8UC4);
 
    cv::Mat rgb32F(rgb8UC4.size(), CV_32F, rgb8UC4.data);
 
    pointCloudChannels.push_back(rgb32F);
 
    // 12 bytes padding
    pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
    pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
    pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));
 
    // merge channels
    cv::Mat pointCloudPadded;
    cv::merge(pointCloudChannels, pointCloudPadded);
 
    // memcpy everything
    memcpy(&pointCloudPCL->points[0], pointCloudPadded.data, pointCloudPadded.rows*pointCloudPadded.cols*sizeof(pcl::PointXYZRGB));*/
 
 
    // Emit result
    emit newPointCloud(pointCloudPCL);
 
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
 
}
 
void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){
 
    // Process sequentially
    for(int i=0; i<frameSequences.size(); i++){
        reconstructPointCloud(frameSequences[i]);
    }
 
}
 
void SMReconstructionWorker::triangulateFromUp(cv::Mat up0, cv::Mat mask0,cv::Mat up1, cv::Mat mask1,cv::Mat &xyz){}
void SMReconstructionWorker::triangulateFromVp(cv::Mat vp0, cv::Mat mask0, cv::Mat vp1, cv::Mat mask1, cv::Mat &xyz){}
void SMReconstructionWorker::triangulateFromUpVp(cv::Mat up0, cv::Mat vp0, cv::Mat mask0, cv::Mat up1, cv::Mat vp1, cv::Mat mask1, cv::Mat &xyz){}
 
//void SMReconstructionWorker::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
 
//    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
//    int N = up.rows * up.cols;
 
//    cv::Mat projPointsCam(2, N, CV_32F);
//    uc.reshape(0,1).copyTo(projPointsCam.row(0));
//    vc.reshape(0,1).copyTo(projPointsCam.row(1));
 
//    cv::Mat projPointsProj(2, N, CV_32F);
//    up.reshape(0,1).copyTo(projPointsProj.row(0));
//    vp.reshape(0,1).copyTo(projPointsProj.row(1));
 
//    cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
//    cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
 
//    cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
//    cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
//    cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
//    Pp = cv::Mat(calibration.Kp) * temp;
 
//    cv::Mat xyzw;
//    cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);
 
//    xyz.create(3, N, CV_32F);
//    for(int i=0; i<N; i++){
//        xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);
//        xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);
//        xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);
//    }
 
//    xyz = xyz.t();
//    xyz = xyz.reshape(3, up.rows);
//}
 
 

Subversion Repositories seema-scanner

(root)/src/SMReconstructionWorker.cpp – Rev 28 → 33

Rev 28		Rev 33
1	`#include "SMReconstructionWorker.h"`	1	`#include "SMReconstructionWorker.h"`
2		2
3	`#include "CodecGrayCode.h"`	3	`#include "CodecGrayCode.h"`
4	`#include "CodecPhaseShift.h"`	4	`#include "CodecPhaseShift.h"`
5		5
6	`#include <QCoreApplication>`	6	`#include <QCoreApplication>`
7	`#include <QSettings>`	7	`#include <QSettings>`
8		8
9	`#include <iostream>`	9	`#include <iostream>`
10	`#include <opencv2/opencv.hpp>`	10	`#include <opencv2/opencv.hpp>`
11		11
12	`#include <pcl/filters/statistical_outlier_removal.h>`	12	`#include <pcl/filters/statistical_outlier_removal.h>`
13	`#include <pcl/io/pcd_io.h>`	13	`#include <pcl/io/pcd_io.h>`
14		14
15		15
16	`void SMReconstructionWorker::setup(){`	16	`void SMReconstructionWorker::setup(){`
17		17
18	`QSettings settings;`	18	`QSettings settings;`
19		19
20	`// Get current calibration`	20	`// Get current calibration`
21	`calibration = settings.value("calibration").value<SMCalibrationParameters>();`	21	`calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();`
22		22
23	`// Create decoder`	23	`// Create decoder`
24	`dir = (CodecDir)settings.value("pattern/direction", CodecDirHorizontal).toInt();`	24	`dir = (CodecDir)settings.value("pattern/direction", CodecDirHorizontal).toInt();`
25	`if(dir == CodecDirNone)`	25	`if(dir == CodecDirNone)`
26	`std::cerr << "SMCaptureWorker: invalid coding direction " << std::endl;`	26	`std::cerr << "SMCaptureWorker: invalid coding direction " << std::endl;`
27		27
28	`QString codec = settings.value("codec", "GrayCode").toString();`	28	`QString codec = settings.value("codec", "GrayCode").toString();`
29	`if(codec == "PhaseShift")`	29	`if(codec == "PhaseShift")`
30	`decoder = new DecoderPhaseShift(dir);`	30	`decoder = new DecoderPhaseShift(dir);`
31	`else if(codec == "GrayCode")`	31	`else if(codec == "GrayCode")`
32	`decoder = new DecoderGrayCode(dir);`	32	`decoder = new DecoderGrayCode(dir);`
33	`else`	33	`else`
34	`std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;`	34	`std::cerr << "SLScanWorker: invalid codec " << codec.toStdString() << std::endl;`
35		35
36		36
37	`// Precompute lens correction maps`	37	`// Precompute lens correction maps`
38	`cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);`	38	`cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);`
39	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap0Horz, lensMap0Vert);`	39	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap0Horz, lensMap0Vert);`
40	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1Horz, lensMap1Vert);`	40	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, eye, calibration.K0, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1Horz, lensMap1Vert);`
41		41
42	`//cv::Mat mapHorz, mapVert;`	42	`//cv::Mat mapHorz, mapVert;`
43	`//cv::normalize(lensMap0Horz, mapHorz, 0, 255, cv::NORM_MINMAX, CV_8U);`	43	`//cv::normalize(lensMap0Horz, mapHorz, 0, 255, cv::NORM_MINMAX, CV_8U);`
44	`//cv::normalize(lensMap0Vert, mapVert, 0, 255, cv::NORM_MINMAX, CV_8U);`	44	`//cv::normalize(lensMap0Vert, mapVert, 0, 255, cv::NORM_MINMAX, CV_8U);`
45	`//cv::imwrite("mapHorz.png", mapHorz);`	45	`//cv::imwrite("mapHorz.png", mapHorz);`
46	`//cv::imwrite("mapVert.png", mapVert);`	46	`//cv::imwrite("mapVert.png", mapVert);`
47	`}`	47	`}`
48		48
49	`void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){`	49	`void SMReconstructionWorker::reconstructPointCloud(SMFrameSequence frameSequence){`
50		50
51	`time.start();`	51	`time.start();`
52		52
53	`// Decode frames`	53	`// Decode frames`
54	`cv::Mat up0, vp0, up1, vp1, shading0, mask0, shading1, mask1;`	54	`cv::Mat up0, vp0, up1, vp1, shading0, mask0, shading1, mask1;`
55	`decoder->decodeFrames(frameSequence.frames0, up0, vp0, mask0, shading0);`	55	`decoder->decodeFrames(frameSequence.frames0, up0, vp0, mask0, shading0);`
56	`decoder->decodeFrames(frameSequence.frames1, up1, vp1, mask1, shading1);`	56	`decoder->decodeFrames(frameSequence.frames1, up1, vp1, mask1, shading1);`
57		57
58	`// Triangulate`	58	`// Triangulate`
59	`cv::Mat pointCloud;`	59	`cv::Mat pointCloud;`
60	`if(dir == CodecDirBoth)`	60	`if(dir == CodecDirBoth)`
61	`triangulateFromUpVp(up0, vp0, mask0, up1, vp1, mask1, pointCloud);`	61	`triangulateFromUpVp(up0, vp0, mask0, up1, vp1, mask1, pointCloud);`
62	`else if(dir == CodecDirHorizontal)`	62	`else if(dir == CodecDirHorizontal)`
63	`triangulateFromUp(up0, mask0, up1, mask1, pointCloud);`	63	`triangulateFromUp(up0, mask0, up1, mask1, pointCloud);`
64	`else if(dir == CodecDirVertical)`	64	`else if(dir == CodecDirVertical)`
65	`triangulateFromVp(vp0, mask0, vp1, mask1, pointCloud);`	65	`triangulateFromVp(vp0, mask0, vp1, mask1, pointCloud);`
66		66
67	`// Simply use shading information from camera 0 (for now)`	67	`// Simply use shading information from camera 0 (for now)`
68	`cv::Mat shading = shading0;`	68	`cv::Mat shading = shading0;`
69		69
70	`// Convert point cloud to PCL format`	70	`// Convert point cloud to PCL format`
71	`pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);`	71	`pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);`
72		72
73	`// Interprete as organized point cloud`	73	`// Interprete as organized point cloud`
74	`pointCloudPCL->width = pointCloud.cols;`	74	`pointCloudPCL->width = pointCloud.cols;`
75	`pointCloudPCL->height = pointCloud.rows;`	75	`pointCloudPCL->height = pointCloud.rows;`
76	`pointCloudPCL->is_dense = false;`	76	`pointCloudPCL->is_dense = false;`
77		77
78	`pointCloudPCL->points.resize(pointCloud.rows*pointCloud.cols);`	78	`pointCloudPCL->points.resize(pointCloud.rows*pointCloud.cols);`
79		79
80	`for(int row=0; row<pointCloud.rows; row++){`	80	`for(int row=0; row<pointCloud.rows; row++){`
81	`int offset = row * pointCloudPCL->width;`	81	`int offset = row * pointCloudPCL->width;`
82	`for(int col=0; col<pointCloud.cols; col++){`	82	`for(int col=0; col<pointCloud.cols; col++){`
83	`const cv::Vec3f pnt = pointCloud.at<cv::Vec3f>(row,col);`	83	`const cv::Vec3f pnt = pointCloud.at<cv::Vec3f>(row,col);`
84	`unsigned char shade = shading.at<unsigned short>(row,col) >> 8;`	84	`unsigned char shade = shading.at<unsigned short>(row,col) >> 8;`
85	`pcl::PointXYZRGB point;`	85	`pcl::PointXYZRGB point;`
86	`point.x = pnt[0]; point.y = pnt[1]; point.z = pnt[2];`	86	`point.x = pnt[0]; point.y = pnt[1]; point.z = pnt[2];`
87	`point.r = shade; point.g = shade; point.b = shade;`	87	`point.r = shade; point.g = shade; point.b = shade;`
88	`pointCloudPCL->points[offset + col] = point;`	88	`pointCloudPCL->points[offset + col] = point;`
89	`}`	89	`}`
90	`}`	90	`}`
91		91
92	`/* // stack xyz data`	92	`/* // stack xyz data`
93	`std::vector<cv::Mat> xyz;`	93	`std::vector<cv::Mat> xyz;`
94	`cv::split(pointCloud, xyz);`	94	`cv::split(pointCloud, xyz);`
95	`std::vector<cv::Mat> pointCloudChannels;`	95	`std::vector<cv::Mat> pointCloudChannels;`
96	`pointCloudChannels.push_back(xyz[0]);`	96	`pointCloudChannels.push_back(xyz[0]);`
97	`pointCloudChannels.push_back(xyz[1]);`	97	`pointCloudChannels.push_back(xyz[1]);`
98	`pointCloudChannels.push_back(xyz[2]);`	98	`pointCloudChannels.push_back(xyz[2]);`
99		99
100	`// 4 byte padding`	100	`// 4 byte padding`
101	`pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));`	101	`pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));`
102		102
103	`// triple uchar color information`	103	`// triple uchar color information`
104	`std::vector<cv::Mat> rgb;`	104	`std::vector<cv::Mat> rgb;`
105	`rgb.push_back(shading);`	105	`rgb.push_back(shading);`
106	`rgb.push_back(shading);`	106	`rgb.push_back(shading);`
107	`rgb.push_back(shading);`	107	`rgb.push_back(shading);`
108	`rgb.push_back(cv::Mat::zeros(shading.size(), CV_8U));`	108	`rgb.push_back(cv::Mat::zeros(shading.size(), CV_8U));`
109		109
110	`cv::Mat rgb8UC4;`	110	`cv::Mat rgb8UC4;`
111	`cv::merge(rgb, rgb8UC4);`	111	`cv::merge(rgb, rgb8UC4);`
112		112
113	`cv::Mat rgb32F(rgb8UC4.size(), CV_32F, rgb8UC4.data);`	113	`cv::Mat rgb32F(rgb8UC4.size(), CV_32F, rgb8UC4.data);`
114		114
115	`pointCloudChannels.push_back(rgb32F);`	115	`pointCloudChannels.push_back(rgb32F);`
116		116
117	`// 12 bytes padding`	117	`// 12 bytes padding`
118	`pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));`	118	`pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));`
119	`pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));`	119	`pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));`
120	`pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));`	120	`pointCloudChannels.push_back(cv::Mat::zeros(pointCloud.size(), CV_32F));`
121		121
122	`// merge channels`	122	`// merge channels`
123	`cv::Mat pointCloudPadded;`	123	`cv::Mat pointCloudPadded;`
124	`cv::merge(pointCloudChannels, pointCloudPadded);`	124	`cv::merge(pointCloudChannels, pointCloudPadded);`
125		125
126	`// memcpy everything`	126	`// memcpy everything`
127	`memcpy(&pointCloudPCL->points[0], pointCloudPadded.data, pointCloudPadded.rowspointCloudPadded.colssizeof(pcl::PointXYZRGB));*/`	127	`memcpy(&pointCloudPCL->points[0], pointCloudPadded.data, pointCloudPadded.rowspointCloudPadded.colssizeof(pcl::PointXYZRGB));*/`
128		128
129		129
130	`// Emit result`	130	`// Emit result`
131	`emit newPointCloud(pointCloudPCL);`	131	`emit newPointCloud(pointCloudPCL);`
132		132
133	`std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;`	133	`std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;`
134		134
135	`}`	135	`}`
136		136
137	`void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){`	137	`void SMReconstructionWorker::reconstructPointClouds(std::vector<SMFrameSequence> frameSequences){`
138		138
139	`// Process sequentially`	139	`// Process sequentially`
140	`for(int i=0; i<frameSequences.size(); i++){`	140	`for(int i=0; i<frameSequences.size(); i++){`
141	`reconstructPointCloud(frameSequences[i]);`	141	`reconstructPointCloud(frameSequences[i]);`
142	`}`	142	`}`
143		143
144	`}`	144	`}`
145		145
146	`void SMReconstructionWorker::triangulateFromUp(cv::Mat up0, cv::Mat mask0,cv::Mat up1, cv::Mat mask1,cv::Mat &xyz){}`	146	`void SMReconstructionWorker::triangulateFromUp(cv::Mat up0, cv::Mat mask0,cv::Mat up1, cv::Mat mask1,cv::Mat &xyz){}`
147	`void SMReconstructionWorker::triangulateFromVp(cv::Mat vp0, cv::Mat mask0, cv::Mat vp1, cv::Mat mask1, cv::Mat &xyz){}`	147	`void SMReconstructionWorker::triangulateFromVp(cv::Mat vp0, cv::Mat mask0, cv::Mat vp1, cv::Mat mask1, cv::Mat &xyz){}`
148	`void SMReconstructionWorker::triangulateFromUpVp(cv::Mat up0, cv::Mat vp0, cv::Mat mask0, cv::Mat up1, cv::Mat vp1, cv::Mat mask1, cv::Mat &xyz){}`	148	`void SMReconstructionWorker::triangulateFromUpVp(cv::Mat up0, cv::Mat vp0, cv::Mat mask0, cv::Mat up1, cv::Mat vp1, cv::Mat mask1, cv::Mat &xyz){}`
149		149
150	`//void SMReconstructionWorker::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){`	150	`//void SMReconstructionWorker::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){`
151		151
152	`// std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;`	152	`// std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;`
153	`// int N = up.rows * up.cols;`	153	`// int N = up.rows * up.cols;`
154		154
155	`// cv::Mat projPointsCam(2, N, CV_32F);`	155	`// cv::Mat projPointsCam(2, N, CV_32F);`
156	`// uc.reshape(0,1).copyTo(projPointsCam.row(0));`	156	`// uc.reshape(0,1).copyTo(projPointsCam.row(0));`
157	`// vc.reshape(0,1).copyTo(projPointsCam.row(1));`	157	`// vc.reshape(0,1).copyTo(projPointsCam.row(1));`
158		158
159	`// cv::Mat projPointsProj(2, N, CV_32F);`	159	`// cv::Mat projPointsProj(2, N, CV_32F);`
160	`// up.reshape(0,1).copyTo(projPointsProj.row(0));`	160	`// up.reshape(0,1).copyTo(projPointsProj.row(0));`
161	`// vp.reshape(0,1).copyTo(projPointsProj.row(1));`	161	`// vp.reshape(0,1).copyTo(projPointsProj.row(1));`
162		162
163	`// cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));`	163	`// cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));`
164	`// cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));`	164	`// cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));`
165		165
166	`// cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);`	166	`// cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);`
167	`// cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));`	167	`// cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));`
168	`// cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));`	168	`// cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));`
169	`// Pp = cv::Mat(calibration.Kp) * temp;`	169	`// Pp = cv::Mat(calibration.Kp) * temp;`
170		170
171	`// cv::Mat xyzw;`	171	`// cv::Mat xyzw;`
172	`// cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);`	172	`// cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);`
173		173
174	`// xyz.create(3, N, CV_32F);`	174	`// xyz.create(3, N, CV_32F);`
175	`// for(int i=0; i<N; i++){`	175	`// for(int i=0; i<N; i++){`
176	`// xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);`	176	`// xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);`
177	`// xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);`	177	`// xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);`
178	`// xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);`	178	`// xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);`
179	`// }`	179	`// }`
180		180
181	`// xyz = xyz.t();`	181	`// xyz = xyz.t();`
182	`// xyz = xyz.reshape(3, up.rows);`	182	`// xyz = xyz.reshape(3, up.rows);`
183	`//}`	183	`//}`
184		184
185		185