00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include "node.h"
00019 #include <cmath>
00020 #include <ctime>
00021 #include <Eigen/Geometry>
00022 #include "pcl/ros/conversions.h"
00023 #include <pcl_ros/point_cloud.h>
00024 #include <pcl/common/transformation_from_correspondences.h>
00025
00026 #include <qtconcurrentrun.h>
00027 #include <QtConcurrentMap>
00028
00029 #ifdef USE_SIFT_GPU
00030 #include "sift_gpu_wrapper.h"
00031 #endif
00032
00033
00034 #include <fstream>
00035 #ifdef USE_ICP_BIN
00036 #include "gicp-fallback.h"
00037 #endif
00038
00039 #ifdef USE_ICP_CODE
00040 #include "../external/gicp/transform.h"
00041 #endif
00042
00043
00044 #include "misc.h"
00045 #include <pcl/filters/voxel_grid.h>
00046 #include <opencv/highgui.h>
00047
00048 QMutex Node::gicp_mutex;
00049 QMutex Node::siftgpu_mutex;
00050
00051 Node::Node(const cv::Mat& visual,
00052 const cv::Mat& depth,
00053 const cv::Mat& detection_mask,
00054 const sensor_msgs::CameraInfoConstPtr& cam_info,
00055 std_msgs::Header depth_header,
00056 cv::Ptr<cv::FeatureDetector> detector,
00057 cv::Ptr<cv::DescriptorExtractor> extractor) :
00058 id_(0),
00059 pc_col(new pointcloud_type()),
00060 flannIndex(NULL),
00061 base2points_(tf::Transform::getIdentity(), depth_header.stamp, ParameterServer::instance()->get<std::string>("base_frame_name"), depth_header.frame_id),
00062 ground_truth_transform_(tf::Transform::getIdentity(), depth_header.stamp, ParameterServer::instance()->get<std::string>("ground_truth_frame_name"), ParameterServer::instance()->get<std::string>("base_frame_name")),
00063 odom_transform_(tf::Transform::getIdentity(), depth_header.stamp, "missing_odometry", depth_header.frame_id),
00064 initial_node_matches_(0)
00065 {
00066 ParameterServer* ps = ParameterServer::instance();
00067 pc_col->header = depth_header;
00068 struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
00069
00070 #ifdef USE_SIFT_GPU
00071 std::vector<float> descriptors;
00072 if(ps->get<std::string>("feature_detector_type") == "SIFTGPU"){
00073 SiftGPUWrapper* siftgpu = SiftGPUWrapper::getInstance();
00074 siftgpu->detect(visual, feature_locations_2d_, descriptors);
00075 ROS_FATAL_COND(descriptors.size()==0, "Can't run SiftGPU");
00076 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", "Feature Detection and Descriptor Extraction runtime: "<< elapsed <<" s");
00077 } else
00078 #endif
00079 {
00080 ROS_FATAL_COND(detector.empty(), "No valid detector!");
00081 detector->detect( visual, feature_locations_2d_, detection_mask);
00082 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", "Feature Detection runtime: "<< elapsed <<" s");
00083 }
00084
00085
00086 #ifdef USE_SIFT_GPU
00087 if(ps->get<std::string>("feature_detector_type") == "SIFTGPU"){
00088 projectTo3DSiftGPU(feature_locations_2d_, feature_locations_3d_, depth, cam_info, descriptors, feature_descriptors_);
00089 }
00090 else
00091 #endif
00092 {
00093 projectTo3D(feature_locations_2d_, feature_locations_3d_, depth, cam_info);
00094 struct timespec starttime2; clock_gettime(CLOCK_MONOTONIC, &starttime2);
00095 extractor->compute(visual, feature_locations_2d_, feature_descriptors_);
00096 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime2.tv_sec); elapsed += (finish.tv_nsec - starttime2.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", "Feature Extraction runtime: "<< elapsed <<" s");
00097 }
00098 assert(feature_locations_2d_.size() == feature_locations_3d_.size());
00099 assert(feature_locations_3d_.size() == (unsigned int)feature_descriptors_.rows);
00100 ROS_INFO_NAMED("statistics", "Feature Count of Node:\t%d", (int)feature_locations_2d_.size());
00101 size_t max_keyp = ps->get<int>("max_keypoints");
00102 if(feature_locations_2d_.size() > max_keyp) {
00103 feature_locations_2d_.resize(max_keyp);
00104 feature_locations_3d_.resize(max_keyp);
00105 feature_descriptors_ = feature_descriptors_.rowRange(0,max_keyp);
00106 }
00107
00108
00109 #ifdef USE_ICP_CODE
00110 if(ps->get<bool>("use_icp")){
00111 ROS_ERROR("ICP cannot be used without PointCloud. Wrong Node Constructor");
00112 }
00113 gicp_initialized = false;
00114 gicp_point_set_ = NULL;
00115 #endif
00116 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
00117
00118 }
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130 Node::Node(const cv::Mat visual,
00131 cv::Ptr<cv::FeatureDetector> detector,
00132 cv::Ptr<cv::DescriptorExtractor> extractor,
00133 pointcloud_type::Ptr point_cloud,
00134 const cv::Mat detection_mask) :
00135 id_(0),
00136 pc_col(point_cloud),
00137 flannIndex(NULL),
00138 base2points_(tf::Transform::getIdentity(), point_cloud->header.stamp,ParameterServer::instance()->get<std::string>("base_frame_name"), point_cloud->header.frame_id),
00139 ground_truth_transform_(tf::Transform::getIdentity(), point_cloud->header.stamp, ParameterServer::instance()->get<std::string>("ground_truth_frame_name"), ParameterServer::instance()->get<std::string>("base_frame_name")),
00140 odom_transform_(tf::Transform::getIdentity(), point_cloud->header.stamp, "missing_odometry", point_cloud->header.frame_id),
00141 initial_node_matches_(0)
00142 {
00143
00144 ParameterServer* ps = ParameterServer::instance();
00145
00146 ROS_INFO_STREAM("Construction of Node with " << ps->get<std::string>("feature_detector_type") << " Features");
00147 struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
00148
00149 #ifdef USE_SIFT_GPU
00150 std::vector<float> descriptors;
00151 if(ps->get<std::string>("feature_detector_type") == "SIFTGPU"){
00152 SiftGPUWrapper* siftgpu = SiftGPUWrapper::getInstance();
00153 siftgpu->detect(visual, feature_locations_2d_, descriptors);
00154 ROS_FATAL_COND(descriptors.size() ==0, "Can't run SiftGPU");
00155 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", "Feature Detection and Descriptor Extraction runtime: "<< elapsed <<" s");
00156 } else
00157 #endif
00158 if(ps->get<std::string>("feature_detector_type") != "GICP")
00159 {
00160 ROS_FATAL_COND(detector.empty(), "No valid detector!");
00161 detector->detect( visual, feature_locations_2d_, detection_mask);
00162 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", "Feature Detection runtime: "<< elapsed <<" s");
00163 }
00164
00165
00166 #ifdef USE_SIFT_GPU
00167 if(ps->get<std::string>("feature_detector_type") == "SIFTGPU")
00168 {
00169
00170
00171 projectTo3DSiftGPU(feature_locations_2d_, feature_locations_3d_, pc_col, descriptors, feature_descriptors_);
00172 }
00173 else
00174 #endif
00175 if(ps->get<std::string>("feature_detector_type") != "GICP")
00176 {
00177 projectTo3D(feature_locations_2d_, feature_locations_3d_, pc_col);
00178
00179 struct timespec starttime2; clock_gettime(CLOCK_MONOTONIC, &starttime2);
00180 extractor->compute(visual, feature_locations_2d_, feature_descriptors_);
00181 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime2.tv_sec); elapsed += (finish.tv_nsec - starttime2.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", "Feature Extraction runtime: "<< elapsed <<" s");
00182 }
00183
00184 if(ps->get<std::string>("feature_detector_type") != "GICP")
00185 {
00186 assert(feature_locations_2d_.size() == feature_locations_3d_.size());
00187 assert(feature_locations_3d_.size() == (unsigned int)feature_descriptors_.rows);
00188 ROS_INFO_NAMED("statistics", "Feature Count of Node:\t%d", (int)feature_locations_2d_.size());
00189 size_t max_keyp = ps->get<int>("max_keypoints");
00190 if(feature_locations_2d_.size() > max_keyp) {
00191 feature_locations_2d_.resize(max_keyp);
00192 feature_locations_3d_.resize(max_keyp);
00193 feature_descriptors_ = feature_descriptors_.rowRange(0,max_keyp);
00194 }
00195 assert(feature_locations_2d_.size() == feature_locations_3d_.size());
00196 assert(feature_locations_3d_.size() == (unsigned int)feature_descriptors_.rows);
00197 }
00198
00199 #ifdef USE_ICP_CODE
00200 gicp_initialized = false;
00201 gicp_point_set_ = NULL;
00202 if(!ps->get<bool>("store_pointclouds") && ps->get<bool>("use_icp"))
00203 {
00204 gicp_mutex.lock();
00205 gicp_point_set_ = this->getGICPStructure();
00206 gicp_mutex.unlock();
00207 }
00208 #endif
00209
00210 if((!ps->get<bool>("use_glwidget") ||
00211 !ps->get<bool>("use_gui")) &&
00212 !ps->get<bool>("store_pointclouds"))
00213 {
00214 ROS_WARN("Clearing out points");
00215 this->clearPointCloud();
00216 } else if(ps->get<double>("voxelfilter_size") > 0.0) {
00217 double vfs = ps->get<double>("voxelfilter_size");
00218 pcl::VoxelGrid<point_type> sor;
00219 sor.setLeafSize(vfs,vfs,vfs);
00220 pointcloud_type::ConstPtr const_cloud_ptr = boost::make_shared<pointcloud_type> (*pc_col);
00221 sor.setInputCloud (const_cloud_ptr);
00222 sor.filter (*pc_col);
00223 }
00224 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
00225
00226 }
00227
00228 Node::~Node() {
00229 delete flannIndex;
00230 }
00231
00232 void Node::setOdomTransform(tf::StampedTransform gt){
00233 odom_transform_ = gt;
00234 }
00235 void Node::setGroundTruthTransform(tf::StampedTransform gt){
00236 ground_truth_transform_ = gt;
00237 }
00238 void Node::setBase2PointsTransform(tf::StampedTransform& b2p){
00239 base2points_ = b2p;
00240 }
00241 tf::StampedTransform Node::getOdomTransform(){
00242 return odom_transform_;
00243 }
00244 tf::StampedTransform Node::getGroundTruthTransform(){
00245 return ground_truth_transform_;
00246 }
00247 tf::StampedTransform Node::getBase2PointsTransform(){
00248 return base2points_;
00249 }
00250
00251 void Node::publish(ros::Time timestamp, ros::Publisher pub){
00252
00253
00254
00255
00256
00257
00258
00259
00260 pc_col->header.stamp = timestamp;
00261 pub.publish(pc_col);
00262 ROS_INFO("Pointcloud with id %i sent with frame %s", id_, pc_col->header.frame_id.c_str());
00263 }
00264
00265 #ifdef USE_ICP_CODE
00266 bool Node::getRelativeTransformationTo_ICP_code(const Node* target_node,
00267 Eigen::Matrix4f& transformation,
00268 const Eigen::Matrix4f& initial_transformation)
00269 {
00270 struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
00271 dgc_transform_t initial;
00272 Eigen2GICP(initial_transformation,initial);
00273
00274 dgc_transform_t final_trafo;
00275 dgc_transform_identity(final_trafo);
00276
00277 gicp_mutex.lock();
00278 dgc::gicp::GICPPointSet* gicp_point_set = this->getGICPStructure();
00279 ROS_INFO("this' (%d) Point Set: %d", this->id_, gicp_point_set->Size());
00280 dgc::gicp::GICPPointSet* target_gicp_point_set = target_node->getGICPStructure();
00281 ROS_INFO("others (%d) Point Set: %d", target_node->id_, target_gicp_point_set->Size());
00282 int iterations = gicp_max_iterations;
00283 if(gicp_point_set->Size() > Node::gicp_min_point_cnt &&
00284 target_gicp_point_set->Size() > Node::gicp_min_point_cnt)
00285 {
00286 iterations = target_gicp_point_set->AlignScan(gicp_point_set, initial, final_trafo, gicp_d_max);
00287 GICP2Eigen(final_trafo,transformation);
00288 } else {
00289 ROS_WARN("GICP Point Sets not big enough. Skipping ICP");
00290 }
00291 gicp_mutex.unlock();
00292
00293
00294 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
00295 return iterations <= gicp_max_iterations;
00296 }
00297
00298 void Node::clearGICPStructure() const
00299 {
00300 gicp_mutex.lock();
00301 delete gicp_point_set_; gicp_point_set_ = NULL;
00302 gicp_mutex.unlock();
00303 }
00304 dgc::gicp::GICPPointSet* Node::getGICPStructure(unsigned int max_count) const
00305 {
00306 struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
00307 if(max_count == 0) max_count = ParameterServer::instance()->get<int>("gicp_max_cloud_size");
00308
00309 if(gicp_point_set_ != NULL){
00310 return gicp_point_set_;
00311 }
00312
00313 dgc::gicp::GICPPointSet* gicp_point_set = new dgc::gicp::GICPPointSet();
00314
00315 dgc::gicp::GICPPoint g_p;
00316 g_p.range = -1;
00317 for(int k = 0; k < 3; k++) {
00318 for(int l = 0; l < 3; l++) {
00319 g_p.C[k][l] = (k == l)?1:0;
00320 }
00321 }
00322
00323 std::vector<dgc::gicp::GICPPoint> non_NaN;
00324 for (unsigned int i=0; i<(*pc_col).points.size(); i++ ){
00325 point_type& p = (*pc_col).points.at(i);
00326 if (!isnan(p.z)) {
00327 g_p.x=p.x;
00328 g_p.y=p.y;
00329 g_p.z=p.z;
00330 non_NaN.push_back(g_p);
00331 }
00332 }
00333 float step = non_NaN.size()/static_cast<float>(max_count);
00334 step = step < 1.0 ? 1.0 : step;
00335 for (float i=0; i<non_NaN.size(); i+=step ){
00336 gicp_point_set->AppendPoint(non_NaN[static_cast<unsigned int>(i)]);
00337 }
00338 ROS_INFO("GICP point set size: %i", gicp_point_set->Size() );
00339
00340 if(gicp_point_set->Size() > Node::gicp_min_point_cnt){
00341
00342 gicp_point_set->SetDebug(true);
00343 gicp_point_set->SetGICPEpsilon(gicp_epsilon);
00344 gicp_point_set->BuildKDTree();
00345 gicp_point_set->ComputeMatrices();
00346 gicp_point_set->SetMaxIterationInner(8);
00347 gicp_point_set->SetMaxIteration(gicp_max_iterations);
00348 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
00349 }
00350 else
00351 {
00352 ROS_WARN("GICP point set too small, this node will not be algined with GICP!");
00353 }
00354
00355
00356
00357 gicp_point_set_ = gicp_point_set;
00358 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
00359 return gicp_point_set;
00360 }
00361 #endif
00362
00363 #ifdef USE_ICP_BIN
00364 bool Node::getRelativeTransformationTo_ICP_bin(const Node* target_node,
00365 Eigen::Matrix4f& transformation,
00366 const Eigen::Matrix4f* initial_transformation){
00367 std::clock_t starttime_icp = std::clock();
00368
00369 bool converged;
00370
00371 pointcloud_type::Ptr target_cloud = target_node->pc_col;
00372 if (initial_transformation != NULL)
00373 {
00374 pointcloud_type pc2;
00375 pcl::transformPointCloud((*pc_col),pc2,*initial_transformation);
00376 converged = gicpfallback(pc2, *target_cloud, transformation);
00377 }
00378 else {
00379 converged = gicpfallback((*pc_col),*target_cloud, transformation); }
00380
00381
00382
00383
00384 return converged;
00385 }
00386 #endif
00387
00388
00389 void Node::buildFlannIndex() {
00390 if (flannIndex == NULL
00391 && ParameterServer::instance()->get<std::string> ("matcher_type") == "FLANN"
00392 && ParameterServer::instance()->get<std::string> ("feature_detector_type") != "GICP"
00393 && ParameterServer::instance()->get<std::string> ("feature_extractor_type") != "ORB")
00394 {
00395 struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
00396
00397
00398 flannIndex = new cv::flann::Index(feature_descriptors_, cv::flann::KDTreeIndexParams(16));
00399 ROS_INFO("Built flannIndex (address %p) for Node %i", flannIndex, this->id_);
00400 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
00401 }
00402 }
00403
00404
00405
00406
00407 unsigned int Node::featureMatching(const Node* other, std::vector<cv::DMatch>* matches) const
00408 {
00409 struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
00410 assert(matches->size()==0);
00411
00412 const int k = 2;
00413
00414
00415
00416 double sum_distances = 0.0;
00417 ParameterServer* ps = ParameterServer::instance();
00418
00419
00420
00421 if(ps->get<std::string>("feature_detector_type") == "GICP"){
00422 return 0;
00423 }
00424 #ifdef USE_SIFT_GPU
00425 if (ps->get<std::string> ("matcher_type") == "SIFTGPU") {
00426 siftgpu_mutex.lock();
00427 sum_distances = SiftGPUWrapper::getInstance()->match(siftgpu_descriptors, feature_descriptors_.rows, other->siftgpu_descriptors, other->feature_descriptors_.rows, matches);
00428 siftgpu_mutex.unlock();
00429 }
00430 else
00431 #endif
00432
00433 if (ps->get<std::string> ("matcher_type") == "BRUTEFORCE" ||
00434 ps->get<std::string> ("feature_extractor_type") == "ORB")
00435 {
00436 cv::Ptr<cv::DescriptorMatcher> matcher;
00437 std::string brute_force_type("BruteForce");
00438 if(ps->get<std::string> ("feature_extractor_type") == "ORB"){
00439 brute_force_type.append("-HammingLUT");
00440 }
00441 matcher = cv::DescriptorMatcher::create(brute_force_type);
00442 std::vector< std::vector<cv::DMatch> > bruteForceMatches;
00443 matcher->knnMatch(feature_descriptors_, other->feature_descriptors_, bruteForceMatches, k);
00444 double max_dist_ratio_fac = ps->get<double>("nn_distance_ratio");
00445
00446 srand((long)std::clock());
00447 std::set<int> train_indices;
00448 for(unsigned int i = 0; i < bruteForceMatches.size(); i++) {
00449 cv::DMatch m1 = bruteForceMatches[i][0];
00450 cv::DMatch m2 = bruteForceMatches[i][1];
00451 float dist_ratio_fac = m1.distance / m2.distance;
00452 if (dist_ratio_fac < max_dist_ratio_fac) {
00453 int train_idx = m1.trainIdx;
00454 if(train_indices.count(train_idx) > 0)
00455 continue;
00456
00457 train_indices.insert(train_idx);
00458 sum_distances += m1.distance;
00459 m1.distance = dist_ratio_fac + (float)rand()/(1000.0*RAND_MAX);
00460 matches->push_back(m1);
00461 }
00462
00463 }
00464
00465 }
00466 else if (ps->get<std::string>("matcher_type") == "FLANN" &&
00467 ps->get<std::string>("feature_extractor_type") != "ORB")
00468 {
00469 if (other->flannIndex == NULL) {
00470 ROS_FATAL("Node %i in featureMatching: flann Index of Node %i was not initialized", this->id_, other->id_);
00471 return -1;
00472 }
00473 int start_feature = 0;
00474 int sufficient_matches = ps->get<int>("sufficient_matches");
00475 int num_segments = feature_descriptors_.rows / (sufficient_matches+100.0);
00476 if(sufficient_matches <= 0 || num_segments <= 0){
00477 num_segments=1;
00478 sufficient_matches = std::numeric_limits<int>::max();
00479 }
00480 int num_features = feature_descriptors_.rows / num_segments;
00481 for(int seg = 1; start_feature < feature_descriptors_.rows && seg <= num_segments; seg++){
00482
00483
00484 cv::Mat indices(num_features, k, CV_32S);
00485 cv::Mat dists(num_features, k, CV_32F);
00486 cv::Mat relevantDescriptors = feature_descriptors_.rowRange(start_feature, start_feature+num_features);
00487
00488
00489 struct timespec flannstarttime, flannfinish; double flannelapsed; clock_gettime(CLOCK_MONOTONIC, &flannstarttime);
00490 other->flannIndex->knnSearch(relevantDescriptors, indices, dists, k, cv::flann::SearchParams(64));
00491 clock_gettime(CLOCK_MONOTONIC, &flannfinish); flannelapsed = (flannfinish.tv_sec - flannstarttime.tv_sec); flannelapsed += (flannfinish.tv_nsec - flannstarttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(flannelapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", "Flann knnsearch runtime: "<< flannelapsed <<" s");
00492
00493
00494
00495 int* indices_ptr = indices.ptr<int> (0);
00496 float* dists_ptr = dists.ptr<float> (0);
00497
00498 cv::DMatch match;
00499 double avg_ratio = 0.0;
00500 double max_dist_ratio_fac = ps->get<double>("nn_distance_ratio");
00501 std::set<int> train_indices;
00502 for(int i = 0; i < indices.rows; ++i) {
00503 float dist_ratio_fac = static_cast<float>(dists_ptr[2 * i]) / static_cast<float>(dists_ptr[2 * i + 1]);
00504 avg_ratio += dist_ratio_fac;
00505
00506 if (max_dist_ratio_fac > dist_ratio_fac) {
00507 int train_idx = indices_ptr[2 * i];
00508 if(train_indices.count(train_idx) > 0)
00509 continue;
00510
00511 train_indices.insert(train_idx);
00512 match.queryIdx = i;
00513 match.trainIdx = train_idx;
00514 match.distance = dist_ratio_fac;
00515 sum_distances += match.distance;
00516
00517 assert(match.trainIdx < other->feature_descriptors_.rows);
00518 assert(match.queryIdx < feature_descriptors_.rows);
00519 matches->push_back(match);
00520 }
00521 }
00522 ROS_INFO("Feature Matches between Nodes %3d (%4d features) and %3d (%4d features) in segment %d/%d (features %d to %d of first node):\t%4d. Percentage: %f%%, Avg NN Ratio: %f",
00523 this->id_, (int)this->feature_locations_2d_.size(), other->id_, (int)other->feature_locations_2d_.size(), seg, num_segments, start_feature, start_feature+num_features,
00524 (int)matches->size(), (100.0*matches->size())/((float)start_feature+num_features), avg_ratio / (start_feature+num_features));
00525 if((int)matches->size() > sufficient_matches){
00526 ROS_INFO("Enough matches. Skipping remaining segments");
00527 break;
00528 }
00529 if((int)matches->size()*num_segments/(float)seg < 0.5*ps->get<int>("min_matches")){
00530 ROS_INFO("Predicted not enough feature matches, aborting matching process");
00531 break;
00532 }
00533 start_feature += num_features;
00534 }
00535 }
00536 else {
00537 ROS_FATAL_STREAM("Cannot match features:\nNo valid combination for " <<
00538 "matcher_type (" << ps->get<std::string>("matcher_type") << ") and " <<
00539 "feature_extractor_type (" << ps->get<std::string>("feature_extractor_type") << ") chosen.");
00540 }
00541
00542 ROS_INFO_NAMED("statistics", "count_matrix(%3d, %3d) = %4d;",
00543 this->id_+1, other->id_+1, (int)matches->size());
00544 ROS_INFO_NAMED("statistics", "dista_matrix(%3d, %3d) = %f;",
00545 this->id_+1, other->id_+1, sum_distances/ (float)matches->size());
00546 ROS_DEBUG_NAMED("statistics", "Feature Matches between Nodes %3d (%4d features) and %3d (%4d features):\t%4d",
00547 this->id_, (int)this->feature_locations_2d_.size(),
00548 other->id_, (int)other->feature_locations_2d_.size(),
00549 (int)matches->size());
00550
00551
00552
00553 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
00554
00555
00556 return matches->size();
00557 }
00558
00559
00560
00561 #ifdef USE_SIFT_GPU
00562 void Node::projectTo3DSiftGPU(std::vector<cv::KeyPoint>& feature_locations_2d,
00563 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
00564 const cv::Mat& depth,
00565 const sensor_msgs::CameraInfoConstPtr& cam_info,
00566 std::vector<float>& descriptors_in, cv::Mat& descriptors_out)
00567 {
00568
00569 struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
00570
00571 double depth_scaling = ParameterServer::instance()->get<double>("depth_scaling_factor");
00572 float x,y;
00573
00574 float cx = cam_info->K[2];
00575 float cy = cam_info->K[5];
00576 float fx = 1.0f / cam_info->K[0];
00577 float fy = 1.0f / cam_info->K[4];
00578 cv::Point2f p2d;
00579
00580 if(feature_locations_3d.size()){
00581 ROS_INFO("There is already 3D Information in the FrameInfo, clearing it");
00582 feature_locations_3d.clear();
00583 }
00584
00585 std::list<int> featuresUsed;
00586
00587 int index = -1;
00588 for(unsigned int i = 0; i < feature_locations_2d.size(); ){
00589 ++index;
00590
00591 p2d = feature_locations_2d[i].pt;
00592 float Z;
00593 if(ParameterServer::instance()->get<bool>("use_feature_min_depth")){
00594 Z = getMinDepthInNeighborhood(depth, p2d, feature_locations_2d[i].size);
00595 } else {
00596 Z = depth.at<float>(p2d.y, p2d.x) * depth_scaling;
00597 }
00598
00599 if (std::isnan (Z))
00600 {
00601 ROS_DEBUG("Feature %d has been extracted at NaN depth. Omitting", i);
00602 feature_locations_2d.erase(feature_locations_2d.begin()+i);
00603 continue;
00604 }
00605 x = (p2d.x - cx) * Z * fx;
00606 y = (p2d.y - cy) * Z * fy;
00607
00608 feature_locations_3d.push_back(Eigen::Vector4f(x,y, Z, 1.0));
00609 featuresUsed.push_back(index);
00610 i++;
00611 }
00612
00613
00614 int size = feature_locations_3d.size();
00615 descriptors_out = cv::Mat(size, 128, CV_32F);
00616 siftgpu_descriptors.resize(size * 128);
00617 for (int y = 0; y < size && featuresUsed.size() > 0; ++y) {
00618 int id = featuresUsed.front();
00619 featuresUsed.pop_front();
00620
00621 for (int x = 0; x < 128; ++x) {
00622 descriptors_out.at<float>(y, x) = descriptors_in[id * 128 + x];
00623 siftgpu_descriptors[y * 128 + x] = descriptors_in[id * 128 + x];
00624 }
00625 }
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
00641 }
00642
00643 void Node::projectTo3DSiftGPU(std::vector<cv::KeyPoint>& feature_locations_2d,
00644 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
00645 const pointcloud_type::Ptr point_cloud,
00646 std::vector<float>& descriptors_in, cv::Mat& descriptors_out)
00647 {
00648
00649 struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
00650
00651 cv::Point2f p2d;
00652
00653 if(feature_locations_3d.size()){
00654 ROS_INFO("There is already 3D Information in the FrameInfo, clearing it");
00655 feature_locations_3d.clear();
00656 }
00657
00658 std::list<int> featuresUsed;
00659
00660 int index = -1;
00661 for(unsigned int i = 0; i < feature_locations_2d.size(); ){
00662 ++index;
00663
00664 p2d = feature_locations_2d[i].pt;
00665 point_type p3d = point_cloud->at((int) p2d.x,(int) p2d.y);
00666
00667
00668 if ( isnan(p3d.x) || isnan(p3d.y) || isnan(p3d.z))
00669 {
00670 ROS_DEBUG_NAMED(__FILE__, "Feature %d has been extracted at NaN depth. Omitting", i);
00671 feature_locations_2d.erase(feature_locations_2d.begin()+i);
00672 continue;
00673 }
00674
00675 feature_locations_3d.push_back(Eigen::Vector4f(p3d.x, p3d.y, p3d.z, 1.0));
00676 featuresUsed.push_back(index);
00677 i++;
00678 }
00679
00680
00681 int size = feature_locations_3d.size();
00682 descriptors_out = cv::Mat(size, 128, CV_32F);
00683 siftgpu_descriptors.resize(size * 128);
00684 for (int y = 0; y < size && featuresUsed.size() > 0; ++y) {
00685 int id = featuresUsed.front();
00686 featuresUsed.pop_front();
00687
00688 for (int x = 0; x < 128; ++x) {
00689 descriptors_out.at<float>(y, x) = descriptors_in[id * 128 + x];
00690 siftgpu_descriptors[y * 128 + x] = descriptors_in[id * 128 + x];
00691 }
00692 }
00693
00694 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
00695 }
00696 #endif
00697
00698 void Node::projectTo3D(std::vector<cv::KeyPoint>& feature_locations_2d,
00699 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
00700 pointcloud_type::ConstPtr point_cloud)
00701 {
00702 struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
00703
00704 cv::Point2f p2d;
00705
00706 if(feature_locations_3d.size()){
00707 ROS_INFO("There is already 3D Information in the FrameInfo, clearing it");
00708 feature_locations_3d.clear();
00709 }
00710
00711 for(unsigned int i = 0; i < feature_locations_2d.size(); ){
00712 p2d = feature_locations_2d[i].pt;
00713 if (p2d.x >= point_cloud->width || p2d.x < 0 ||
00714 p2d.y >= point_cloud->height || p2d.y < 0 ||
00715 std::isnan(p2d.x) || std::isnan(p2d.y)){
00716 ROS_WARN_STREAM("Ignoring invalid keypoint: " << p2d);
00717 feature_locations_2d.erase(feature_locations_2d.begin()+i);
00718 continue;
00719 }
00720
00721 point_type p3d = point_cloud->at((int) p2d.x,(int) p2d.y);
00722
00723
00724 if ( isnan(p3d.x) || isnan(p3d.y) || isnan(p3d.z))
00725 {
00726 ROS_DEBUG_NAMED(__FILE__, "Feature %d has been extracted at NaN depth. Omitting", i);
00727 feature_locations_2d.erase(feature_locations_2d.begin()+i);
00728 continue;
00729 }
00730
00731 feature_locations_3d.push_back(Eigen::Vector4f(p3d.x, p3d.y, p3d.z, 1.0));
00732 i++;
00733 }
00734
00735 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
00736 }
00737
00738 void Node::projectTo3D(std::vector<cv::KeyPoint>& feature_locations_2d,
00739 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& feature_locations_3d,
00740 const cv::Mat& depth,
00741 const sensor_msgs::CameraInfoConstPtr& cam_info)
00742 {
00743 struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
00744 double depth_scaling = ParameterServer::instance()->get<double>("depth_scaling_factor");
00745 float x,y;
00746
00747 float cx = cam_info->K[2];
00748 float cy = cam_info->K[5];
00749 float fx = 1.0f / cam_info->K[0];
00750 float fy = 1.0f / cam_info->K[4];
00751
00752 cv::Point2f p2d;
00753
00754 if(feature_locations_3d.size()){
00755 ROS_INFO("There is already 3D Information in the FrameInfo, clearing it");
00756 feature_locations_3d.clear();
00757 }
00758
00759 for(unsigned int i = 0; i < feature_locations_2d.size(); ){
00760 p2d = feature_locations_2d[i].pt;
00761 if (p2d.x >= depth.cols || p2d.x < 0 ||
00762 p2d.y >= depth.rows || p2d.y < 0 ||
00763 std::isnan(p2d.x) || std::isnan(p2d.y)){
00764 ROS_WARN_STREAM("Ignoring invalid keypoint: " << p2d);
00765 feature_locations_2d.erase(feature_locations_2d.begin()+i);
00766 continue;
00767 }
00768 float Z;
00769 if(ParameterServer::instance()->get<bool>("use_feature_min_depth")){
00770 Z = getMinDepthInNeighborhood(depth, p2d, feature_locations_2d[i].size);
00771 } else {
00772 Z = depth.at<float>(p2d.y, p2d.x) * depth_scaling;
00773 }
00774
00775 if(std::isnan (Z))
00776 {
00777 ROS_DEBUG("Feature %d has been extracted at NaN depth. Omitting", i);
00778 feature_locations_2d.erase(feature_locations_2d.begin()+i);
00779 continue;
00780 }
00781 x = (p2d.x - cx) * Z * fx;
00782 y = (p2d.y - cy) * Z * fy;
00783
00784 feature_locations_3d.push_back(Eigen::Vector4f(x,y, Z, 1.0));
00785 i++;
00786 }
00787
00788 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
00789 }
00790
00791 template<class CONTAINER>
00792 void Node::computeInliersAndError(const CONTAINER& matches,
00793 const Eigen::Matrix4f& transformation,
00794 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& origins,
00795 const std::vector<std::pair<float, float> > origins_depth_stats,
00796 const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& earlier,
00797 const std::vector<std::pair<float, float> > targets_depth_stats,
00798 std::vector<cv::DMatch>& inliers,
00799 double& mean_error,
00800 std::vector<double>& errors,
00801 double squaredMaxInlierDistInM) const
00802 {
00803
00804 struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
00805
00806 inliers.clear();
00807 errors.clear();
00808
00809 std::vector<std::pair<float,int> > dists;
00810
00811 assert(matches.size() > 0);
00812 mean_error = 0.0;
00813 BOOST_FOREACH(const cv::DMatch& m, matches)
00814 {
00815 const Eigen::Vector4f& origin = origins[m.queryIdx];
00816 const Eigen::Vector4f& target = earlier[m.trainIdx];
00817 if(origin(2) == 0.0 || target(2) == 0.0){
00818 ROS_WARN_STREAM("Invalid point. Query Pt " << m.queryIdx << ":\n" << origin << "\nTarget Pt " << m.trainIdx << ":\n" << target);
00819 continue;
00820 }
00821 double mahal_dist = errorFunction2(origin, target, transformation);
00822 if(mahal_dist > squaredMaxInlierDistInM)
00823 continue;
00824 if(!(mahal_dist >= 0.0)){
00825 ROS_WARN_STREAM("Mahalanobis_ML_Error: "<<mahal_dist);
00826 ROS_WARN_STREAM("Transformation for error !>= 0:\n" << transformation << "Matches: " << matches.size());
00827 continue;
00828 }
00829 inliers.push_back(m);
00830 mean_error += mahal_dist;
00831 errors.push_back(mahal_dist );
00832 }
00833
00834 if (inliers.size()<3){
00835 ROS_WARN_COND(inliers.size() > 3, "No inliers at all in %d matches!", (int)matches.size());
00836 mean_error = 1e9;
00837 } else {
00838 mean_error /= inliers.size();
00839 mean_error = sqrt(mean_error);
00840 }
00841 if(!(mean_error>0)) ROS_DEBUG_STREAM("Transformation for mean error !> 0: " << transformation);
00842 if(!(mean_error>0)) ROS_DEBUG_STREAM(mean_error << " " << inliers.size());
00843 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
00844
00845 }
00846
00847 template<class CONTAINER>
00848 Eigen::Matrix4f Node::getTransformFromMatchesUmeyama(const Node* earlier_node, CONTAINER matches) const
00849 {
00850 Eigen::Matrix<float, 3, Eigen::Dynamic> to(3,matches.size()), from(3,matches.size());
00851 typename CONTAINER::const_iterator it = matches.begin();
00852 for (int i = 0 ;it!=matches.end(); it++, i++) {
00853 int this_id = it->queryIdx;
00854 int earlier_id = it->trainIdx;
00855
00856 from.col(i) = this->feature_locations_3d_[this_id].head<3>();
00857 to.col(i) = earlier_node->feature_locations_3d_[earlier_id].head<3>();
00858 }
00859 Eigen::Matrix4f res = Eigen::umeyama(from, to, false);
00860 return res;
00861 }
00862
00863 template<class CONTAINER>
00864 Eigen::Matrix4f Node::getTransformFromMatches(const Node* earlier_node,
00865 const CONTAINER & matches,
00866 bool& valid,
00867 const float max_dist_m) const
00868 {
00869 pcl::TransformationFromCorrespondences tfc;
00870 valid = true;
00871 std::vector<Eigen::Vector3f> t, f;
00872
00873 BOOST_FOREACH(const cv::DMatch& m, matches)
00874 {
00875 Eigen::Vector3f from = this->feature_locations_3d_[m.queryIdx].head<3>();
00876 Eigen::Vector3f to = earlier_node->feature_locations_3d_[m.trainIdx].head<3>();
00877
00878
00879 if (max_dist_m > 0) {
00880 if(f.size() >= 1)
00881 {
00882 float delta_f = (from - f.back()).squaredNorm();
00883 float delta_t = (to - t.back()).squaredNorm();
00884
00885 if ( abs(delta_f-delta_t) > max_dist_m * max_dist_m ) {
00886 valid = false;
00887 return Eigen::Matrix4f();
00888 }
00889 }
00890 f.push_back(from);
00891 t.push_back(to);
00892 }
00893
00894 tfc.add(from, to,1.0);
00895 }
00896
00897
00898 return tfc.getTransformation().matrix();
00899 }
00900
00901
00904 bool Node::getRelativeTransformationTo(const Node* earlier_node,
00905 std::vector<cv::DMatch>* initial_matches,
00906 Eigen::Matrix4f& resulting_transformation,
00907 float& rmse,
00908 std::vector<cv::DMatch>& matches) const
00909 {
00910 struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
00911
00912 assert(initial_matches != NULL);
00913 matches.clear();
00914
00915 if(initial_matches->size() <= (unsigned int) ParameterServer::instance()->get<int>("min_matches")){
00916 ROS_INFO("Only %d feature matches between %d and %d (minimal: %i)",(int)initial_matches->size() , this->id_, earlier_node->id_, ParameterServer::instance()->get<int>("min_matches"));
00917 return false;
00918 }
00919
00920
00921 unsigned int min_inlier_threshold = (unsigned int) ParameterServer::instance()->get<int>("min_matches");
00922 if(min_inlier_threshold > 0.75 * initial_matches->size()){
00923 ROS_WARN("Lowering min_inlier_threshold from %d to %d, because there are only %d matches to begin with", min_inlier_threshold, (int) (0.75 * initial_matches->size()), (int)initial_matches->size());
00924 min_inlier_threshold = 0.75 * initial_matches->size();
00925 }
00926
00927 std::vector<cv::DMatch> inlier;
00928 double inlier_error;
00929 srand((long)std::clock());
00930
00931
00932 const float max_dist_m = ParameterServer::instance()->get<double>("max_dist_for_inliers");
00933 const int ransac_iterations = ParameterServer::instance()->get<int>("ransac_iterations");
00934 std::vector<double> dummy;
00935
00936
00937 double best_error = 1e6, best_error_coarse = 1e6;
00938 unsigned int best_inlier = 0, valid_iterations = 0;
00939 unsigned int best_inlier_coarse = 0;
00940
00941 Eigen::Matrix4f transformation;
00942 Eigen::Matrix4f transformationU;
00943
00944
00945 const unsigned int sample_size = 3;
00946 for (int n_iter = 0; n_iter < ransac_iterations; n_iter++) {
00947
00948 std::set<cv::DMatch> sample_matches;
00949 while(sample_matches.size() < sample_size){
00950 int id = rand() % initial_matches->size();
00951 sample_matches.insert(initial_matches->at(id));
00952 }
00953
00954 bool valid;
00955
00956 transformation = getTransformFromMatches(earlier_node, sample_matches,valid,max_dist_m);
00957
00958
00959
00960 if (!valid) continue;
00961 if(transformation!=transformation) continue;
00962
00963 computeInliersAndError(sample_matches, transformation,
00964 this->feature_locations_3d_,
00965 this->feature_depth_stats_,
00966 earlier_node->feature_locations_3d_,
00967 earlier_node->feature_depth_stats_,
00968 inlier, inlier_error,
00969 dummy, max_dist_m*max_dist_m);
00970
00971 ROS_DEBUG_NAMED(__FILE__, "Transformation from and for %u samples results in an error of %f and %i inliers.", sample_size, inlier_error, (int)inlier.size());
00972 if(inlier_error > 1000) continue;
00973
00974
00975
00976
00977
00978
00979
00980
00981
00982
00983
00984
00985 computeInliersAndError(*initial_matches, transformation,
00986 this->feature_locations_3d_,
00987 this->feature_depth_stats_,
00988 earlier_node->feature_locations_3d_,
00989 earlier_node->feature_depth_stats_,
00990 inlier, inlier_error,
00991 dummy, max_dist_m*max_dist_m*4);
00992 ROS_DEBUG_NAMED("statistics", "TFC Transformation from %u samples results in an error of %f and %i inliers for all matches (%i).", sample_size, inlier_error, (int)inlier.size(), (int)initial_matches->size());
00993
00994 if(inlier.size() < min_inlier_threshold || inlier_error > max_dist_m){
00995 ROS_DEBUG_NAMED(__FILE__, "Skipped iteration: inliers: %i (min %i), inlier_error: %.2f (max %.2f)", (int)inlier.size(), (int) min_inlier_threshold, inlier_error*100, max_dist_m*100);
00996 continue;
00997 }
00998
00999 if (inlier.size() < best_inlier_coarse && inlier_error > best_error_coarse) {
01000 continue;
01001 }
01002 best_inlier_coarse = inlier.size();
01003 best_error_coarse = inlier_error;
01004
01005 if (inlier.size() > best_inlier && inlier_error < best_error) {
01006 best_inlier = inlier.size();
01007 best_error = inlier_error;
01008 }
01009
01010 ROS_DEBUG_NAMED(__FILE__, "Refining iteration from %i samples: all matches: %i, inliers: %i, inlier_error: %f", (int)sample_size, (int)initial_matches->size(), (int)inlier.size(), inlier_error);
01011 valid_iterations++;
01012 assert(inlier_error>=0);
01013
01014
01015 if (inlier.size() > initial_matches->size()*0.5) n_iter+=10;
01016 if (inlier.size() > initial_matches->size()*0.8) n_iter+=20;
01017
01018 if (inlier_error < best_error) {
01019 resulting_transformation = transformation;
01020 matches = inlier;
01021 assert(matches.size()>= min_inlier_threshold);
01022
01023 rmse = inlier_error;
01024 best_error = inlier_error;
01025 }
01026
01027
01028
01029 double new_inlier_error;
01030 transformation = getTransformFromMatches(earlier_node, matches, valid);
01031 if(transformation!=transformation) continue;
01032 computeInliersAndError(*initial_matches, transformation,
01033 this->feature_locations_3d_,
01034 this->feature_depth_stats_,
01035 earlier_node->feature_locations_3d_,
01036 earlier_node->feature_depth_stats_,
01037 inlier, new_inlier_error, dummy, max_dist_m*max_dist_m);
01038 ROS_DEBUG_NAMED(__FILE__, "Refined Transformation from all matches (%i) results in an error of %f and %i inliers for all matches.", (int)initial_matches->size(), inlier_error, (int)inlier.size());
01039
01040 if(inlier.size() < min_inlier_threshold || new_inlier_error > max_dist_m){
01041 continue;
01042 }
01043
01044 if (inlier.size() > best_inlier && new_inlier_error < best_error) {
01045 best_inlier = inlier.size();
01046 best_error = inlier_error;
01047 resulting_transformation = transformation;
01048 matches = inlier;
01049 assert(matches.size()>= min_inlier_threshold);
01050
01051 rmse = new_inlier_error;
01052 best_error = new_inlier_error;
01053 }
01054 }
01055
01056 ROS_INFO("%i good iterations (from %i), inlier pct %i, inlier cnt: %i, error: %.2f cm",valid_iterations, ransac_iterations, (int) (matches.size()*1.0/initial_matches->size()*100),(int) matches.size(),rmse*100);
01057
01058
01059 clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
01060 bool enough_absolute = matches.size() >= min_inlier_threshold;
01061 return enough_absolute;
01062 }
01063
01064
01065 #ifdef USE_ICP_CODE
01066 void Node::Eigen2GICP(const Eigen::Matrix4f& m, dgc_transform_t g_m){
01067 for(int i=0;i<4; i++)
01068 for(int j=0;j<4; j++)
01069 g_m[i][j] = m(i,j);
01070
01071 }
01072 void Node::GICP2Eigen(const dgc_transform_t g_m, Eigen::Matrix4f& m){
01073 for(int i=0;i<4; i++)
01074 for(int j=0;j<4; j++)
01075 m(i,j) = g_m[i][j];
01076 }
01077
01078 void Node::gicpSetIdentity(dgc_transform_t m){
01079 for(int i=0;i<4; i++)
01080 for(int j=0;j<4; j++)
01081 if (i==j)
01082 m[i][j] = 1;
01083 else
01084 m[i][j] = 0;
01085 }
01086 #endif
01087
01088
01089
01090 MatchingResult Node::matchNodePair(const Node* older_node)
01091 {
01092 MatchingResult mr;
01093 bool found_transformation = false;
01094 if(initial_node_matches_ > ParameterServer::instance()->get<int>("max_connections")) return mr;
01095 const unsigned int min_matches = (unsigned int) ParameterServer::instance()->get<int>("min_matches");
01096
01097
01098 this->featureMatching(older_node, &mr.all_matches);
01099
01100 ROS_DEBUG_NAMED(__FILE__, "found %i inital matches",(int) mr.all_matches.size());
01101 if (mr.all_matches.size() < min_matches){
01102 ROS_INFO("Too few inliers between %i and %i for RANSAC method. Only %i correspondences to begin with.",
01103 older_node->id_,this->id_,(int)mr.all_matches.size());
01104 }
01105 else {
01106 found_transformation = getRelativeTransformationTo(older_node,&mr.all_matches, mr.ransac_trafo, mr.rmse, mr.inlier_matches);
01107
01108 float nn_ratio = 0.0;
01109 if(found_transformation){
01110 double w = 1.0 + (double)mr.inlier_matches.size()-(double)min_matches;
01111 for(unsigned int i = 0; i < mr.inlier_matches.size(); i++){
01112 nn_ratio += mr.inlier_matches[i].distance;
01113 }
01114 nn_ratio /= mr.inlier_matches.size();
01115 mr.final_trafo = mr.ransac_trafo;
01116 mr.edge.informationMatrix = Eigen::Matrix<double,6,6>::Identity()*(w*w);
01117 ROS_INFO("RANSAC found a valid transformation with %d inliers matches with average ratio %f",(int) mr.inlier_matches.size(), nn_ratio);
01118 } else {
01119 for(unsigned int i = 0; i < mr.all_matches.size(); i++){
01120 nn_ratio += mr.all_matches[i].distance;
01121 }
01122 nn_ratio /= mr.all_matches.size();
01123 ROS_WARN("RANSAC found no valid trafo, but had initially %d feature matches with average ratio %f",(int) mr.all_matches.size(), nn_ratio);
01124 }
01125 }
01126
01127 #ifdef USE_ICP_CODE
01128 if(ParameterServer::instance()->get<bool>("use_icp")){
01129 if((int)this->id_ - (int)older_node->id_ >= 5){
01130
01131 }
01132 else if(!found_transformation && initial_node_matches_ == 0)
01133 {
01134 ROS_INFO("Falling back to GICP for Transformation between Nodes %d and %d",this->id_ , older_node->id_);
01135 if(getRelativeTransformationTo_ICP_code(older_node,mr.icp_trafo, mr.ransac_trafo) &&
01136 !((mr.icp_trafo.array() != mr.icp_trafo.array()).any()))
01137 {
01138 ROS_INFO("GICP Successful");
01139 found_transformation = true;
01140 mr.final_trafo = mr.icp_trafo;
01141 mr.edge.informationMatrix = Eigen::Matrix<double,6,6>::Identity()*(1000);
01142
01143 mr.edge.informationMatrix(0,0) = 200;
01144 mr.edge.informationMatrix(1,1) = 200;
01145 mr.edge.informationMatrix(2,2) = 200;
01146 }
01147 }
01148 }
01149 #endif
01150
01151 #ifdef USE_ICP_BIN
01152 if(!found_transformation && (this->id_ - older_node->id_ < 5) &&
01153 initial_node_matches_ == 0 && ParameterServer::instance()->get<bool>("use_icp"))
01154 {
01155 ROS_INFO("Falling back to GICP binary");
01156
01157
01158 bool converged = getRelativeTransformationTo_ICP_bin(older_node,mr.icp_trafo, &mr.ransac_trafo);
01159
01160
01161 ROS_INFO("icp: inliers: %i", (int)mr.inlier_matches.size());
01162 if(converged){
01163 found_transformation = true;
01164 mr.final_trafo = mr.ransac_trafo * mr.icp_trafo;
01165
01166 std::vector<double> errors;
01167 double error;
01168 std::vector<cv::DMatch> inliers;
01169
01170 computeInliersAndError(mr.inlier_matches, mr.final_trafo,
01171 this->feature_locations_3d_,
01172 this->feature_depth_stats_,
01173 older_node->feature_locations_3d_,
01174 older_node->feature_depth_stats_,
01175 inliers, error, errors, 0.04*0.04);
01176
01177 for (uint i=0; i<errors.size(); i++)
01178 {
01179 ROS_INFO_COND("error: " << round(errors[i]*10000)/100 );
01180 }
01181
01182 ROS_INFO_COND("error was: " << mr.rmse << " and is now: " << error );
01183
01184 double roll, pitch, yaw, dist;
01185 mat2components(mr.ransac_trafo, roll, pitch, yaw, dist);
01186 ROS_INFO_COND("ransac: " << roll << " "<< pitch << " "<< yaw << " "<< dist );
01187
01188 mat2components(mr.icp_trafo, roll, pitch, yaw, dist);
01189 ROS_INFO_COND("icp: " << roll << " "<< pitch << " "<< yaw << " "<< dist );
01190
01191 mat2components(mr.final_trafo, roll, pitch, yaw, dist);
01192 ROS_INFO_COND("final: " << roll << " "<< pitch << " "<< yaw << " "<< dist );
01193 ROS_INFO_COND("ransac: " << std::endl << mr.ransac_trafo );
01194 ROS_INFO_COND("icp: " << std::endl << mr.icp_trafo );
01195 ROS_INFO_COND("total: " << std::endl << mr.final_trafo );
01196
01197
01198 if (error > (mr.rmse+0.02))
01199 {
01200 ROS_WARN("#### icp-error is too large, ignoring the connection");
01201 }
01202 else
01203 mr.final_trafo = mr.ransac_trafo * mr.icp_trafo;
01204 }
01205 ROS_INFO_COND(!converged, "ICP did not converge.");
01206 w = (double)mr.inlier_matches.size();
01207 mr.edge.informationMatrix = Eigen::Matrix<double,6,6>::Identity()*(w*w);
01208 }
01209 #endif
01210
01211 if(found_transformation) {
01212 ROS_INFO("Returning Valid Edge");
01213 ++initial_node_matches_;
01214
01215 mr.edge.id1 = older_node->id_;
01216 mr.edge.id2 = this->id_;
01217 mr.edge.mean = eigen2G2O(mr.final_trafo.cast<double>());
01218 }
01219 return mr;
01220 }
01221
01222 void Node::clearFeatureInformation(){
01223
01224 std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > f_l_3d;
01225 f_l_3d.swap(feature_locations_3d_);
01226 std::vector<cv::KeyPoint> f_l_2d;
01227 f_l_2d.swap(feature_locations_2d_);
01228 feature_descriptors_.release();
01229 }
01230 void Node::addPointCloud(pointcloud_type::Ptr new_pc){
01231 pc_col = new_pc;
01232 }
01233 void Node::clearPointCloud(){
01234
01235 pc_col->width = 0;
01236 pc_col->height = 0;
01237 pointcloud_type pc_empty;
01238 pc_empty.points.swap(pc_col->points);
01239 }
01240
01241
01242
01243
01244
01245
01246
01247
01248
01249
01250
01251
01252
01253
01254
01255
01256
01257
01258
01259
01260
01261