int main(int argc, char** argv)
{
std::string seg_path("data/ln_joint/");
std::string gt_path("final_joint_gt/");
std::string out_path("");
//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);
int c1 = 0, c2 = -1;
pcl::console::parse_argument(argc, argv, "--gt", gt_path);
pcl::console::parse_argument(argc, argv, "--eg", seg_path);
pcl::console::parse_argument(argc, argv, "--o", out_path);
pcl::console::parse_argument(argc, argv, "--c1", c1);
pcl::console::parse_argument(argc, argv, "--c2", c2);
if( out_path.empty() == false && exists_dir(out_path) == false )
boost::filesystem::create_directories(out_path);
float acc = 0;
int acc_count = 0;
for(int i = c1 ; i <= c2 ; i++ )
{
std::stringstream ss;
ss << i;
std::string pcd_file(seg_path + "seg_" + ss.str() + ".pcd");
std::string link_pose_file(gt_path + "link_gt_" + ss.str() + ".csv");
std::string node_pose_file(gt_path + "node_gt_" + ss.str() + ".csv");
std::cerr << "Loading... " << pcd_file << std::endl;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
pcl::io::loadPCDFile(pcd_file, *cloud);
if( cloud->empty() == true )
break;
acc_count++;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
std::vector<poseT> all_poses;
readCSV(link_pose_file, "link", all_poses);
readCSV(node_pose_file, "node", all_poses);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//read meshes
ModelT link_mesh = LoadMesh(link_mesh_name, "link");
ModelT node_mesh = LoadMesh(node_mesh_name, "node");
std::vector<ModelT> mesh_set;
mesh_set.push_back(link_mesh);
mesh_set.push_back(node_mesh);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//viewer->removePointCloud("cloud");
//viewer->addPointCloud(cloud, "cloud");
//viewer->spin();
float cur_acc = segAcc(mesh_set, all_poses, cloud);
std::cerr << "P: " << cur_acc << std::endl;
acc += cur_acc;
if( out_path.empty() == false )
pcl::io::savePCDFile(out_path + "seg_" + ss.str() + ".pcd", *cloud, true);
//viewer->removePointCloud("cloud");
//viewer->addPointCloud(cloud, "cloud");
//viewer->spin();
/*
cv::Mat gt_label, seg_label, uv;
int gt_num = MeshOn2D(mesh_set, all_poses, gt_label);
int seg_num = segProj(cloud, seg_label, uv);
int true_pos = overlapGT(gt_label, seg_label, uv);
std::cerr << "P: " << (true_pos+0.0) / seg_num << std::endl;
acc += (true_pos+0.0) / seg_num;
//*
showLabel(gt_label, "GT");
showLabel(seg_label, "Seg");
//*/
}
std::cerr << "Seg-Acc: " << acc / acc_count << std::endl;
/*************************************************************************************************************************/
return 0;
}
int main(int argc, char** argv)
{
std::string database_path,result_path;
database_path = "PCA.txt";
result_path = "result";
double pi = 4 * atan(1);
double distance_limit = 0.001, angle_limit = pi * 0.25;
/* Note that if the model is read from a file the scale should be 1.0, otherwise if the model is calculated from a database the scale should be around 0.1 */
double scale = 1.0;
double energy_weight = 0.001;
int device = CV_CAP_OPENNI;
Registration registrator;
/* Path to the database of the model */
pcl::console::parse_argument (argc, argv, "-database", database_path);
/* Path to the file where the final result is saved */
pcl::console::parse_argument (argc, argv, "-result", result_path);
/* The distance threshold for establishing the correspondences */
pcl::console::parse_argument (argc, argv, "-distance", distance_limit);
/* The angle threshold between the normals for establishing the correspondences */
pcl::console::parse_argument (argc, argv, "-angle", angle_limit);
/* The scale to which the training set has to be brought to */
pcl::console::parse_argument (argc, argv, "-scale", scale);
/* The weight that the regularizing part of the Non-Rigid Registration should have */
pcl::console::parse_argument (argc, argv, "-energy_weight", energy_weight);
Eigen::Matrix3d transform_matrix = Eigen::Matrix3d::Identity();
Eigen::Vector3d translation = Eigen::Vector3d::Zero();
transform_matrix(0,0) = scale;
transform_matrix(1,1) = scale;
transform_matrix(2,2) = scale;
/* For some reason, the cloud returned by the Asus Xtion is always up-side down, so the model is rotated by 180 degrees along the Ox axis */
transform_matrix = Eigen::AngleAxisd(pi,Eigen::Vector3d::UnitX()) * transform_matrix;
/* This switch enables the debug mode in order to analyze in the PCLVisualizer the intermiediate steps of the registration */
bool debug = pcl::console::find_switch (argc, argv, "-debug");
if( pcl::console::find_switch (argc, argv, "-Asus") )
{
device = CV_CAP_OPENNI_ASUS;
}
registrator.setDebugMode ( debug );
/* In this if branch the target cloud is a simple snapshot from the Kinect/Xtion */
if(pcl::console::find_switch (argc, argv, "--camera"))
{
std::string xml_file("haarcascade_frontalface_alt.xml");
pcl::console::parse_argument (argc, argv, "-xml_file", xml_file);
registrator.getTargetPointCloudFromCamera(device,xml_file);
registrator.getDataForModel(database_path, transform_matrix, translation, scale);
registrator.alignModel();
registrator.calculateAlternativeRegistrations(50,energy_weight,15,100,angle_limit,distance_limit,debug);
}
/* In this if branch the target cloud is read from a pcd file */
if(pcl::console::find_switch (argc, argv, "--scan"))
{
std::string pcd_file("target.pcd");
pcl::console::parse_argument (argc, argv, "-target", pcd_file);
float x,y,z;
pcl::console::parse_argument (argc, argv, "-x", x);
pcl::console::parse_argument (argc, argv, "-y", y);
pcl::console::parse_argument (argc, argv, "-z", z);
pcl::PointXYZ face(x,y,z);
registrator.getTargetPointCloudFromFile(pcd_file, face);
registrator.getDataForModel(database_path, transform_matrix, translation, scale);
registrator.alignModel();
registrator.calculateAlternativeRegistrations(50,energy_weight,15,100,angle_limit,distance_limit,debug);
}
/* In this if branch the target cloud is read by using the KinfuTracker */
if(pcl::console::find_switch (argc, argv, "--kinfu"))
{
registrator.getDataForModel (database_path, transform_matrix, translation, scale);
registrator.calculateKinfuTrackerRegistrations (device,50,energy_weight,100,angle_limit,distance_limit);
}
registrator.writeDataToPCD(result_path);
return (0);
}
