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


#include "SMReconstructionWorker.h"
 
#include "AlgorithmGrayCode.h"
#include "AlgorithmGrayCodeHorzVert.h"
#include "AlgorithmPhaseShiftTwoFreq.h"
#include "AlgorithmPhaseShiftTwoFreqHorzVert.h"
#include "AlgorithmPhaseShiftThreeFreq.h"
#include "AlgorithmPhaseShiftEmbedded.h"
#include "AlgorithmLineShift.h"
 
#include <QCoreApplication>
#include <QSettings>
 
#include <iostream>
#include <opencv2/opencv.hpp>
 
#include "cvtools.h"
#include <opencv2/core/eigen.hpp>
 
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/common/transforms.h>
 
 
void SMReconstructionWorker::reconstructPointCloud(const SMFrameSequence &frameSequence){
 
    QSettings settings;
 
    // Get current calibration
    calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();
 
    // Create Algorithm
    QString codec = frameSequence.codec;
    int resX = settings.value("projector/resX").toInt();
    int resY = settings.value("projector/resY").toInt();
 
    if(codec == "GrayCode")
        algorithm = new AlgorithmGrayCode(resX, resY);
    else if(codec == "GrayCodeHorzVert")
        algorithm = new AlgorithmGrayCodeHorzVert(resX, resY);
    else if(codec == "PhaseShiftTwoFreq")
        algorithm = new AlgorithmPhaseShiftTwoFreq(resX, resY);
    else if(codec == "PhaseShiftTwoFreqHorzVert")
        algorithm = new AlgorithmPhaseShiftTwoFreqHorzVert(resX, resY);
    else if(codec == "PhaseShiftThreeFreq")
        algorithm = new AlgorithmPhaseShiftThreeFreq(resX, resY);
    else if(codec == "PhaseShiftEmbedded")
        algorithm = new AlgorithmPhaseShiftEmbedded(resX, resY);
    else if(codec == "LineShift")
        algorithm = new AlgorithmLineShift(resX, resY);
    else{
        std::cerr << "SLScanWorker: invalid codec (Please set codec in preferences): " << codec.toStdString() << std::endl;
        return; // otherwise segfault TODO no default?
    }
 
    assert(frameSequence.frames0.size() == algorithm->getNPatterns());
    assert(frameSequence.frames1.size() == algorithm->getNPatterns());
 
    // Print OpenCV build information
    cv::setUseOptimized(true);
    std::cout << cv::getBuildInformation();
 
    time.start();
 
    // Get 3D Points
    std::vector<cv::Point3f> Q;
    std::vector<cv::Vec3b> color;
    algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);
 
    // Convert point cloud to PCL format
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 
    pointCloudPCL->width = Q.size();
    pointCloudPCL->height = 1;
    pointCloudPCL->is_dense = true;
 
    pointCloudPCL->points.resize(Q.size());
 
    for(unsigned int i=0; i<Q.size(); i++){
        pcl::PointXYZRGBNormal point;
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
        pointCloudPCL->points[i] = point;
    }
 
    // Transform point cloud to rotation axis coordinate system
    /*cv::Mat TRCV(3, 4, CV_32F);
    cv::Mat(calibration.Rr).copyTo(TRCV.colRange(0, 3));
    cv::Mat(calibration.Tr).copyTo(TRCV.col(3));
    Eigen::Affine3f TR;
    cv::cv2eigen(TRCV, TR.matrix());
    pcl::transformPointCloud(*pointCloudPCL, *pointCloudPCL, TR);
 
    // Estimate surface normals (does not produce proper normals...)
    std::cout << "Estimating normals..." << std::endl;
    pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*pointCloudPCL, *points);
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
    tree->setInputCloud(points);
    ne.setSearchMethod(tree);
    ne.setRadiusSearch(1.0);
    //ne.setKSearch(50);
    ne.setViewPoint(0.0, 0.0, 0.0);
    ne.setInputCloud(points);
    ne.compute(*normals);
    pcl::copyPointCloud(*normals, *pointCloudPCL);*/
 
    // Assemble SMPointCloud data structure
    SMPointCloud smPointCloud;
    smPointCloud.id = frameSequence.id;
    smPointCloud.pointCloud = pointCloudPCL;
    smPointCloud.rotationAngle = frameSequence.rotationAngle;
 
    // Determine transform in world (camera0) coordinate system
    float angleRadians = frameSequence.rotationAngle/180.0*M_PI;
    cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
    cv::Mat R;
    cv::Rodrigues(rot_rvec, R);
    smPointCloud.R = calibration.Rr.t()*cv::Matx33f(R)*calibration.Rr;
    smPointCloud.T = calibration.Rr.t()*cv::Matx33f(R)*calibration.Tr - calibration.Rr.t()*calibration.Tr;
 
 
    // Determine transform in world (camera0) coordinate system
    /*float angleRadians = frameSequence.rotationAngle/180.0*M_PI;
    cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
    cv::Mat R;
    cv::Rodrigues(rot_rvec, R);
    smPointCloud.R = cv::Matx33f(R);
    smPointCloud.T = cv::Vec3f(0.0,0.0,0.0);*/
 
    // Emit result
    emit newPointCloud(smPointCloud);
    std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;
    std::cout << "SMReconstructionWorker: " << smPointCloud.pointCloud->size() << " Points" << std::endl;
 
}
 
