Blame view
src/triangulation.cpp
2.73 KB
b0bb08d1c
init
|
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 |
#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)));
}
}
|