18 #ifndef PIC_COMPUTER_VISION_TRIANGULATION_HPP 19 #define PIC_COMPUTER_VISION_TRIANGULATION_HPP 25 #include "../base.hpp" 27 #include "../image.hpp" 29 #include "../util/math.hpp" 31 #include "../util/eigen_util.hpp" 33 #include "../computer_vision/nelder_mead_opt_triangulation.hpp" 35 #ifndef PIC_DISABLE_EIGEN 37 #ifndef PIC_EIGEN_NOT_BUNDLED 38 #include "../externals/Eigen/Dense" 40 #include <Eigen/Dense> 47 #ifndef PIC_DISABLE_EIGEN 61 Eigen::Vector3d r_0 = Eigen::Vector3d(R(0, 0), R(0, 1), R(0, 2));
62 Eigen::Vector3d r_2 = Eigen::Vector3d(R(2, 0), R(2, 1), R(2, 2));
64 Eigen::Vector3d tmp = r_0 - point_1[0] * r_2;
66 ret[2] = tmp.dot(t) / tmp.dot(point_0);
68 ret[0] = point_0[0] * ret[2];
69 ret[1] = point_0[1] * ret[2];
84 Eigen::Matrix34d &M0, Eigen::Matrix34d &M1,
int maxIter = 100)
86 Eigen::Vector4d M0_row[3], M1_row[3];
88 for(
int i = 0; i < 3; i++) {
89 M0_row[i] = Eigen::Vector4d(M0(i, 0), M0(i, 1), M0(i, 2), M0(i, 3));
90 M1_row[i] = Eigen::Vector4d(M1(i, 0), M1(i, 1), M1(i, 2), M1(i, 3));
95 double weight0_prev = 1.0;
98 double weight1_prev = 1.0;
102 Eigen::Vector4d A0 = (M0_row[0] - point_0[0] * M0_row[2]) / weight0;
103 Eigen::Vector4d A1 = (M0_row[1] - point_0[1] * M0_row[2]) / weight0;
105 Eigen::Vector4d A2 = (M1_row[0] - point_1[0] * M1_row[2]) / weight1;
106 Eigen::Vector4d A3 = (M1_row[1] - point_1[1] * M1_row[2]) / weight1;
108 Eigen::MatrixXd A(4, 4);
109 for(
int i = 0; i < 4; i++) {
116 Eigen::JacobiSVD< Eigen::MatrixXd > svdA(A, Eigen::ComputeFullV);
117 Eigen::MatrixXd V = svdA.matrixV();
118 int n = int(V.cols()) - 1;
126 weight0_prev = weight0;
127 weight1_prev = weight1;
129 weight0 = x.dot(M0_row[2]);
130 weight1 = x.dot(M1_row[2]);
132 double d0 = weight0_prev - weight0;
133 double d1 = weight1_prev - weight1;
134 double err = sqrt(d0 * d0 + d1 * d1);
144 printf(
"triangulationHartleySturm's Iterations: %d\n",j);
161 Eigen::Matrix34d &M1,
162 std::vector< Eigen::Vector2f > &m0f,
163 std::vector< Eigen::Vector2f > &m1f,
164 std::vector< Eigen::Vector3d > &points_3d,
165 std::vector< unsigned char > &colors,
171 if(m0f.size() != m1f.size()) {
176 for(
unsigned int i = 0; i < m0f.size(); i++) {
178 Eigen::Vector3d p0 = Eigen::Vector3d(m0f[i][0], m0f[i][1], 1.0);
179 Eigen::Vector3d p1 = Eigen::Vector3d(m1f[i][0], m1f[i][1], 1.0);
185 nmTri.
update(m0f[i], m1f[i]);
186 double tmpp[] = {point[0], point[1], point[2]};
188 nmTri.
run(tmpp, 3, 1e-9f, 10000, &out[0]);
191 points_3d.push_back(Eigen::Vector3d(out[0], out[1], out[2]));
194 float *color0 = (*img0)(int(m0f[i][0]), int(m0f[i][1]));
195 float *color1 = (*img1)(int(m1f[i][0]), int(m1f[i][1]));
197 for(
int j = 0; j < img0->channels; j++) {
198 float c_mean = (color0[j] + color1[j]) * 0.5f;
199 c_mean =
CLAMPi(c_mean, 0.0f, 1.0f);
200 unsigned char c = int(c_mean * 255.0f);
207 #endif // PIC_DISABLE_EIGEN 211 #endif // PIC_COMPUTER_VISION_TRIANGULATION_HPP PIC_INLINE Eigen::Vector4d triangulationHartleySturm(Eigen::Vector3d &point_0, Eigen::Vector3d &point_1, Eigen::Matrix34d &M0, Eigen::Matrix34d &M1, int maxIter=100)
triangulationHartl Sturm
Definition: triangulation.hpp:83
PIC_INLINE Eigen::Vector3d triangulationLonguetHiggins(Eigen::Vector3d &point_0, Eigen::Vector3d &point_1, Eigen::Matrix3d &R, Eigen::Vector3d &t)
triangulationLonguetHiggins computes triangulation using Longuet-Higgins equations.
Definition: triangulation.hpp:57
PIC_INLINE void triangulationPoints(Eigen::Matrix34d &M0, Eigen::Matrix34d &M1, std::vector< Eigen::Vector2f > &m0f, std::vector< Eigen::Vector2f > &m1f, std::vector< Eigen::Vector3d > &points_3d, std::vector< unsigned char > &colors, Image *img0=NULL, Image *img1=NULL, bool bColor=false)
triangulationPoints
Definition: triangulation.hpp:160
virtual Scalar * run(Scalar *x_start, unsigned int n, Scalar epsilon=1e-4f, int max_iterations=1000, Scalar *x=NULL)
Definition: nelder_mead_opt_base.hpp:333
#define PIC_INLINE
Definition: base.hpp:33
The Image class stores an image as buffer of float.
Definition: image.hpp:60
void update(Eigen::Vector2f &p0, Eigen::Vector2f &p1)
update
Definition: nelder_mead_opt_triangulation.hpp:69
#define CLAMPi(x, a, b)
Definition: math.hpp:81
Definition: bilateral_separation.hpp:25
Definition: nelder_mead_opt_triangulation.hpp:36