void SMReconstructionWorker::reconstructPointClouds(const std::vector<SMFrameSequence> &frameSequences){
 
    // Process sequentially
    #pragma omp parallel for
    for(unsigned int i=0; i<frameSequences.size(); i++){
        if(!frameSequences[i].reconstructed) reconstructPointCloud(frameSequences[i]);
    }
}
 
void SMReconstructionWorker::triangulate(const std::vector<cv::Point2f>& q0, const std::vector<cv::Point2f>& q1, std::vector<cv::Point3f> &Q){
    cv::Mat P0(3,4,CV_32F,cv::Scalar(0.0));
    cv::Mat(calibration.K0).copyTo(P0(cv::Range(0,3), cv::Range(0,3)));
 
    cv::Mat temp(3,4,CV_32F);
    cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
    cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
    cv::Mat P1 = cv::Mat(calibration.K1) * temp;
 
    cv::Mat QMatHomogenous, QMat;
    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
    cvtools::matToPoints3f(QMat, Q);
}
 

Subversion Repositories seema-scanner

(root)/src/SMReconstructionWorker.cpp – Rev 225 → 229

Rev 225		Rev 229
1	`#include "SMReconstructionWorker.h"`	1	`#include "SMReconstructionWorker.h"`
2		2
3	`#include "AlgorithmGrayCode.h"`	3	`#include "AlgorithmGrayCode.h"`
4	`#include "AlgorithmGrayCodeHorzVert.h"`	4	`#include "AlgorithmGrayCodeHorzVert.h"`
5	`#include "AlgorithmPhaseShiftTwoFreq.h"`	5	`#include "AlgorithmPhaseShiftTwoFreq.h"`
6	`#include "AlgorithmPhaseShiftTwoFreqHorzVert.h"`	6	`#include "AlgorithmPhaseShiftTwoFreqHorzVert.h"`
7	`#include "AlgorithmPhaseShiftThreeFreq.h"`	7	`#include "AlgorithmPhaseShiftThreeFreq.h"`
8	`#include "AlgorithmPhaseShiftEmbedded.h"`	8	`#include "AlgorithmPhaseShiftEmbedded.h"`
9	`#include "AlgorithmLineShift.h"`	9	`#include "AlgorithmLineShift.h"`
10		10
11	`#include <QCoreApplication>`	11	`#include <QCoreApplication>`
12	`#include <QSettings>`	12	`#include <QSettings>`
13		13
14	`#include <iostream>`	14	`#include <iostream>`
15	`#include <opencv2/opencv.hpp>`	15	`#include <opencv2/opencv.hpp>`
16		16
17	`#include "cvtools.h"`	17	`#include "cvtools.h"`
18	`#include <opencv2/core/eigen.hpp>`	18	`#include <opencv2/core/eigen.hpp>`
19		19
20	`#include <pcl/filters/statistical_outlier_removal.h>`	20	`#include <pcl/filters/statistical_outlier_removal.h>`
21	`#include <pcl/io/pcd_io.h>`	21	`#include <pcl/io/pcd_io.h>`
22	`#include <pcl/features/normal_3d.h>`	22	`#include <pcl/features/normal_3d.h>`
23	`#include <pcl/features/normal_3d_omp.h>`	23	`#include <pcl/features/normal_3d_omp.h>`
24	`#include <pcl/common/transforms.h>`	24	`#include <pcl/common/transforms.h>`
25		25
26		26
27	`void SMReconstructionWorker::reconstructPointCloud(const SMFrameSequence &frameSequence){`	27	`void SMReconstructionWorker::reconstructPointCloud(const SMFrameSequence &frameSequence){`
28		28
29	`QSettings settings;`	29	`QSettings settings;`
30		30
31	`// Get current calibration`	31	`// Get current calibration`
32	`calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();`	32	`calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();`
33		33
34	`// Create Algorithm`	34	`// Create Algorithm`
35	`QString codec = frameSequence.codec;`	35	`QString codec = frameSequence.codec;`
36	`int resX = settings.value("projector/resX").toInt();`	36	`int resX = settings.value("projector/resX").toInt();`
37	`int resY = settings.value("projector/resY").toInt();`	37	`int resY = settings.value("projector/resY").toInt();`
38		38
39	`if(codec == "GrayCode")`	39	`if(codec == "GrayCode")`
40	`algorithm = new AlgorithmGrayCode(resX, resY);`	40	`algorithm = new AlgorithmGrayCode(resX, resY);`
41	`else if(codec == "GrayCodeHorzVert")`	41	`else if(codec == "GrayCodeHorzVert")`
42	`algorithm = new AlgorithmGrayCodeHorzVert(resX, resY);`	42	`algorithm = new AlgorithmGrayCodeHorzVert(resX, resY);`
43	`else if(codec == "PhaseShiftTwoFreq")`	43	`else if(codec == "PhaseShiftTwoFreq")`
44	`algorithm = new AlgorithmPhaseShiftTwoFreq(resX, resY);`	44	`algorithm = new AlgorithmPhaseShiftTwoFreq(resX, resY);`
45	`else if(codec == "PhaseShiftTwoFreqHorzVert")`	45	`else if(codec == "PhaseShiftTwoFreqHorzVert")`
46	`algorithm = new AlgorithmPhaseShiftTwoFreqHorzVert(resX, resY);`	46	`algorithm = new AlgorithmPhaseShiftTwoFreqHorzVert(resX, resY);`
47	`else if(codec == "PhaseShiftThreeFreq")`	47	`else if(codec == "PhaseShiftThreeFreq")`
48	`algorithm = new AlgorithmPhaseShiftThreeFreq(resX, resY);`	48	`algorithm = new AlgorithmPhaseShiftThreeFreq(resX, resY);`
49	`else if(codec == "PhaseShiftEmbedded")`	49	`else if(codec == "PhaseShiftEmbedded")`
50	`algorithm = new AlgorithmPhaseShiftEmbedded(resX, resY);`	50	`algorithm = new AlgorithmPhaseShiftEmbedded(resX, resY);`
51	`else if(codec == "LineShift")`	51	`else if(codec == "LineShift")`
52	`algorithm = new AlgorithmLineShift(resX, resY);`	52	`algorithm = new AlgorithmLineShift(resX, resY);`
53	`else{`	53	`else{`
54	`std::cerr << "SLScanWorker: invalid codec (Please set codec in preferences): " << codec.toStdString() << std::endl;`	54	`std::cerr << "SLScanWorker: invalid codec (Please set codec in preferences): " << codec.toStdString() << std::endl;`
55	`return; // otherwise segfault TODO no default?`	55	`return; // otherwise segfault TODO no default?`
56	`}`	56	`}`
57		57
58	`assert(frameSequence.frames0.size() == algorithm->getNPatterns());`	58	`assert(frameSequence.frames0.size() == algorithm->getNPatterns());`
59	`assert(frameSequence.frames1.size() == algorithm->getNPatterns());`	59	`assert(frameSequence.frames1.size() == algorithm->getNPatterns());`
60		60
-		61	`// Print OpenCV build information`
-		62	`cv::setUseOptimized(true);`
-		63	`std::cout << cv::getBuildInformation();`
-		64
61	`time.start();`	65	`time.start();`
62		66
63	`// Get 3D Points`	67	`// Get 3D Points`
64	`std::vector<cv::Point3f> Q;`	68	`std::vector<cv::Point3f> Q;`
65	`std::vector<cv::Vec3b> color;`	69	`std::vector<cv::Vec3b> color;`
66	`algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);`	70	`algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);`
67		71
68	`// Convert point cloud to PCL format`	72	`// Convert point cloud to PCL format`
69	`pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGBNormal>);`	73	`pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGBNormal>);`
70		74
71	`pointCloudPCL->width = Q.size();`	75	`pointCloudPCL->width = Q.size();`
72	`pointCloudPCL->height = 1;`	76	`pointCloudPCL->height = 1;`
73	`pointCloudPCL->is_dense = true;`	77	`pointCloudPCL->is_dense = true;`
74		78
75	`pointCloudPCL->points.resize(Q.size());`	79	`pointCloudPCL->points.resize(Q.size());`
76		80
77	`for(unsigned int i=0; i<Q.size(); i++){`	81	`for(unsigned int i=0; i<Q.size(); i++){`
78	`pcl::PointXYZRGBNormal point;`	82	`pcl::PointXYZRGBNormal point;`
79	`point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;`	83	`point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;`
80	`point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];`	84	`point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];`
81	`pointCloudPCL->points[i] = point;`	85	`pointCloudPCL->points[i] = point;`
82	`}`	86	`}`
83		87
84	`// Transform point cloud to rotation axis coordinate system`	88	`// Transform point cloud to rotation axis coordinate system`
85	`/*cv::Mat TRCV(3, 4, CV_32F);`	89	`/*cv::Mat TRCV(3, 4, CV_32F);`
86	`cv::Mat(calibration.Rr).copyTo(TRCV.colRange(0, 3));`	90	`cv::Mat(calibration.Rr).copyTo(TRCV.colRange(0, 3));`
87	`cv::Mat(calibration.Tr).copyTo(TRCV.col(3));`	91	`cv::Mat(calibration.Tr).copyTo(TRCV.col(3));`
88	`Eigen::Affine3f TR;`	92	`Eigen::Affine3f TR;`
89	`cv::cv2eigen(TRCV, TR.matrix());`	93	`cv::cv2eigen(TRCV, TR.matrix());`
90	`pcl::transformPointCloud(pointCloudPCL, pointCloudPCL, TR);`	94	`pcl::transformPointCloud(pointCloudPCL, pointCloudPCL, TR);`
91		95
92	`// Estimate surface normals (does not produce proper normals...)`	96	`// Estimate surface normals (does not produce proper normals...)`
93	`std::cout << "Estimating normals..." << std::endl;`	97	`std::cout << "Estimating normals..." << std::endl;`
94	`pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);`	98	`pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);`
95	`pcl::copyPointCloud(pointCloudPCL, points);`	99	`pcl::copyPointCloud(pointCloudPCL, points);`
96	`pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);`	100	`pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);`
97	`pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;`	101	`pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;`
98	`pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());`	102	`pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());`
99	`tree->setInputCloud(points);`	103	`tree->setInputCloud(points);`
100	`ne.setSearchMethod(tree);`	104	`ne.setSearchMethod(tree);`
101	`ne.setRadiusSearch(1.0);`	105	`ne.setRadiusSearch(1.0);`
102	`//ne.setKSearch(50);`	106	`//ne.setKSearch(50);`
103	`ne.setViewPoint(0.0, 0.0, 0.0);`	107	`ne.setViewPoint(0.0, 0.0, 0.0);`
104	`ne.setInputCloud(points);`	108	`ne.setInputCloud(points);`
105	`ne.compute(*normals);`	109	`ne.compute(*normals);`
106	`pcl::copyPointCloud(normals, pointCloudPCL);*/`	110	`pcl::copyPointCloud(normals, pointCloudPCL);*/`
107		111
108	`// Assemble SMPointCloud data structure`	112	`// Assemble SMPointCloud data structure`
109	`SMPointCloud smPointCloud;`	113	`SMPointCloud smPointCloud;`
110	`smPointCloud.id = frameSequence.id;`	114	`smPointCloud.id = frameSequence.id;`
111	`smPointCloud.pointCloud = pointCloudPCL;`	115	`smPointCloud.pointCloud = pointCloudPCL;`
112	`smPointCloud.rotationAngle = frameSequence.rotationAngle;`	116	`smPointCloud.rotationAngle = frameSequence.rotationAngle;`
113		117
114	`// Determine transform in world (camera0) coordinate system`	118	`// Determine transform in world (camera0) coordinate system`
115	`float angleRadians = frameSequence.rotationAngle/180.0*M_PI;`	119	`float angleRadians = frameSequence.rotationAngle/180.0*M_PI;`
116	`cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);`	120	`cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);`
117	`cv::Mat R;`	121	`cv::Mat R;`
118	`cv::Rodrigues(rot_rvec, R);`	122	`cv::Rodrigues(rot_rvec, R);`
119	`smPointCloud.R = calibration.Rr.t()cv::Matx33f(R)calibration.Rr;`	123	`smPointCloud.R = calibration.Rr.t()cv::Matx33f(R)calibration.Rr;`
120	`smPointCloud.T = calibration.Rr.t()cv::Matx33f(R)calibration.Tr - calibration.Rr.t()*calibration.Tr;`	124	`smPointCloud.T = calibration.Rr.t()cv::Matx33f(R)calibration.Tr - calibration.Rr.t()*calibration.Tr;`
121		125
122		126
123	`// Determine transform in world (camera0) coordinate system`	127	`// Determine transform in world (camera0) coordinate system`
124	`/float angleRadians = frameSequence.rotationAngle/180.0M_PI;`	128	`/float angleRadians = frameSequence.rotationAngle/180.0M_PI;`
125	`cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);`	129	`cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);`
126	`cv::Mat R;`	130	`cv::Mat R;`
127	`cv::Rodrigues(rot_rvec, R);`	131	`cv::Rodrigues(rot_rvec, R);`
128	`smPointCloud.R = cv::Matx33f(R);`	132	`smPointCloud.R = cv::Matx33f(R);`
129	`smPointCloud.T = cv::Vec3f(0.0,0.0,0.0);*/`	133	`smPointCloud.T = cv::Vec3f(0.0,0.0,0.0);*/`
130		134
131	`// Emit result`	135	`// Emit result`
132	`emit newPointCloud(smPointCloud);`	136	`emit newPointCloud(smPointCloud);`
133	`std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;`	137	`std::cout << "SMReconstructionWorker: " << time.elapsed() << "ms" << std::endl;`
134	`std::cout << "SMReconstructionWorker: " << smPointCloud.pointCloud->size() << " Points" << std::endl;`	138	`std::cout << "SMReconstructionWorker: " << smPointCloud.pointCloud->size() << " Points" << std::endl;`
135		139
136	`}`	140	`}`
137		141
138	`void SMReconstructionWorker::reconstructPointClouds(const std::vector<SMFrameSequence> &frameSequences){`	142	`void SMReconstructionWorker::reconstructPointClouds(const std::vector<SMFrameSequence> &frameSequences){`
139		143
140	`// Process sequentially`	144	`// Process sequentially`
141	`#pragma omp parallel for`	145	`#pragma omp parallel for`
142	`for(unsigned int i=0; i<frameSequences.size(); i++){`	146	`for(unsigned int i=0; i<frameSequences.size(); i++){`
143	`if(!frameSequences[i].reconstructed) reconstructPointCloud(frameSequences[i]);`	147	`if(!frameSequences[i].reconstructed) reconstructPointCloud(frameSequences[i]);`
144	`}`	148	`}`
145	`}`	149	`}`
146		150
147	`void SMReconstructionWorker::triangulate(const std::vector<cv::Point2f>& q0, const std::vector<cv::Point2f>& q1, std::vector<cv::Point3f> &Q){`	151	`void SMReconstructionWorker::triangulate(const std::vector<cv::Point2f>& q0, const std::vector<cv::Point2f>& q1, std::vector<cv::Point3f> &Q){`
148	`cv::Mat P0(3,4,CV_32F,cv::Scalar(0.0));`	152	`cv::Mat P0(3,4,CV_32F,cv::Scalar(0.0));`
149	`cv::Mat(calibration.K0).copyTo(P0(cv::Range(0,3), cv::Range(0,3)));`	153	`cv::Mat(calibration.K0).copyTo(P0(cv::Range(0,3), cv::Range(0,3)));`
150		154
151	`cv::Mat temp(3,4,CV_32F);`	155	`cv::Mat temp(3,4,CV_32F);`
152	`cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));`	156	`cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));`
153	`cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));`	157	`cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));`
154	`cv::Mat P1 = cv::Mat(calibration.K1) * temp;`	158	`cv::Mat P1 = cv::Mat(calibration.K1) * temp;`
155		159
156	`cv::Mat QMatHomogenous, QMat;`	160	`cv::Mat QMatHomogenous, QMat;`
157	`cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);`	161	`cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);`
158	`cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);`	162	`cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);`
159	`cvtools::matToPoints3f(QMat, Q);`	163	`cvtools::matToPoints3f(QMat, Q);`
160	`}`	164	`}`
161		165