/*****************************************************************************
// 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;
}
