00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
00040 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_
00041 #include <pcl/cloud_iterator.h>
00042
00044 template <typename PointSource, typename PointTarget, typename Scalar> inline void
00045 pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
00046 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
00047 const pcl::PointCloud<PointTarget> &cloud_tgt,
00048 Matrix4 &transformation_matrix) const
00049 {
00050 size_t nr_points = cloud_src.points.size ();
00051 if (cloud_tgt.points.size () != nr_points)
00052 {
00053 PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", nr_points, cloud_tgt.points.size ());
00054 return;
00055 }
00056
00057 if (weights_.size () != nr_points)
00058 {
00059 PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
00060 return;
00061 }
00062
00063 ConstCloudIterator<PointSource> source_it (cloud_src);
00064 ConstCloudIterator<PointTarget> target_it (cloud_tgt);
00065 typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
00066 estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
00067 }
00068
00070 template <typename PointSource, typename PointTarget, typename Scalar> void
00071 pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
00072 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
00073 const std::vector<int> &indices_src,
00074 const pcl::PointCloud<PointTarget> &cloud_tgt,
00075 Matrix4 &transformation_matrix) const
00076 {
00077 size_t nr_points = indices_src.size ();
00078 if (cloud_tgt.points.size () != nr_points)
00079 {
00080 PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ());
00081 return;
00082 }
00083
00084 if (weights_.size () != nr_points)
00085 {
00086 PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
00087 return;
00088 }
00089
00090
00091 ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
00092 ConstCloudIterator<PointTarget> target_it (cloud_tgt);
00093 typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
00094 estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
00095 }
00096
00097
00099 template <typename PointSource, typename PointTarget, typename Scalar> inline void
00100 pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
00101 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
00102 const std::vector<int> &indices_src,
00103 const pcl::PointCloud<PointTarget> &cloud_tgt,
00104 const std::vector<int> &indices_tgt,
00105 Matrix4 &transformation_matrix) const
00106 {
00107 size_t nr_points = indices_src.size ();
00108 if (indices_tgt.size () != nr_points)
00109 {
00110 PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), indices_tgt.size ());
00111 return;
00112 }
00113
00114 if (weights_.size () != nr_points)
00115 {
00116 PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
00117 return;
00118 }
00119
00120 ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
00121 ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
00122 typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
00123 estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
00124 }
00125
00127 template <typename PointSource, typename PointTarget, typename Scalar> inline void
00128 pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
00129 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
00130 const pcl::PointCloud<PointTarget> &cloud_tgt,
00131 const pcl::Correspondences &correspondences,
00132 Matrix4 &transformation_matrix) const
00133 {
00134 ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
00135 ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
00136 std::vector<Scalar> weights (correspondences.size ());
00137 for (size_t i = 0; i < correspondences.size (); ++i)
00138 weights[i] = correspondences[i].weight;
00139 typename std::vector<Scalar>::const_iterator weights_it = weights.begin ();
00140 estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
00141 }
00142
00144 template <typename PointSource, typename PointTarget, typename Scalar> inline void
00145 pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
00146 constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
00147 const double & tx, const double & ty, const double & tz,
00148 Matrix4 &transformation_matrix) const
00149 {
00150
00151 transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero ();
00152 transformation_matrix (0, 0) = static_cast<Scalar> ( cos (gamma) * cos (beta));
00153 transformation_matrix (0, 1) = static_cast<Scalar> (-sin (gamma) * cos (alpha) + cos (gamma) * sin (beta) * sin (alpha));
00154 transformation_matrix (0, 2) = static_cast<Scalar> ( sin (gamma) * sin (alpha) + cos (gamma) * sin (beta) * cos (alpha));
00155 transformation_matrix (1, 0) = static_cast<Scalar> ( sin (gamma) * cos (beta));
00156 transformation_matrix (1, 1) = static_cast<Scalar> ( cos (gamma) * cos (alpha) + sin (gamma) * sin (beta) * sin (alpha));
00157 transformation_matrix (1, 2) = static_cast<Scalar> (-cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * cos (alpha));
00158 transformation_matrix (2, 0) = static_cast<Scalar> (-sin (beta));
00159 transformation_matrix (2, 1) = static_cast<Scalar> ( cos (beta) * sin (alpha));
00160 transformation_matrix (2, 2) = static_cast<Scalar> ( cos (beta) * cos (alpha));
00161
00162 transformation_matrix (0, 3) = static_cast<Scalar> (tx);
00163 transformation_matrix (1, 3) = static_cast<Scalar> (ty);
00164 transformation_matrix (2, 3) = static_cast<Scalar> (tz);
00165 transformation_matrix (3, 3) = static_cast<Scalar> (1);
00166 }
00167
00169 template <typename PointSource, typename PointTarget, typename Scalar> inline void
00170 pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
00171 estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
00172 ConstCloudIterator<PointTarget>& target_it,
00173 typename std::vector<Scalar>::const_iterator& weights_it,
00174 Matrix4 &transformation_matrix) const
00175 {
00176 typedef Eigen::Matrix<double, 6, 1> Vector6d;
00177 typedef Eigen::Matrix<double, 6, 6> Matrix6d;
00178
00179 Matrix6d ATA;
00180 Vector6d ATb;
00181 ATA.setZero ();
00182 ATb.setZero ();
00183
00184 while (source_it.isValid () && target_it.isValid ())
00185 {
00186 if (!pcl_isfinite (source_it->x) ||
00187 !pcl_isfinite (source_it->y) ||
00188 !pcl_isfinite (source_it->z) ||
00189 !pcl_isfinite (source_it->normal_x) ||
00190 !pcl_isfinite (source_it->normal_y) ||
00191 !pcl_isfinite (source_it->normal_z) ||
00192 !pcl_isfinite (target_it->x) ||
00193 !pcl_isfinite (target_it->y) ||
00194 !pcl_isfinite (target_it->z) ||
00195 !pcl_isfinite (target_it->normal_x) ||
00196 !pcl_isfinite (target_it->normal_y) ||
00197 !pcl_isfinite (target_it->normal_z))
00198 {
00199 ++ source_it;
00200 ++ target_it;
00201 ++ weights_it;
00202 continue;
00203 }
00204
00205 const float & sx = source_it->x;
00206 const float & sy = source_it->y;
00207 const float & sz = source_it->z;
00208 const float & dx = target_it->x;
00209 const float & dy = target_it->y;
00210 const float & dz = target_it->z;
00211 const float & nx = target_it->normal[0] * (*weights_it);
00212 const float & ny = target_it->normal[1] * (*weights_it);
00213 const float & nz = target_it->normal[2] * (*weights_it);
00214
00215 double a = nz*sy - ny*sz;
00216 double b = nx*sz - nz*sx;
00217 double c = ny*sx - nx*sy;
00218
00219
00220
00221
00222
00223
00224
00225
00226 ATA.coeffRef (0) += a * a;
00227 ATA.coeffRef (1) += a * b;
00228 ATA.coeffRef (2) += a * c;
00229 ATA.coeffRef (3) += a * nx;
00230 ATA.coeffRef (4) += a * ny;
00231 ATA.coeffRef (5) += a * nz;
00232 ATA.coeffRef (7) += b * b;
00233 ATA.coeffRef (8) += b * c;
00234 ATA.coeffRef (9) += b * nx;
00235 ATA.coeffRef (10) += b * ny;
00236 ATA.coeffRef (11) += b * nz;
00237 ATA.coeffRef (14) += c * c;
00238 ATA.coeffRef (15) += c * nx;
00239 ATA.coeffRef (16) += c * ny;
00240 ATA.coeffRef (17) += c * nz;
00241 ATA.coeffRef (21) += nx * nx;
00242 ATA.coeffRef (22) += nx * ny;
00243 ATA.coeffRef (23) += nx * nz;
00244 ATA.coeffRef (28) += ny * ny;
00245 ATA.coeffRef (29) += ny * nz;
00246 ATA.coeffRef (35) += nz * nz;
00247
00248 double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
00249 ATb.coeffRef (0) += a * d;
00250 ATb.coeffRef (1) += b * d;
00251 ATb.coeffRef (2) += c * d;
00252 ATb.coeffRef (3) += nx * d;
00253 ATb.coeffRef (4) += ny * d;
00254 ATb.coeffRef (5) += nz * d;
00255
00256 ++ source_it;
00257 ++ target_it;
00258 ++ weights_it;
00259 }
00260
00261 ATA.coeffRef (6) = ATA.coeff (1);
00262 ATA.coeffRef (12) = ATA.coeff (2);
00263 ATA.coeffRef (13) = ATA.coeff (8);
00264 ATA.coeffRef (18) = ATA.coeff (3);
00265 ATA.coeffRef (19) = ATA.coeff (9);
00266 ATA.coeffRef (20) = ATA.coeff (15);
00267 ATA.coeffRef (24) = ATA.coeff (4);
00268 ATA.coeffRef (25) = ATA.coeff (10);
00269 ATA.coeffRef (26) = ATA.coeff (16);
00270 ATA.coeffRef (27) = ATA.coeff (22);
00271 ATA.coeffRef (30) = ATA.coeff (5);
00272 ATA.coeffRef (31) = ATA.coeff (11);
00273 ATA.coeffRef (32) = ATA.coeff (17);
00274 ATA.coeffRef (33) = ATA.coeff (23);
00275 ATA.coeffRef (34) = ATA.coeff (29);
00276
00277
00278 Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
00279
00280
00281 constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
00282 }
00283 #endif