/***************************************************************************** // Runs the matching // This code reads all files from the set directory path as query images // and matches them against the database // @param directory_path: the path of all input images for query, e.g., C:/my_database // @param files: a std::vector object with a list of filenames. */ void run_matching(std::string directory_path, std::vector<std::string> files) { // loop through all images in the query folder for (int i = 0; i<files.size(); i++) { // Fetch the ref. image name from the input array\ // and assemble a path. std::string query_image_str = directory_path; query_image_str.append("/"); query_image_str.append( files[i]); // Check whether the file exists. bool ret = exists_test(query_image_str); if (ret == false) continue; // Load the image and convert it into a greyscale image cv::Mat query_image = cv::imread(query_image_str); cvtColor( query_image , query_image , CV_BGR2GRAY); std::vector< cv::DMatch > matches; // Match it against the database. int img_idx = knn_match(query_image, &matches); // release the image query_image.release(); } }
int main(int argc, char** argv) { std::string path("/home/chi/arco/test/build/temp/"); std::string filename("temp_0_0"); pcl::console::parse_argument(argc, argv, "--f", filename); pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>()); pcl::io::loadPCDFile(path+filename+".pcd", *cloud); int w = cloud->width; int h = cloud->height; cv::Mat img = cv::Mat::zeros(h, w, CV_8UC3); for( size_t i = 0 ; i < cloud->size() ; i++ ) { int r = i / w; int c = i % w; uint32_t rgb_tmp = cloud->at(i).rgba; img.at<uchar>(r, c*3+2) = (rgb_tmp >> 16) & 0x0000ff; img.at<uchar>(r, c*3+1) = (rgb_tmp >> 8) & 0x0000ff; img.at<uchar>(r, c*3+0) = (rgb_tmp) & 0x0000ff; } cv::imwrite(filename+".png", img); return 0; std::string in_path("/home/chi/JHUIT/raw/"); std::string out_path("img_tmp/"); std::string inst_name("driller_kel_0"); std::string seq_id("1");; float elev = ELEV; int max_frames = 200; pcl::console::parse_argument(argc, argv, "--p", in_path); pcl::console::parse_argument(argc, argv, "--s", seq_id); pcl::console::parse_argument(argc, argv, "--i", inst_name); pcl::console::parse_argument(argc, argv, "--elev", elev); pcl::console::parse_argument(argc, argv, "--max", max_frames); std::cerr << "Loading-"<< inst_name << std::endl; if( exists_dir(out_path) == false ) boost::filesystem::create_directories(out_path); std::ifstream fp; fp.open((in_path+"/PlaneCoef_"+ seq_id + ".txt").c_str(), std::ios::in); if( fp.is_open() == false ) { std::cerr << "Failed to open: " << in_path+"/PlaneCoef_"+ seq_id + ".txt" << std::endl; exit(0); } pcl::ModelCoefficients::Ptr planeCoef(new pcl::ModelCoefficients()); planeCoef->values.resize(4); fp >> planeCoef->values[0] >> planeCoef->values[1] >> planeCoef->values[2] >> planeCoef->values[3]; fp.close(); std::vector< pcl::PointCloud<PointT>::Ptr > cloud_set; std::vector< KEYSET > keypt_set; std::vector< cv::Mat > keydescr_set; for( size_t j = 0 ; j < max_frames ; j++ ) { std::stringstream ss; ss << j; if( j % 3 != 0 ) continue; std::string filename(in_path + inst_name + "/" + inst_name + "_" + seq_id + "_" + ss.str() + ".pcd"); if( exists_test(filename) == false ) continue; pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>()); pcl::io::loadPCDFile(filename, *cloud); int w = cloud->width; int h = cloud->height; //std::cerr << w << " " << h << " " << cloud->size() << std::endl; cv::Mat rgb = cv::Mat::zeros(h, w, CV_8UC3); cv::Mat gray = cv::Mat::zeros(h, w, CV_8UC1); for(size_t i = 0 ; i < cloud->size() ; i++ ) { int r_idx = i / w; int c_idx = i % w; uint32_t rgb_tmp = cloud->at(i).rgba; rgb.at<uchar>(r_idx, c_idx*3+2) = (rgb_tmp >> 16) & 0x0000ff; rgb.at<uchar>(r_idx, c_idx*3+1) = (rgb_tmp >> 8) & 0x0000ff; rgb.at<uchar>(r_idx, c_idx*3+0) = (rgb_tmp) & 0x0000ff; } cv::cvtColor(rgb,gray,CV_BGR2GRAY); //cv::imshow("GRAY", rgb); //cv::waitKey(); cv::SiftFeatureDetector *sift_det = new cv::SiftFeatureDetector( 0, // nFeatures 4, // nOctaveLayers 0.04, // contrastThreshold 10, //edgeThreshold 1.6 //sigma ); cv::SiftDescriptorExtractor * sift_ext = new cv::SiftDescriptorExtractor(); KEYSET keypoints; cv::Mat descriptors; sift_det->detect(gray, keypoints); sift_ext->compute(gray, keypoints, descriptors); printf("%d sift keypoints are found.\n", (int)keypoints.size()); keypt_set.push_back(keypoints); keydescr_set.push_back(descriptors); cloud = cropCloud(cloud, planeCoef, 0.114); cloud_set.push_back(cloud); //cv::imshow("GRAY", in_rgb); //cv::waitKey(); //cv::imwrite(out_path + ss.str() + ".jpg", in_rgb); //viewer->addPointCloud(cloud, "cloud"); //viewer->spin(); //if( show_keys ) /* { cv::Mat out_image; cv::drawKeypoints(gray, keypoints, out_image); cv::imshow("keypoints", out_image); cv::waitKey(); } //*/ } int total = cloud_set.size(); /* std::vector< pcl::PointCloud<PointT>::Ptr > first_half, second_half; std::vector< KEYSET > first_keypt, second_keypt; std::vector< cv::Mat > first_keydescr, second_keydescr; first_half.insert(first_half.end(), cloud_set.begin(), cloud_set.begin()+total/2); first_keypt.insert(first_keypt.end(), keypt_set.begin(), keypt_set.begin()+total/2); first_keydescr.insert(first_keydescr.end(), keydescr_set.begin(), keydescr_set.begin()+total/2); second_half.push_back(cloud_set[0]); second_keypt.push_back(keypt_set[0]); second_keydescr.push_back(keydescr_set[0]); for( int i = 1 ; i < total/2 ; i++ ) { second_half.push_back(cloud_set[total-i]); second_keypt.push_back(keypt_set[total-i]); second_keydescr.push_back(keydescr_set[total-i]); } pcl::PointCloud<PointT>::Ptr first_model = Meshing(first_half, first_keypt, first_keydescr); pcl::PointCloud<PointT>::Ptr second_model = Meshing(second_half, second_keypt, second_keydescr); pcl::PointCloud<PointT>::Ptr full_model (new pcl::PointCloud<PointT>()); full_model->insert(full_model->end(), first_model->begin(), first_model->end()); full_model->insert(full_model->end(), second_model->begin(), second_model->end()); */ pcl::PointCloud<PointT>::Ptr full_model = Meshing(cloud_set, keypt_set, keydescr_set); full_model = cropCloud(full_model, planeCoef, elev); pcl::io::savePCDFile("temp_full.pcd", *full_model); pcl::PLYWriter ply_writer; ply_writer.write("temp_full.ply", *full_model, true); /* pcl::PointCloud<PointT>::Ptr full_model(new pcl::PointCloud<PointT>()); pcl::io::loadPCDFile("temp_full.pcd", *full_model); std::vector<int> idx_ff; pcl::removeNaNFromPointCloud(*full_model, *full_model, idx_ff); pcl::PointCloud<NormalT>::Ptr full_model_normals (new pcl::PointCloud<NormalT>()); computeNormals(full_model, full_model_normals, 0.02); AdjustCloudNormal(full_model, full_model_normals); pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer()); viewer->initCameraParameters(); viewer->addCoordinateSystem(0.1); viewer->setCameraPosition(0, 0, 0.1, 0, 0, 1, 0, -1, 0); //viewer->addPointCloud(full_model, "full_model"); //viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 1.0, 1.0, "full_model"); //viewer->addPointCloudNormals<PointT, NormalT> (full_model, full_model_normals, 5, 0.01, "normals"); //viewer->spin(); pcl::PointCloud<pcl::PointNormal>::Ptr joint_model (new pcl::PointCloud<pcl::PointNormal>()); pcl::copyPointCloud(*full_model, *joint_model); pcl::copyPointCloud(*full_model_normals, *joint_model); //std::cerr << full_model->size() << " " << full_model_normals->size() << " " << joint_model->size() << std::endl; //pcl::concatenateFields (*full_model, *full_model_normals, *joint_model); pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointNormal>); tree2->setInputCloud (joint_model); pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3; pcl::PolygonMesh model_mesh; gp3.setMu(2.5); gp3.setMaximumNearestNeighbors(100); gp3.setSearchRadius(0.03); gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees gp3.setMinimumAngle(M_PI/18); // 10 degrees gp3.setMaximumAngle(2*M_PI/3); // 120 degrees gp3.setNormalConsistency(false); // Get result gp3.setInputCloud (joint_model); gp3.setSearchMethod (tree2); gp3.reconstruct (model_mesh); //viewer->addPointCloud(full_model, "model"); viewer->addPolygonMesh(model_mesh, "full_model_mesh"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.55, 0.05, "full_model_mesh"); viewer->spin(); */ return 0; }
/***************************************************************************** Inits the database by loading all images from a certain directory, extracts the feature keypoints and descriptors and saves them in the database -keypointsRefDB and _descriptorsRefDB @param directory_path: the path of all input images for query, e.g., C:/my_database @param files: a std::vector object with a list of filenames. */ void init_database(std::string directory_path, std::vector<std::string> files) { // This code reads the image names from the argv variable, loads the image // detect keypoints, extract the descriptors and push everything into // two database objects, indicate by the ending DB. for (int i = 0; i<files.size(); i++) { FeatureMap fm; // Fetch the ref. image name from the input array fm._ref_image_str = directory_path; fm._ref_image_str.append("/"); fm._ref_image_str.append( files[i]); bool ret = exists_test(fm._ref_image_str); if (ret == false) continue; // Load the image fm._ref_image = cv::imread(fm._ref_image_str); cvtColor( fm._ref_image , fm._ref_image , CV_BGR2GRAY); // Detect the keypoints _detector->detect(fm._ref_image,fm._keypointsRef); std::cout << "For referemce image " << fm._ref_image_str << " - found " << fm._keypointsRef.size() << " keypoints" << std::endl; // If keypoints found if(fm._keypointsRef.size() > 0) { // extract descriptors _extractor->compute( fm._ref_image, fm._keypointsRef, fm._descriptorRef); // push descriptors and keypoints into database _keypointsRefDB.push_back(fm._keypointsRef); _descriptorsRefDB.push_back(fm._descriptorRef); // count the total number of feature descriptors and keypoints in the database. _num_ref_features_in_db += fm._keypointsRef.size(); } // Draw the keypoints /* cv::Mat out; cv::drawKeypoints(fm._ref_image , fm._keypointsRef, out); cv::imshow("out", out ); cv::waitKey(); out.release(); */ feature_map_database.push_back(fm); } // Converting the total number of features to a string. std::strstream conv; conv << _num_ref_features_in_db; conv >> _num_ref_features_in_db_str; }