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