91 if (cloud->height == 1 || cloud->width == 1)
93 PCL_ERROR (
"[pcl::estimateProjectionMatrix] Input dataset is not organized!\n");
97 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>
A = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
98 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>
B = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
99 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> C = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
100 Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>
D = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
110 if (std::isfinite (point.x))
112 Scalar
xx = point.x * point.x;
113 Scalar
xy = point.x * point.y;
114 Scalar
xz = point.x * point.z;
115 Scalar
yy = point.y * point.y;
116 Scalar
yz = point.y * point.z;
117 Scalar
zz = point.z * point.z;
120 A.coeffRef (0) +=
xx;
121 A.coeffRef (1) +=
xy;
122 A.coeffRef (2) +=
xz;
123 A.coeffRef (3) += point.x;
125 A.coeffRef (5) +=
yy;
126 A.coeffRef (6) +=
yz;
127 A.coeffRef (7) += point.y;
129 A.coeffRef (10) +=
zz;
130 A.coeffRef (11) += point.z;
131 A.coeffRef (15) += 1.0;
136 B.coeffRef (3) -= point.x *
static_cast<double>(
xIdx);
140 B.coeffRef (7) -= point.y *
static_cast<double>(
xIdx);
143 B.coeffRef (11) -= point.z *
static_cast<double>(
xIdx);
145 B.coeffRef (15) -=
xIdx;
147 C.coeffRef (0) -=
xx *
yIdx;
148 C.coeffRef (1) -=
xy *
yIdx;
149 C.coeffRef (2) -=
xz *
yIdx;
150 C.coeffRef (3) -= point.x *
static_cast<double>(
yIdx);
152 C.coeffRef (5) -=
yy *
yIdx;
153 C.coeffRef (6) -=
yz *
yIdx;
154 C.coeffRef (7) -= point.y *
static_cast<double>(
yIdx);
156 C.coeffRef (10) -=
zz *
yIdx;
157 C.coeffRef (11) -= point.z *
static_cast<double>(
yIdx);
159 C.coeffRef (15) -=
yIdx;
164 D.coeffRef (3) += point.x *
xx_yy;
168 D.coeffRef (7) += point.y *
xx_yy;
171 D.coeffRef (11) += point.z *
xx_yy;
184 Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> X = Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor>::Zero ();
185 X.topLeftCorner<4,4> ().
matrix () =
A;
186 X.block<4,4> (0, 8).
matrix () =
B;
187 X.block<4,4> (8, 0).
matrix () =
B;
188 X.block<4,4> (4, 4).
matrix () =
A;
189 X.block<4,4> (4, 8).
matrix () = C;
190 X.block<4,4> (8, 4).
matrix () = C;
191 X.block<4,4> (8, 8).
matrix () =
D;
193 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> >
ei_symm (X);
double estimateProjectionMatrix(typename pcl::PointCloud< PointT >::ConstPtr cloud, Eigen::Matrix< float, 3, 4, Eigen::RowMajor > &projection_matrix, const Indices &indices)
Estimates the projection matrix P = K * (R|-R*t) from organized point clouds, with K = [[fx,...