38 #ifndef __PCL_ORGANIZED_PROJECTION_MATRIX_HPP__
39 #define __PCL_ORGANIZED_PROJECTION_MATRIX_HPP__
41 #include <pcl/cloud_iterator.h>
50 template <
typename MatrixT>
void
53 if (use_upper_triangular && (MatrixT::Flags & Eigen::RowMajorBit))
55 matrix.coeffRef (4) = matrix.coeff (1);
56 matrix.coeffRef (8) = matrix.coeff (2);
57 matrix.coeffRef (9) = matrix.coeff (6);
58 matrix.coeffRef (12) = matrix.coeff (3);
59 matrix.coeffRef (13) = matrix.coeff (7);
60 matrix.coeffRef (14) = matrix.coeff (11);
64 matrix.coeffRef (1) = matrix.coeff (4);
65 matrix.coeffRef (2) = matrix.coeff (8);
66 matrix.coeffRef (6) = matrix.coeff (9);
67 matrix.coeffRef (3) = matrix.coeff (12);
68 matrix.coeffRef (7) = matrix.coeff (13);
69 matrix.coeffRef (11) = matrix.coeff (14);
77 template <
typename Po
intT>
double
80 Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix,
81 const std::vector<int>& indices)
84 typedef double Scalar;
85 projection_matrix.setZero ();
88 PCL_ERROR (
"[pcl::estimateProjectionMatrix] Input dataset is not organized!\n");
92 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> A = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
93 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>
B = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
94 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> C = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
95 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> D = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
104 const PointT& point = *pointIt;
105 if (pcl_isfinite (point.x))
107 Scalar xx = point.x * point.x;
108 Scalar xy = point.x * point.y;
109 Scalar xz = point.x * point.z;
110 Scalar yy = point.y * point.y;
111 Scalar yz = point.y * point.z;
112 Scalar zz = point.z * point.z;
113 Scalar xx_yy = xIdx * xIdx + yIdx * yIdx;
115 A.coeffRef (0) += xx;
116 A.coeffRef (1) += xy;
117 A.coeffRef (2) += xz;
118 A.coeffRef (3) += point.x;
120 A.coeffRef (5) += yy;
121 A.coeffRef (6) += yz;
122 A.coeffRef (7) += point.y;
124 A.coeffRef (10) += zz;
125 A.coeffRef (11) += point.z;
126 A.coeffRef (15) += 1.0;
128 B.coeffRef (0) -= xx * xIdx;
129 B.coeffRef (1) -= xy * xIdx;
130 B.coeffRef (2) -= xz * xIdx;
131 B.coeffRef (3) -= point.x *
static_cast<double>(xIdx);
133 B.coeffRef (5) -= yy * xIdx;
134 B.coeffRef (6) -= yz * xIdx;
135 B.coeffRef (7) -= point.y *
static_cast<double>(xIdx);
137 B.coeffRef (10) -= zz * xIdx;
138 B.coeffRef (11) -= point.z *
static_cast<double>(xIdx);
140 B.coeffRef (15) -= xIdx;
142 C.coeffRef (0) -= xx * yIdx;
143 C.coeffRef (1) -= xy * yIdx;
144 C.coeffRef (2) -= xz * yIdx;
145 C.coeffRef (3) -= point.x *
static_cast<double>(yIdx);
147 C.coeffRef (5) -= yy * yIdx;
148 C.coeffRef (6) -= yz * yIdx;
149 C.coeffRef (7) -= point.y *
static_cast<double>(yIdx);
151 C.coeffRef (10) -= zz * yIdx;
152 C.coeffRef (11) -= point.z *
static_cast<double>(yIdx);
154 C.coeffRef (15) -= yIdx;
156 D.coeffRef (0) += xx * xx_yy;
157 D.coeffRef (1) += xy * xx_yy;
158 D.coeffRef (2) += xz * xx_yy;
159 D.coeffRef (3) += point.x * xx_yy;
161 D.coeffRef (5) += yy * xx_yy;
162 D.coeffRef (6) += yz * xx_yy;
163 D.coeffRef (7) += point.y * xx_yy;
165 D.coeffRef (10) += zz * xx_yy;
166 D.coeffRef (11) += point.z * xx_yy;
168 D.coeffRef (15) += xx_yy;
179 Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> X = Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor>::Zero ();
180 X.topLeftCorner<4,4> ().matrix () = A;
181 X.block<4,4> (0, 8).matrix () =
B;
182 X.block<4,4> (8, 0).matrix () =
B;
183 X.block<4,4> (4, 4).matrix () = A;
184 X.block<4,4> (4, 8).matrix () = C;
185 X.block<4,4> (8, 4).matrix () = C;
186 X.block<4,4> (8, 8).matrix () = D;
188 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> > ei_symm (X);
189 Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> eigen_vectors = ei_symm.eigenvectors ();
192 Eigen::Matrix<Scalar, 1, 1> residual_sqr = eigen_vectors.col (0).transpose () * X * eigen_vectors.col (0);
194 double residual = residual_sqr.coeff (0);
196 projection_matrix.coeffRef (0) = static_cast <
float> (eigen_vectors.coeff (0));
197 projection_matrix.coeffRef (1) = static_cast <
float> (eigen_vectors.coeff (12));
198 projection_matrix.coeffRef (2) = static_cast <
float> (eigen_vectors.coeff (24));
199 projection_matrix.coeffRef (3) = static_cast <
float> (eigen_vectors.coeff (36));
200 projection_matrix.coeffRef (4) = static_cast <
float> (eigen_vectors.coeff (48));
201 projection_matrix.coeffRef (5) = static_cast <
float> (eigen_vectors.coeff (60));
202 projection_matrix.coeffRef (6) = static_cast <
float> (eigen_vectors.coeff (72));
203 projection_matrix.coeffRef (7) = static_cast <
float> (eigen_vectors.coeff (84));
204 projection_matrix.coeffRef (8) = static_cast <
float> (eigen_vectors.coeff (96));
205 projection_matrix.coeffRef (9) = static_cast <
float> (eigen_vectors.coeff (108));
206 projection_matrix.coeffRef (10) = static_cast <
float> (eigen_vectors.coeff (120));
207 projection_matrix.coeffRef (11) = static_cast <
float> (eigen_vectors.coeff (132));
209 if (projection_matrix.coeff (0) < 0)
210 projection_matrix *= -1.0;
void makeSymmetric(MatrixT &matrix, bool use_upper_triangular=true)
uint32_t width
The point cloud width (if organized as an image-structure).
Iterator class for point clouds with or without given indices.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
unsigned getCurrentPointIndex() const
uint32_t height
The point cloud height (if organized as an image-structure).
double estimateProjectionMatrix(typename pcl::PointCloud< PointT >::ConstPtr cloud, Eigen::Matrix< float, 3, 4, Eigen::RowMajor > &projection_matrix, const std::vector< int > &indices=std::vector< int >())
Estimates the projection matrix P = K * (R|-R*t) from organized point clouds, with K = [[fx...
A point structure representing Euclidean xyz coordinates, and the RGB color.