triangulation.h
2.23 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
#ifndef TRIANGULATION_H
#define TRIANGULATION_H
#include <stdio.h>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#define NOMINMAX ///< needed for OpenCV functionning
#ifdef _WIN32
#include <windows.h> // required by 1394camapi.h
#include <strsafe.h> // required by 1394camapi.h
#endif
#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <list>
#include <set>
#include <cstdlib>
#include <cmath>
//#include <core/core.hpp>
#include "transformations.h"
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/io.h>
#include <pcl/visualization/pcl_plotter.h>
#undef max
class Triangulator
{
public:
Triangulator();
Triangulator(int method);
//! Possible triangulation methods :
enum { LINEAR_LS, LINEAR_EIGEN, ITERATIVE_LS, ITERATIVE_EIGEN, DIRECT_AFFINE};
void setTriangulationMethod(int method);
double getReprojectionError();
void triangulate(const cv::Mat & W,
const cv::Mat & P,
pcl::PointCloud<pcl::PointXYZ> & pointCloud);
/*void triangulatePoints(const std::vector<cv::KeyPoint>& pt_set1,
const std::vector<cv::KeyPoint>& pt_set2,
const cv::Mat&Kinv,
const cv::Matx34d& P,
const cv::Matx34d& P1,
std::vector<pcl::PointXYZ>& pointcloud);*/
pcl::PointCloud<pcl::PointXYZ> triangulatePoints_LinearLS(const cv::Mat & Wf, // Full measurement matrix 2i x p
const cv::Mat & Pm // Camera matrices 3i x 4
); // 4 x p
void triangulateAffine(const cv::Mat & W,
const cv::Mat & P,
pcl::PointCloud<pcl::PointXYZ> & pointCloud);
private:
int triangulationMethod;
double reprojectionError;
//cv::Mat_<double> linearLS( cv::Point3d u, cv::Matx34d P, cv::Point3d u1, cv::Matx34d P1);
//cv::Mat_<double> linearEigen( cv::Point3d u, cv::Matx34d P, cv::Point3d u1, cv::Matx34d P1);
//cv::Mat_<double> iterativeLSandEigen( cv::Point3d u, cv::Matx34d P, cv::Point3d u1, cv::Matx34d P1);
};
///@}
#endif