triangulation.cpp
2.73 KB
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
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
#include "triangulation.h"
Triangulator::Triangulator(){
triangulationMethod = 0; //Default method - linearLS
reprojectionError = 0;
}
Triangulator::Triangulator(int method){
triangulationMethod = method;
reprojectionError = 0;
}
void Triangulator::setTriangulationMethod (int method){
triangulationMethod = method;
}
double Triangulator::getReprojectionError(){
return reprojectionError;
}
void Triangulator::triangulate(const cv::Mat &W, const cv::Mat &P, pcl::PointCloud<pcl::PointXYZ> &pointCloud)
{
switch(triangulationMethod){
case ITERATIVE_LS : break;
case ITERATIVE_EIGEN : break;
case DIRECT_AFFINE : triangulateAffine(W,P,pointCloud); break;
default : break;
}
}
pcl::PointCloud<pcl::PointXYZ> Triangulator::triangulatePoints_LinearLS(const cv::Mat &Wf, const cv::Mat &Pm)
{
//std::cout<<Wf.size()<<std::endl;
//std::cout<<Pm<<std::endl;
int nPts = (int) Wf.cols ;
int nCams = (int) Wf.rows / 2;
cv::Mat A;
cv::Mat B;
cv::Mat_<double> pt;
cv::Mat temp;
pcl::PointCloud<pcl::PointXYZ> cloud;
//cloud.is_dense = false;
for ( int p = 0 ; p < nPts ; p++ ){
temp = cv::Mat(0,4,cv::DataType<double>::type);
for ( int i = 0 ; i < nCams ; i++ ){
temp.push_back( Wf.at<double> ( 2*i + 0, p) * Pm.row( 3*i+2 ) - Pm.row( 3*i + 0 ));
temp.push_back( Wf.at<double> ( 2*i + 1, p) * Pm.row( 3*i+2 ) - Pm.row( 3*i + 1 ));
}
A = temp.colRange(0,3);
B = -1*temp.col(3);
cv::solve(A,B,pt,cv::DECOMP_SVD);
//cout<< pt<<endl;
cloud.push_back(pcl::PointXYZ(pt(0),pt(1),pt(2)));
//cloud.points[p].x = pt(0);
//cloud.points[p].y = pt(1);
//cloud.points[p].z = pt(2);
//std::cout<<pt(2)<<std::endl;
temp.release();
}
return cloud;
}
void Triangulator::triangulateAffine(const cv::Mat &W, const cv::Mat &P, pcl::PointCloud<pcl::PointXYZ> &pointCloud)
{
if (W.rows%3 == 0){
}
cv::Mat q1x = W.row(0);
cv::Mat q1y = W.row(1);
cv::Mat q2x;
if (W.rows%3 == 0) q2x = W.row(3);
else q2x = W.row(2);
double t1,rho,t2;
//cv::Mat R = rotmFromCameraMat(P.rowRange(0,0+3));
cv::Mat R = rotmat::fromCameraMatrixAffine(P.rowRange(3,3+3).clone());
rotmat::decomposeEulerZYZ(R,t1,rho,t2);
std::cout << rho/PI*180;
//rho = 3.0/180.0*PI;
double cosRho = cos(rho);
double sinRho = sin(rho);
cv::Mat q1z = (q1x * cosRho - q2x) / sinRho;
for (auto i = 0; i < q1x.cols; i++){
pointCloud.push_back(pcl::PointXYZ(
q1x.at<double>(0,i),
q1y.at<double>(0,i),
q1z.at<double>(0,i)));
}
}