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 namespace pcl {
00039
00040 template <typename FeatureType>
00041 inline void ObjectModelList::freeFeatureListMemory(std::vector<std::vector<std::vector<FeatureType*>*>*>& feature_list)
00042 {
00043 for (unsigned int i=0; i<feature_list.size(); ++i)
00044 {
00045 ObjectModel::freeFeatureListMemory(*feature_list[i]);
00046 delete feature_list[i];
00047 }
00048 feature_list.clear();
00049 }
00050
00051 template <typename FeatureType>
00052 inline void ObjectModelList::getMergedFeatureList(const std::vector<std::vector<std::vector<FeatureType*>*>*>& feature_list,
00053 std::vector<FeatureType*>& merged_feature_list, std::vector<FeatureSource>& feature_sources)
00054 {
00055 merged_feature_list.clear();
00056 for (unsigned int i=0; i<feature_list.size(); ++i)
00057 {
00058 const std::vector<std::vector<FeatureType*>*>& features_per_model = *feature_list[i];
00059 for (unsigned int j=0; j<features_per_model.size(); ++j)
00060 {
00061 const std::vector<FeatureType*>& features_per_view = *features_per_model[j];
00062 for (unsigned int k=0; k<features_per_view.size(); ++k)
00063 {
00064 merged_feature_list.push_back(features_per_view[k]);
00065 feature_sources.push_back(FeatureSource(i, j, k));
00066 }
00067 }
00068 }
00069 }
00070
00071 template <typename FeatureType>
00072 void ObjectModelList::buildKdTree(const std::vector<FeatureType*>& merged_feature_list, KdTree<FeatureType*>& kdtree)
00073 {
00074
00075 if (merged_feature_list.empty())
00076 return;
00077
00078 boost::shared_ptr<PointCloud<FeatureType*> > feature_pc(new PointCloud<FeatureType*>);
00079 for (unsigned int i=0; i<merged_feature_list.size(); ++i)
00080 feature_pc->points.push_back(merged_feature_list[i]);
00081 boost::shared_ptr<typename FeatureType::FeaturePointRepresentation> point_representation(
00082 new typename FeatureType::FeaturePointRepresentation(merged_feature_list[0]->getDescriptorSize()));
00083 kdtree.setPointRepresentation (point_representation);
00084 kdtree.setInputCloud(feature_pc);
00085 }
00086
00087 template <typename FeatureType>
00088 void ObjectModelList::getFeatureMatches(const std::vector<FeatureType*>& scene_features,
00089 const std::vector<FeatureType*>& model_database_features,
00090 const std::vector<ObjectModelList::FeatureSource>& model_database_feature_sources,
00091 float max_descriptor_distance, float min_distance_to_same_scene_feature,
00092 std::vector<std::vector<PointCorrespondences6DVector> >& feature_matches,
00093 const KdTree<FeatureType*>* kdtree, float max_descriptor_distance_kdtree) const
00094 {
00095 if (model_database_features.empty())
00096 return;
00097
00098 float min_distance_to_same_scene_feature_squared = pow(min_distance_to_same_scene_feature, 2);
00099 feature_matches.clear();
00100 feature_matches.resize(size());
00101 for (unsigned int model_idx=0; model_idx<feature_matches.size(); ++model_idx)
00102 {
00103 std::vector<PointCorrespondences6DVector>& model_feature_matches = feature_matches[model_idx];
00104 model_feature_matches.clear();
00105 model_feature_matches.resize(at(model_idx)->getViews().size());
00106 }
00107
00108 if (kdtree != NULL)
00109 {
00110 if (max_descriptor_distance_kdtree < 0.0f)
00111 {
00112 max_descriptor_distance_kdtree = max_descriptor_distance * model_database_features[0]->getDescriptorSize();
00113 std::cerr << "Maximum descriptor distance for usage in kdTree not given. Using "<<max_descriptor_distance_kdtree<<" instead.\n";
00114 }
00115 std::vector<int> neighbor_indices;
00116 std::vector<float> neighbor_distances;
00117 for (unsigned int scene_feature_idx=0; scene_feature_idx<scene_features.size(); ++scene_feature_idx)
00118 {
00119 const FeatureType& scene_feature = *scene_features[scene_feature_idx];
00120 neighbor_indices.clear();
00121 neighbor_distances.clear();
00122 kdtree->radiusSearch((FeatureType*)&scene_feature, max_descriptor_distance_kdtree,
00123 neighbor_indices, neighbor_distances, INT_MAX);
00124 for (unsigned int neighbor_indices_idx=0; neighbor_indices_idx<neighbor_indices.size(); ++neighbor_indices_idx)
00125 {
00126 int database_feature_idx = neighbor_indices[neighbor_indices_idx];
00127 FeatureType& database_feature = *model_database_features[database_feature_idx];
00128 float descriptor_distance = scene_feature.getDescriptorDistance(database_feature);
00129 if (descriptor_distance > max_descriptor_distance)
00130 continue;
00131 float score = 1.0f-descriptor_distance;
00132 integrateIntoMatchList(scene_feature_idx, database_feature_idx, score, scene_features, model_database_features,
00133 model_database_feature_sources, min_distance_to_same_scene_feature_squared, feature_matches);
00134 }
00135 }
00136 }
00137 else
00138 {
00139 for (unsigned int scene_feature_idx=0; scene_feature_idx<scene_features.size(); ++scene_feature_idx)
00140 {
00141 const FeatureType& scene_feature = *scene_features[scene_feature_idx];
00142
00143 for (unsigned int database_feature_idx=0; database_feature_idx<model_database_feature_sources.size(); ++database_feature_idx)
00144 {
00145 FeatureType& database_feature = *model_database_features[database_feature_idx];
00146 float descriptor_distance = scene_feature.getDescriptorDistance(database_feature);
00147 if (descriptor_distance > max_descriptor_distance)
00148 continue;
00149 float score = 1.0f-descriptor_distance;
00150 integrateIntoMatchList(scene_feature_idx, database_feature_idx, score, scene_features, model_database_features,
00151 model_database_feature_sources, min_distance_to_same_scene_feature_squared, feature_matches);
00152 }
00153 }
00154 }
00155 }
00156
00157 template <typename FeatureType>
00158 inline void ObjectModelList::integrateIntoMatchList(int scene_feature_idx, int database_feature_idx, float score,
00159 const std::vector<FeatureType*>& scene_features,
00160 const std::vector<FeatureType*>& model_database_features,
00161 const std::vector<ObjectModelList::FeatureSource>& model_database_feature_sources,
00162 float min_distance_to_same_scene_feature_squared,
00163 std::vector<std::vector<PointCorrespondences6DVector> >& feature_matches)
00164 {
00165 const FeatureSource& feature_source = model_database_feature_sources[database_feature_idx];
00166 PointCorrespondences6DVector& view_feature_matches = feature_matches[feature_source.model_index][feature_source.view_index];
00167
00168
00169
00170 int replace_idx = -1;
00171 bool better_match_already_exists = false;
00172 const FeatureType& database_feature = *model_database_features[database_feature_idx];
00173 for (int match_idx=view_feature_matches.size()-1; match_idx>=0; --match_idx)
00174 {
00175 PointCorrespondence6D& already_existing_match = view_feature_matches[match_idx];
00176 if (already_existing_match.index1 != (int)scene_feature_idx)
00177 break;
00178
00179 if ((database_feature.getPosition()-already_existing_match.point2).squaredNorm() <= min_distance_to_same_scene_feature_squared)
00180 {
00181
00182 if (score <= already_existing_match.score)
00183 {
00184 better_match_already_exists = true;
00185 break;
00186 }
00187 else
00188 {
00189 replace_idx = match_idx;
00190 break;
00191 }
00192 }
00193 }
00194
00195 if (better_match_already_exists)
00196 return;
00197
00198 const FeatureType& scene_feature = *scene_features[scene_feature_idx];
00199
00200 PointCorrespondence6D match;
00201 match.score = score;
00202 match.index1 = scene_feature_idx;
00203 match.index2 = feature_source.feature_index;
00204 match.point1 = scene_feature.getPosition();
00205 match.point2 = database_feature.getPosition();
00206 match.transformation = scene_feature.getTransformation().inverse() * database_feature.getTransformation();
00207 if (replace_idx < 0)
00208 view_feature_matches.push_back(match);
00209 else
00210 view_feature_matches[replace_idx] = match;
00211 }
00212
00213 }