Go to the documentation of this file.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
00040 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
00041 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
00042
00043 #include <pcl/sample_consensus/sac_model_registration.h>
00044
00046 template <typename PointT> bool
00047 pcl::SampleConsensusModelRegistration<PointT>::isSampleGood (const std::vector<int> &samples) const
00048 {
00049 return ((input_->points[samples[1]].getArray4fMap () - input_->points[samples[0]].getArray4fMap ()).matrix ().squaredNorm () > sample_dist_thresh_ &&
00050 (input_->points[samples[2]].getArray4fMap () - input_->points[samples[0]].getArray4fMap ()).matrix ().squaredNorm () > sample_dist_thresh_ &&
00051 (input_->points[samples[2]].getArray4fMap () - input_->points[samples[1]].getArray4fMap ()).matrix ().squaredNorm () > sample_dist_thresh_);
00052 }
00053
00055 template <typename PointT> bool
00056 pcl::SampleConsensusModelRegistration<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
00057 {
00058 if (!target_)
00059 {
00060 PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeModelCoefficients] No target dataset given!\n");
00061 return (false);
00062 }
00063
00064 if (samples.size () != 3)
00065 return (false);
00066
00067 std::vector<int> indices_tgt (3);
00068 for (int i = 0; i < 3; ++i)
00069 indices_tgt[i] = correspondences_[samples[i]];
00070
00071 estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients);
00072 return (true);
00073 }
00074
00076 template <typename PointT> void
00077 pcl::SampleConsensusModelRegistration<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
00078 {
00079 if (indices_->size () != indices_tgt_->size ())
00080 {
00081 PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
00082 distances.clear ();
00083 return;
00084 }
00085 if (!target_)
00086 {
00087 PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n");
00088 return;
00089 }
00090
00091 if (!isModelValid (model_coefficients))
00092 {
00093 distances.clear ();
00094 return;
00095 }
00096 distances.resize (indices_->size ());
00097
00098
00099 Eigen::Matrix4f transform;
00100 transform.row (0) = model_coefficients.segment<4>(0);
00101 transform.row (1) = model_coefficients.segment<4>(4);
00102 transform.row (2) = model_coefficients.segment<4>(8);
00103 transform.row (3) = model_coefficients.segment<4>(12);
00104
00105 for (size_t i = 0; i < indices_->size (); ++i)
00106 {
00107 Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap ();
00108 pt_src[3] = 1;
00109 Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap ();
00110 pt_tgt[3] = 1;
00111
00112 Eigen::Vector4f p_tr (transform * pt_src);
00113
00114
00115 distances[i] = (p_tr - pt_tgt).norm ();
00116 }
00117 }
00118
00120 template <typename PointT> void
00121 pcl::SampleConsensusModelRegistration<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
00122 {
00123 if (indices_->size () != indices_tgt_->size ())
00124 {
00125 PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
00126 inliers.clear ();
00127 return;
00128 }
00129 if (!target_)
00130 {
00131 PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n");
00132 return;
00133 }
00134
00135 double thresh = threshold * threshold;
00136
00137
00138 if (!isModelValid (model_coefficients))
00139 {
00140 inliers.clear ();
00141 return;
00142 }
00143
00144 inliers.resize (indices_->size ());
00145
00146 Eigen::Matrix4f transform;
00147 transform.row (0) = model_coefficients.segment<4>(0);
00148 transform.row (1) = model_coefficients.segment<4>(4);
00149 transform.row (2) = model_coefficients.segment<4>(8);
00150 transform.row (3) = model_coefficients.segment<4>(12);
00151
00152 int nr_p = 0;
00153 for (size_t i = 0; i < indices_->size (); ++i)
00154 {
00155 Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap ();
00156 pt_src[3] = 1;
00157 Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap ();
00158 pt_tgt[3] = 1;
00159
00160 Eigen::Vector4f p_tr (transform * pt_src);
00161
00162 if ((p_tr - pt_tgt).squaredNorm () < thresh)
00163 inliers[nr_p++] = (*indices_)[i];
00164 }
00165 inliers.resize (nr_p);
00166 }
00167
00169 template <typename PointT> int
00170 pcl::SampleConsensusModelRegistration<PointT>::countWithinDistance (
00171 const Eigen::VectorXf &model_coefficients, const double threshold)
00172 {
00173 if (indices_->size () != indices_tgt_->size ())
00174 {
00175 PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
00176 return (0);
00177 }
00178 if (!target_)
00179 {
00180 PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] No target dataset given!\n");
00181 return (0);
00182 }
00183
00184 double thresh = threshold * threshold;
00185
00186
00187 if (!isModelValid (model_coefficients))
00188 return (0);
00189
00190 Eigen::Matrix4f transform;
00191 transform.row (0) = model_coefficients.segment<4>(0);
00192 transform.row (1) = model_coefficients.segment<4>(4);
00193 transform.row (2) = model_coefficients.segment<4>(8);
00194 transform.row (3) = model_coefficients.segment<4>(12);
00195
00196 int nr_p = 0;
00197 for (size_t i = 0; i < indices_->size (); ++i)
00198 {
00199 Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap ();
00200 pt_src[3] = 1;
00201 Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap ();
00202 pt_tgt[3] = 1;
00203
00204 Eigen::Vector4f p_tr (transform * pt_src);
00205
00206 if ((p_tr - pt_tgt).squaredNorm () < thresh)
00207 nr_p++;
00208 }
00209 return (nr_p);
00210 }
00211
00213 template <typename PointT> void
00214 pcl::SampleConsensusModelRegistration<PointT>::optimizeModelCoefficients (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
00215 {
00216 if (indices_->size () != indices_tgt_->size ())
00217 {
00218 PCL_ERROR ("[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
00219 optimized_coefficients = model_coefficients;
00220 return;
00221 }
00222
00223
00224 if (!isModelValid (model_coefficients) || !target_)
00225 {
00226 optimized_coefficients = model_coefficients;
00227 return;
00228 }
00229
00230 std::vector<int> indices_src (inliers.size ());
00231 std::vector<int> indices_tgt (inliers.size ());
00232 for (size_t i = 0; i < inliers.size (); ++i)
00233 {
00234
00235 indices_src[i] = (*indices_)[inliers[i]];
00236 indices_tgt[i] = (*indices_tgt_)[inliers[i]];
00237 }
00238
00239 estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients);
00240 }
00241
00243 template <typename PointT> void
00244 pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD (
00245 const pcl::PointCloud<PointT> &cloud_src,
00246 const std::vector<int> &indices_src,
00247 const pcl::PointCloud<PointT> &cloud_tgt,
00248 const std::vector<int> &indices_tgt,
00249 Eigen::VectorXf &transform)
00250 {
00251 transform.resize (16);
00252 Eigen::Vector4f centroid_src, centroid_tgt;
00253
00254 compute3DCentroid (cloud_src, indices_src, centroid_src);
00255 compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);
00256
00257
00258 Eigen::MatrixXf cloud_src_demean;
00259 demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
00260
00261 Eigen::MatrixXf cloud_tgt_demean;
00262 demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);
00263
00264
00265 Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
00266
00267
00268 Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
00269 Eigen::Matrix3f u = svd.matrixU ();
00270 Eigen::Matrix3f v = svd.matrixV ();
00271
00272
00273 if (u.determinant () * v.determinant () < 0)
00274 {
00275 for (int x = 0; x < 3; ++x)
00276 v (x, 2) *= -1;
00277 }
00278
00279 Eigen::Matrix3f R = v * u.transpose ();
00280
00281
00282 transform.segment<3> (0) = R.row (0); transform[12] = 0;
00283 transform.segment<3> (4) = R.row (1); transform[13] = 0;
00284 transform.segment<3> (8) = R.row (2); transform[14] = 0;
00285
00286 Eigen::Vector3f t = centroid_tgt.head<3> () - R * centroid_src.head<3> ();
00287 transform[3] = t[0]; transform[7] = t[1]; transform[11] = t[2]; transform[15] = 1.0;
00288 }
00289
00290 #define PCL_INSTANTIATE_SampleConsensusModelRegistration(T) template class PCL_EXPORTS pcl::SampleConsensusModelRegistration<T>;
00291
00292 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
00293