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
00041 #ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
00042 #define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
00043
00045 template <typename PointSource, typename PointTarget, typename Scalar> bool
00046 pcl::registration::CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::initCompute ()
00047 {
00048
00049 target_cloud_updated_ = false;
00050 if (!CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ())
00051 return (false);
00052
00054 if (!target_->isOrganized ())
00055 {
00056 PCL_WARN ("[pcl::registration::%s::initCompute] Target cloud is not organized.\n", getClassName ().c_str ());
00057 return (false);
00058 }
00059
00061 projection_matrix_ (0, 0) = fx_;
00062 projection_matrix_ (1, 1) = fy_;
00063 projection_matrix_ (0, 2) = cx_;
00064 projection_matrix_ (1, 2) = cy_;
00065
00066 return (true);
00067 }
00068
00070 template <typename PointSource, typename PointTarget, typename Scalar> void
00071 pcl::registration::CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::determineCorrespondences (
00072 pcl::Correspondences &correspondences,
00073 double max_distance)
00074 {
00075 if (!initCompute ())
00076 return;
00077
00078 correspondences.resize (indices_->size ());
00079 size_t c_index = 0;
00080
00081 for (std::vector<int>::const_iterator src_it = indices_->begin (); src_it != indices_->end (); ++src_it)
00082 {
00083 if (isFinite (input_->points[*src_it]))
00084 {
00085 Eigen::Vector4f p_src (src_to_tgt_transformation_ * input_->points[*src_it].getVector4fMap ());
00086 Eigen::Vector3f p_src3 (p_src[0], p_src[1], p_src[2]);
00087 Eigen::Vector3f uv (projection_matrix_ * p_src3);
00088
00090 if (uv[2] <= 0)
00091 continue;
00092
00093 int u = static_cast<int> (uv[0] / uv[2]);
00094 int v = static_cast<int> (uv[1] / uv[2]);
00095
00096 if (u >= 0 && u < static_cast<int> (target_->width) &&
00097 v >= 0 && v < static_cast<int> (target_->height))
00098 {
00099 const PointTarget &pt_tgt = target_->at (u, v);
00100 if (!isFinite (pt_tgt))
00101 continue;
00103 if (fabs (uv[2] - pt_tgt.z) > depth_threshold_)
00104 continue;
00105
00106 double dist = (p_src3 - pt_tgt.getVector3fMap ()).norm ();
00107 if (dist < max_distance)
00108 correspondences[c_index++] = pcl::Correspondence (*src_it, v * target_->width + u, static_cast<float> (dist));
00109 }
00110 }
00111 }
00112
00113 correspondences.resize (c_index);
00114 }
00115
00117 template <typename PointSource, typename PointTarget, typename Scalar> void
00118 pcl::registration::CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::determineReciprocalCorrespondences (
00119 pcl::Correspondences &correspondences,
00120 double max_distance)
00121 {
00122
00123 determineCorrespondences (correspondences, max_distance);
00124 }
00125
00126 #endif // PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_
00127