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
00039 #include <pcl/features/narf.h>
00040
00041 #include <iostream>
00042 #include <fstream>
00043 #include <cmath>
00044 using std::cout;
00045 using std::cerr;
00046 using std::vector;
00047
00048 using Eigen::Vector3f;
00049
00050 #include <pcl/range_image/range_image.h>
00051 #include <pcl/common/vector_average.h>
00052 #include <pcl/common/common_headers.h>
00053
00054 #define USE_OMP 1
00055
00056 namespace pcl {
00057
00058 Narf::Narf() : surface_patch_(NULL), descriptor_(NULL)
00059 {
00060 reset();
00061 }
00062
00063 Narf::~Narf()
00064 {
00065 reset();
00066 }
00067
00068 Narf::Narf(const Narf& other) : surface_patch_(NULL), surface_patch_pixel_size_(0), descriptor_(NULL), descriptor_size_(0)
00069 {
00070 deepCopy(other);
00071 }
00072
00073 const Narf& Narf::operator=(const Narf& other)
00074 {
00075 deepCopy(other);
00076 return *this;
00077 }
00078
00079 void Narf::reset()
00080 {
00081 delete[] descriptor_;
00082 descriptor_ = NULL;
00083 descriptor_size_ = 0;
00084 delete[] surface_patch_;
00085 surface_patch_ = NULL;
00086 surface_patch_pixel_size_ = 0;
00087 surface_patch_world_size_ = 0.0f;
00088 surface_patch_rotation_ = 0.0f;
00089 }
00090
00091 void Narf::deepCopy(const Narf& other)
00092 {
00093
00094 if (&other == this)
00095 return;
00096
00097 position_ = other.position_;
00098 transformation_ = other.transformation_;
00099
00100 if (surface_patch_pixel_size_ != other.surface_patch_pixel_size_)
00101 {
00102 surface_patch_pixel_size_ = other.surface_patch_pixel_size_;
00103 delete[] surface_patch_;
00104 surface_patch_ = new float[surface_patch_pixel_size_*surface_patch_pixel_size_];
00105 }
00106 memcpy(surface_patch_, other.surface_patch_, sizeof(*surface_patch_)*surface_patch_pixel_size_*surface_patch_pixel_size_);
00107 surface_patch_world_size_ = other.surface_patch_world_size_;
00108 surface_patch_rotation_ = other.surface_patch_rotation_;
00109
00110 if (descriptor_size_ != other.descriptor_size_)
00111 {
00112 descriptor_size_ = other.descriptor_size_;
00113 delete[] descriptor_;
00114 descriptor_ = new float[descriptor_size_];
00115 }
00116 memcpy(descriptor_, other.descriptor_, sizeof(*descriptor_)*descriptor_size_);
00117 }
00118
00119
00120 bool Narf::extractDescriptor(int descriptor_size)
00121 {
00122 float weight_for_first_point = 2.0f;
00123 int no_of_beam_points = getNoOfBeamPoints();
00124 float weight_factor = -2.0f*(weight_for_first_point-1.0f) / ((weight_for_first_point+1.0f)*float(no_of_beam_points-1)),
00125 weight_offset = 2.0f*weight_for_first_point / (weight_for_first_point+1.0f);
00126
00127
00128
00129
00130
00131
00132
00133
00134 if (descriptor_size != descriptor_size_)
00135 {
00136 descriptor_size_ = descriptor_size;
00137 delete descriptor_;
00138 descriptor_ = new float[descriptor_size_];
00139 }
00140 float angle_step_size = deg2rad(360.0f)/descriptor_size_;
00141
00142
00143 float cell_size = surface_patch_world_size_/float(surface_patch_pixel_size_),
00144 cell_factor = 1.0f/cell_size,
00145 cell_offset = 0.5f*(surface_patch_world_size_ - cell_size),
00146 max_dist = 0.5f*surface_patch_world_size_,
00147 beam_point_factor = (max_dist-0.5f*cell_size) / no_of_beam_points;
00148
00149 for (int descriptor_value_idx=0; descriptor_value_idx<descriptor_size_; ++descriptor_value_idx)
00150 {
00151 float& descriptor_value = descriptor_[descriptor_value_idx];
00152 descriptor_value = 0.0f;
00153 float angle = descriptor_value_idx*angle_step_size + surface_patch_rotation_,
00154 beam_point_factor_x = sinf(angle)*beam_point_factor,
00155 beam_point_factor_y = -cosf(angle)*beam_point_factor;
00156
00157 std::vector<float> beam_values (no_of_beam_points + 1);
00158 float current_cell = 0.0f;
00159 for (int beam_point_idx=0; beam_point_idx<=no_of_beam_points; ++beam_point_idx)
00160 {
00161 float& beam_value = beam_values[beam_point_idx];
00162
00163 float beam_point_x = beam_point_factor_x * beam_point_idx,
00164 beam_point_y = beam_point_factor_y * beam_point_idx;
00165 float beam_point_cell_x = cell_factor * (beam_point_x+cell_offset),
00166 beam_point_cell_y = cell_factor * (beam_point_y+cell_offset);
00167
00168 int cell_x=lrint(beam_point_cell_x), cell_y=lrint(beam_point_cell_y);
00169 beam_value = surface_patch_[cell_y*surface_patch_pixel_size_ + cell_x];
00170 if (!pcl_isfinite(beam_value))
00171 {
00172 if (beam_value > 0.0f)
00173 beam_value = max_dist;
00174 else
00175 beam_value = -std::numeric_limits<float>::infinity ();
00176 }
00177 }
00178 for (int beam_value_idx=0; beam_value_idx<no_of_beam_points; ++beam_value_idx)
00179 {
00180 float beam_value1=beam_values[beam_value_idx],
00181 beam_value2=beam_values[beam_value_idx+1];
00182
00183
00184
00185 float current_weight = weight_factor*float(beam_value_idx) + weight_offset;
00186 float diff = beam_value2-beam_value1;
00187
00188 current_cell += current_weight * diff;
00189 }
00190
00191 current_cell = atan2(current_cell, max_dist)/deg2rad(180.0f);
00192
00193
00194
00195
00196 descriptor_value = current_cell;
00197 }
00198 return true;
00199 }
00200
00201 bool Narf::extractFromRangeImage(const RangeImage& range_image, const Eigen::Affine3f& pose, int descriptor_size,
00202 float support_size, int surface_patch_pixel_size)
00203 {
00204 reset();
00205 position_ = getTranslation(getInverse(pose));
00206 surface_patch_world_size_ = support_size;
00207 surface_patch_pixel_size_ = surface_patch_pixel_size;
00208 surface_patch_rotation_ = 0.0f;
00209 surface_patch_ = range_image.getInterpolatedSurfaceProjection(pose, surface_patch_pixel_size_, surface_patch_world_size_);
00210 int new_pixel_size = 2*surface_patch_pixel_size_;
00211 int blur_radius = 1;
00212 float* new_surface_patch = getBlurredSurfacePatch(new_pixel_size, blur_radius);
00213 delete[] surface_patch_;
00214 surface_patch_pixel_size_ = new_pixel_size;
00215 surface_patch_ = new_surface_patch;
00216 extractDescriptor(descriptor_size);
00217 return true;
00218 }
00219
00220 bool Narf::extractFromRangeImage(const RangeImage& range_image, float x, float y, int descriptor_size, float support_size)
00221 {
00222 if (!range_image.isValid(lrint(x), lrint(y)))
00223 return false;
00224 Eigen::Vector3f feature_pos;
00225 range_image.calculate3DPoint(x, y, feature_pos);
00226
00227 return extractFromRangeImage(range_image, feature_pos, descriptor_size, support_size);
00228 }
00229
00230 bool Narf::extractFromRangeImage(const RangeImage& range_image, const InterestPoint& interest_point, int descriptor_size, float support_size)
00231 {
00232 return extractFromRangeImage(range_image, Eigen::Vector3f(interest_point.x, interest_point.y, interest_point.z), descriptor_size, support_size);
00233 }
00234
00235 bool Narf::extractFromRangeImage(const RangeImage& range_image, const Eigen::Vector3f& interest_point, int descriptor_size, float support_size)
00236 {
00237 if (!range_image.getNormalBasedUprightTransformation(interest_point, 0.5f*support_size, transformation_))
00238 return false;
00239 return extractFromRangeImage(range_image, transformation_, descriptor_size, support_size);
00240 }
00241
00242 bool Narf::extractFromRangeImageWithBestRotation(const RangeImage& range_image, const Eigen::Vector3f& interest_point,
00243 int descriptor_size, float support_size)
00244 {
00245 extractFromRangeImage(range_image, interest_point, descriptor_size, support_size);
00246 vector<float> rotations, strengths;
00247 getRotations(rotations, strengths);
00248 if (rotations.empty())
00249 return false;
00250 float best_rotation=rotations[0], best_strength=strengths[0];
00251 for (unsigned int i=1; i<rotations.size(); ++i)
00252 {
00253 if (strengths[i] > best_strength)
00254 {
00255 best_rotation = rotations[i];
00256 best_strength = strengths[i];
00257 }
00258 }
00259
00260 transformation_ = Eigen::AngleAxisf(-best_rotation, Eigen::Vector3f(0.0f, 0.0f, 1.0f))*transformation_;
00261 surface_patch_rotation_ = best_rotation;
00262 return extractDescriptor(descriptor_size_);
00263 }
00264
00265 float* Narf::getBlurredSurfacePatch(int new_pixel_size, int blur_radius) const
00266 {
00267 float new_to_old_factor = float(surface_patch_pixel_size_)/float(new_pixel_size);
00268 int new_size = new_pixel_size*new_pixel_size;
00269
00270 float* integral_image = new float[new_size];
00271 float* integral_image_ptr = integral_image;
00272 for (int y=0; y<new_pixel_size; ++y)
00273 {
00274 for (int x=0; x<new_pixel_size; ++x)
00275 {
00276 float& integral_pixel = *(integral_image_ptr++);
00277 int old_x = lrint(floor(new_to_old_factor * float(x))),
00278 old_y = lrint(floor(new_to_old_factor * float(y)));
00279
00280 integral_pixel = surface_patch_[old_y*surface_patch_pixel_size_ + old_x];
00281 if (pcl_isinf(integral_pixel))
00282 integral_pixel = 0.5f*surface_patch_world_size_;
00283 float left_value=0, top_left_value=0, top_value=0;
00284 if (x>0)
00285 {
00286 left_value = integral_image[y*new_pixel_size+x-1];
00287 if (y>0)
00288 top_left_value = integral_image[(y-1)*new_pixel_size+x-1];
00289 }
00290 if (y>0)
00291 top_value = integral_image[(y-1)*new_pixel_size+x];
00292
00293 integral_pixel += left_value + top_value - top_left_value;
00294
00295 }
00296 }
00297
00298 float* new_surface_patch = new float[new_size];
00299 float* new_surface_patch_ptr = new_surface_patch;
00300 for (int y=0; y<new_pixel_size; ++y)
00301 {
00302 for (int x=0; x<new_pixel_size; ++x)
00303 {
00304 float& new_value = *(new_surface_patch_ptr++);
00305
00306 int top = (std::max) (-1, y-blur_radius-1), right=(std::min) (new_pixel_size-1, x+blur_radius), bottom = (std::min) (new_pixel_size-1, y+blur_radius), left=(std::max) (-1, x-blur_radius-1),
00307 pixelSum = (right-left)*(bottom-top);
00308 float normalizationFactor = 1.0f / pixelSum;
00309
00310 float top_left_value=0, top_right_value=0, bottom_right_value=integral_image[bottom*new_pixel_size+right], bottom_left_value=0;
00311 if (left>=0)
00312 {
00313 bottom_left_value = integral_image[bottom*new_pixel_size+left];
00314 if (top>=0)
00315 top_left_value = integral_image[top*new_pixel_size+left];
00316 }
00317 if (top>=0)
00318 top_right_value = integral_image[top*new_pixel_size+right];
00319 new_value = normalizationFactor * (bottom_right_value + top_left_value - bottom_left_value - top_right_value);
00320 }
00321 }
00322 delete[] integral_image;
00323 return new_surface_patch;
00324 }
00325
00326 void Narf::extractFromRangeImageAndAddToList(const RangeImage& range_image, const Eigen::Vector3f& interest_point, int descriptor_size,
00327 float support_size, bool rotation_invariant, std::vector<Narf*>& feature_list)
00328 {
00329 Narf* feature = new Narf;
00330 if (!feature->extractFromRangeImage(range_image, interest_point, descriptor_size, support_size))
00331 {
00332 delete feature;
00333 return;
00334 }
00335 if (!rotation_invariant)
00336 {
00337 feature_list.push_back(feature);
00338 return;
00339 }
00340 vector<float> rotations, strengths;
00341 feature->getRotations(rotations, strengths);
00342 feature->getRotatedVersions(range_image, rotations, feature_list);
00343 delete feature;
00344 }
00345
00346 void Narf::extractFromRangeImageAndAddToList(const RangeImage& range_image, float image_x, float image_y, int descriptor_size,
00347 float support_size, bool rotation_invariant, std::vector<Narf*>& feature_list)
00348 {
00349 if (!range_image.isValid(lrint(image_x), lrint(image_y)))
00350 return;
00351 Eigen::Vector3f feature_pos;
00352 range_image.calculate3DPoint(image_x, image_y, feature_pos);
00353 extractFromRangeImageAndAddToList(range_image, feature_pos, descriptor_size, support_size, rotation_invariant, feature_list);
00354 }
00355
00356 void
00357 Narf::extractForInterestPoints (const RangeImage& range_image, const PointCloud<InterestPoint>& interest_points,
00358 int descriptor_size, float support_size, bool rotation_invariant, std::vector<Narf*>& feature_list)
00359 {
00360
00361 #if USE_OMP
00362
00363 #endif
00364 for (unsigned int interest_point_idx=0; interest_point_idx<interest_points.points.size(); ++interest_point_idx)
00365 {
00366 Vector3fMapConst point = interest_points.points[interest_point_idx].getVector3fMap ();
00367
00368 Narf* feature = new Narf;
00369 if (!feature->extractFromRangeImage(range_image, point, descriptor_size, support_size))
00370 {
00371 delete feature;
00372 continue;
00373 }
00374 if (!rotation_invariant)
00375 {
00376 # pragma omp critical
00377 {
00378 feature_list.push_back(feature);
00379 }
00380 continue;
00381 }
00382 vector<float> rotations, strengths;
00383 feature->getRotations(rotations, strengths);
00384 # pragma omp critical
00385 {
00386 feature->getRotatedVersions(range_image, rotations, feature_list);
00387 }
00388 delete feature;
00389 }
00390 }
00391
00392 void Narf::extractForEveryRangeImagePointAndAddToList(const RangeImage& range_image, int descriptor_size, float support_size,
00393 bool rotation_invariant, std::vector<Narf*>& feature_list)
00394 {
00395 for (unsigned int y=0; y<range_image.height; ++y)
00396 {
00397 for (unsigned int x=0; x<range_image.width; ++x)
00398 {
00399 extractFromRangeImageAndAddToList(range_image, x, y, descriptor_size, support_size,
00400 rotation_invariant, feature_list);
00401 }
00402 }
00403 }
00404
00405 void
00406 Narf::getRotations (std::vector<float>& rotations, std::vector<float>& strengths) const
00407 {
00408
00409
00410
00411 int angle_steps_no = (std::max) (descriptor_size_, 36);
00412 float min_angle_dist_between_rotations = deg2rad(70.0f);
00413 float angle_step_size1 = deg2rad(360.0f)/float(angle_steps_no);
00414 float angle_step_size2 = deg2rad(360.0f)/descriptor_size_;
00415
00416 float score_normalization = 1.0f / float(descriptor_size_);
00417
00418 std::multimap<float, float> scored_orientations;
00419 for (int step=0; step<angle_steps_no; ++step)
00420 {
00421 float angle = step*angle_step_size1;
00422 float score = 0.0f;
00423
00424 for (int descriptor_value_idx=0; descriptor_value_idx<descriptor_size_; ++descriptor_value_idx)
00425 {
00426 float value = descriptor_[descriptor_value_idx];
00427 float angle2 = descriptor_value_idx*angle_step_size2;
00428 float distance_weight = powf(1.0f - fabs(normAngle(angle-angle2))/deg2rad(180.0f), 2);
00429
00430 score += value * distance_weight;
00431 }
00432 score = score_normalization*score + 0.5f;
00433 scored_orientations.insert(std::make_pair(score, angle));
00434 }
00435
00436
00437
00438
00439 float min_score = scored_orientations.begin()->first,
00440 max_score = scored_orientations.rbegin()->first;
00441
00442 float min_score_for_remaining_rotations = max_score - 0.2f*(max_score-min_score);
00443 scored_orientations.erase(scored_orientations.begin(), scored_orientations.upper_bound(min_score_for_remaining_rotations));
00444
00445
00446 while (!scored_orientations.empty())
00447 {
00448 std::multimap<float, float>::iterator best_remaining_orientation_it = scored_orientations.end();
00449 --best_remaining_orientation_it;
00450 rotations.push_back(best_remaining_orientation_it->second);
00451 strengths.push_back(best_remaining_orientation_it->first);
00452 scored_orientations.erase(best_remaining_orientation_it);
00453 for (std::multimap<float, float>::iterator it = scored_orientations.begin(); it!=scored_orientations.end();)
00454 {
00455 std::multimap<float, float>::iterator current_it = it++;
00456 if (normAngle(current_it->second - rotations.back()) < min_angle_dist_between_rotations)
00457 scored_orientations.erase(current_it);
00458 }
00459 }
00460 }
00461
00462 void Narf::getRotatedVersions(const RangeImage& range_image, const std::vector<float>& rotations, std::vector<Narf*>& features) const
00463 {
00464
00465
00466 for (unsigned int i=0; i<rotations.size(); ++i)
00467 {
00468
00469
00470
00471 float rotation = rotations[i];
00472
00473 Narf* feature = new Narf(*this);
00474 feature->transformation_ = Eigen::AngleAxisf(-rotation, Eigen::Vector3f(0.0f, 0.0f, 1.0f))*feature->transformation_;
00475 feature->surface_patch_rotation_ = rotation;
00476 if (!feature->extractDescriptor(feature->descriptor_size_))
00477 {
00478 delete feature;
00479 continue;
00480 }
00481 features.push_back(feature);
00482 }
00483 }
00484
00485 void Narf::saveHeader(std::ostream& file) const
00486 {
00487 file << "\n"<<getHeaderKeyword()<<" "<<Narf::VERSION<<" ";
00488 }
00489
00490 void Narf::saveBinary(std::ostream& file) const
00491 {
00492 saveHeader(file);
00493 pcl::saveBinary(position_.matrix(), file);
00494 pcl::saveBinary(transformation_.matrix(), file);
00495 file.write(reinterpret_cast<const char*>(&surface_patch_pixel_size_), sizeof(surface_patch_pixel_size_));
00496 file.write(reinterpret_cast<const char*>(surface_patch_),
00497 surface_patch_pixel_size_*surface_patch_pixel_size_*sizeof(*surface_patch_));
00498 file.write(reinterpret_cast<const char*>(&surface_patch_world_size_), sizeof(surface_patch_world_size_));
00499 file.write(reinterpret_cast<const char*>(&surface_patch_rotation_), sizeof(surface_patch_rotation_));
00500 file.write(reinterpret_cast<const char*>(&descriptor_size_), sizeof(descriptor_size_));
00501 file.write(reinterpret_cast<const char*>(descriptor_), descriptor_size_*sizeof(*descriptor_));
00502 }
00503
00504 void Narf::saveBinary(const std::string& filename) const
00505 {
00506 std::ofstream file;
00507 file.open(filename.c_str());
00508 saveBinary(file);
00509 file.close();
00510 }
00511
00512 int Narf::loadHeader(std::istream& file) const
00513 {
00514 size_t pos_in_file = file.tellg();
00515 file.width(getHeaderKeyword().size()+10);
00516 std::string header;
00517 file >> header;
00518 file.width(0);
00519 if (header != getHeaderKeyword())
00520 {
00521 file.seekg(pos_in_file);
00522 return -1;
00523 }
00524 int version;
00525 file >> version;
00526 file.ignore(1);
00527
00528 return version;
00529 }
00530
00531 void Narf::loadBinary(std::istream& file)
00532 {
00533 reset();
00534 int version = loadHeader(file);
00535 if (version<0)
00536 {
00537 std::cerr << __PRETTY_FUNCTION__ << "Incorrect header!\n";
00538 return;
00539 }
00540 pcl::loadBinary(position_.matrix(), file);
00541 pcl::loadBinary(transformation_.matrix(), file);
00542 file.read(reinterpret_cast<char*>(&surface_patch_pixel_size_), sizeof(surface_patch_pixel_size_));
00543 surface_patch_ = new float[surface_patch_pixel_size_*surface_patch_pixel_size_];
00544 file.read(reinterpret_cast<char*>(surface_patch_),
00545 surface_patch_pixel_size_*surface_patch_pixel_size_*sizeof(*surface_patch_));
00546 file.read(reinterpret_cast<char*>(&surface_patch_world_size_), sizeof(surface_patch_world_size_));
00547 file.read(reinterpret_cast<char*>(&surface_patch_rotation_), sizeof(surface_patch_rotation_));
00548 file.read(reinterpret_cast<char*>(&descriptor_size_), sizeof(descriptor_size_));
00549 descriptor_ = new float[descriptor_size_];
00550 if (file.eof())
00551 cout << ":-(\n";
00552 file.read(reinterpret_cast<char*>(descriptor_), descriptor_size_*sizeof(*descriptor_));
00553 }
00554
00555 void Narf::loadBinary(const std::string& filename)
00556 {
00557 std::ifstream file;
00558 file.open(filename.c_str());
00559 loadBinary(file);
00560 file.close();
00561 }
00562
00563
00564 }
00565
00566