40 #include <pcl/common/projection_matrix.h>
41 #include <pcl/cloud_iterator.h>
53 template <
typename MatrixT>
void
56 if (use_upper_triangular && (MatrixT::Flags & Eigen::RowMajorBit))
58 matrix.coeffRef (4) = matrix.coeff (1);
59 matrix.coeffRef (8) = matrix.coeff (2);
60 matrix.coeffRef (9) = matrix.coeff (6);
61 matrix.coeffRef (12) = matrix.coeff (3);
62 matrix.coeffRef (13) = matrix.coeff (7);
63 matrix.coeffRef (14) = matrix.coeff (11);
67 matrix.coeffRef (1) = matrix.coeff (4);
68 matrix.coeffRef (2) = matrix.coeff (8);
69 matrix.coeffRef (6) = matrix.coeff (9);
70 matrix.coeffRef (3) = matrix.coeff (12);
71 matrix.coeffRef (7) = matrix.coeff (13);
72 matrix.coeffRef (11) = matrix.coeff (14);
80 template <
typename Po
intT>
double
83 Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix,
84 const std::vector<int>& indices)
87 using Scalar = double;
88 projection_matrix.setZero ();
91 PCL_ERROR (
"[pcl::estimateProjectionMatrix] Input dataset is not organized!\n");
95 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> A = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
96 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>
B = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
97 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> C = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
98 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> D = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
107 const PointT& point = *pointIt;
108 if (std::isfinite (point.x))
110 Scalar xx = point.x * point.x;
111 Scalar xy = point.x * point.y;
112 Scalar xz = point.x * point.z;
113 Scalar yy = point.y * point.y;
114 Scalar yz = point.y * point.z;
115 Scalar zz = point.z * point.z;
116 Scalar xx_yy = xIdx * xIdx + yIdx * yIdx;
118 A.coeffRef (0) += xx;
119 A.coeffRef (1) += xy;
120 A.coeffRef (2) += xz;
121 A.coeffRef (3) += point.x;
123 A.coeffRef (5) += yy;
124 A.coeffRef (6) += yz;
125 A.coeffRef (7) += point.y;
127 A.coeffRef (10) += zz;
128 A.coeffRef (11) += point.z;
129 A.coeffRef (15) += 1.0;
131 B.coeffRef (0) -= xx * xIdx;
132 B.coeffRef (1) -= xy * xIdx;
133 B.coeffRef (2) -= xz * xIdx;
134 B.coeffRef (3) -= point.x *
static_cast<double>(xIdx);
136 B.coeffRef (5) -= yy * xIdx;
137 B.coeffRef (6) -= yz * xIdx;
138 B.coeffRef (7) -= point.y *
static_cast<double>(xIdx);
140 B.coeffRef (10) -= zz * xIdx;
141 B.coeffRef (11) -= point.z *
static_cast<double>(xIdx);
143 B.coeffRef (15) -= xIdx;
145 C.coeffRef (0) -= xx * yIdx;
146 C.coeffRef (1) -= xy * yIdx;
147 C.coeffRef (2) -= xz * yIdx;
148 C.coeffRef (3) -= point.x *
static_cast<double>(yIdx);
150 C.coeffRef (5) -= yy * yIdx;
151 C.coeffRef (6) -= yz * yIdx;
152 C.coeffRef (7) -= point.y *
static_cast<double>(yIdx);
154 C.coeffRef (10) -= zz * yIdx;
155 C.coeffRef (11) -= point.z *
static_cast<double>(yIdx);
157 C.coeffRef (15) -= yIdx;
159 D.coeffRef (0) += xx * xx_yy;
160 D.coeffRef (1) += xy * xx_yy;
161 D.coeffRef (2) += xz * xx_yy;
162 D.coeffRef (3) += point.x * xx_yy;
164 D.coeffRef (5) += yy * xx_yy;
165 D.coeffRef (6) += yz * xx_yy;
166 D.coeffRef (7) += point.y * xx_yy;
168 D.coeffRef (10) += zz * xx_yy;
169 D.coeffRef (11) += point.z * xx_yy;
171 D.coeffRef (15) += xx_yy;
182 Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> X = Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor>::Zero ();
183 X.topLeftCorner<4,4> ().matrix () = A;
184 X.block<4,4> (0, 8).matrix () =
B;
185 X.block<4,4> (8, 0).matrix () =
B;
186 X.block<4,4> (4, 4).matrix () = A;
187 X.block<4,4> (4, 8).matrix () = C;
188 X.block<4,4> (8, 4).matrix () = C;
189 X.block<4,4> (8, 8).matrix () = D;
191 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> > ei_symm (X);
192 Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> eigen_vectors = ei_symm.eigenvectors ();
195 Eigen::Matrix<Scalar, 1, 1> residual_sqr = eigen_vectors.col (0).transpose () * X * eigen_vectors.col (0);
197 double residual = residual_sqr.coeff (0);
199 projection_matrix.coeffRef (0) = static_cast <
float> (eigen_vectors.coeff (0));
200 projection_matrix.coeffRef (1) = static_cast <
float> (eigen_vectors.coeff (12));
201 projection_matrix.coeffRef (2) = static_cast <
float> (eigen_vectors.coeff (24));
202 projection_matrix.coeffRef (3) = static_cast <
float> (eigen_vectors.coeff (36));
203 projection_matrix.coeffRef (4) = static_cast <
float> (eigen_vectors.coeff (48));
204 projection_matrix.coeffRef (5) = static_cast <
float> (eigen_vectors.coeff (60));
205 projection_matrix.coeffRef (6) = static_cast <
float> (eigen_vectors.coeff (72));
206 projection_matrix.coeffRef (7) = static_cast <
float> (eigen_vectors.coeff (84));
207 projection_matrix.coeffRef (8) = static_cast <
float> (eigen_vectors.coeff (96));
208 projection_matrix.coeffRef (9) = static_cast <
float> (eigen_vectors.coeff (108));
209 projection_matrix.coeffRef (10) = static_cast <
float> (eigen_vectors.coeff (120));
210 projection_matrix.coeffRef (11) = static_cast <
float> (eigen_vectors.coeff (132));
212 if (projection_matrix.coeff (0) < 0)
213 projection_matrix *= -1.0;
Iterator class for point clouds with or without given indices.
unsigned getCurrentPointIndex() const
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
void makeSymmetric(MatrixT &matrix, bool use_upper_triangular=true)
shared_ptr< const PointCloud< PointT > > ConstPtr
double estimateProjectionMatrix(typename pcl::PointCloud< PointT >::ConstPtr cloud, Eigen::Matrix< float, 3, 4, Eigen::RowMajor > &projection_matrix, const std::vector< int > &indices)
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.