Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_ 00042 #define PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_ 00043 00044 /////////////////////////////////////////////////////////////////////////////////////////// 00045 template <typename PointSource, typename PointTarget, typename Scalar> bool 00046 pcl::registration::CorrespondenceEstimationOrganizedProjection<PointSource, PointTarget, Scalar>::initCompute () 00047 { 00048 // Set the target_cloud_updated_ variable to true, so that the kd-tree is not built - it is not needed for this class 00049 target_cloud_updated_ = false; 00050 if (!CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ()) 00051 return (false); 00052 00053 /// Check if the target cloud is organized 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 00060 /// Put the projection matrix together 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 00069 /////////////////////////////////////////////////////////////////////////////////////////// 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 00089 /// Check if the point was behind the camera 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; 00102 /// Check if the depth difference is larger than the threshold 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 00116 /////////////////////////////////////////////////////////////////////////////////////////// 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 // Call the normal determineCorrespondences (...), as doing it both ways will not improve the results 00123 determineCorrespondences (correspondences, max_distance); 00124 } 00125 00126 #endif // PCL_REGISTRATION_CORRESPONDENCE_ESTIMATION_ORGANIZED_PROJECTION_IMPL_HPP_ 00127