int superMi::harmonic_mapping(Solid* mesh, double deltaE, double deltaT)
{
double oldEnergy = 0;
double newEnergy = 0;
int error = 0;
bool flag = false;
error = energy(mesh, &oldEnergy, HARMONIC);
std::cout << "Initial Harmonic energy: " << oldEnergy << std::endl;
for (int i = 0; (i < 13) && !flag; i++){
error = gradient(mesh, HARMONIC);
error = absolute_derivative(mesh);
error = update_mesh(mesh, deltaT);
error = mass_center(mesh);
error = energy(mesh, &newEnergy, HARMONIC);
if (i % 1 == 0) std::cout << i + 1 << ": " << newEnergy << std::endl;
error = check_energy_change(&oldEnergy, &newEnergy, deltaE, &flag);
}
return 0;
}
int main(int argc,char** argv){
if (argc < 2){
std::cout<<"Please enter <input.pcd> file"<<std::endl;
return 0;
}
if (pcl::io::loadPCDFile (argv[1], *model) < 0)
{
std::cout << "Error loading model cloud." << std::endl;
return (-1);
}
model_name = argv[1];
model_name = model_name.substr(0,model_name.size()-4);
if(pcl::console::find_switch(argc,argv,"-vfh")){
vfh = true;
}
else if(pcl::console::find_switch(argc,argv,"-rv")){
std::cout<<"performing Resultant vector feature calculation"<<std::endl;
rv = true;
}else{
std::cout<<"no algorithm specified using default algorithm vfh"<<std::endl;
vfh = true;
}
if (pcl::console::find_switch (argc, argv, "-ds"))
{
_downsample = true;
std::cout<<"performing downsampling on the input file"<<std::endl;
}
if (pcl::console::find_switch (argc, argv, "-sn"))
{
show_normals = true;
std::cout<<"showing calclated normals"<<std::endl;
}
if(_downsample){
rec.setInputCloud(model,_leaf_size);
std::cout<<"saving downsampled file to model_down.pcd"<<std::endl;
pcl::io::savePCDFileASCII ("model_down.pcd", *(rec.getCloud()));
}
else{
rec.setInputCloud(model);
std::cout<<"setting input model without further downsampling"<<std::endl;
}
if(pcl::console::find_switch(argc,argv,"--showaxis")){
_show_axis = true;
}
if(vfh){
std::cout<<"estimating VFH features"<<std::endl;
rec.Estimate_VFH_Features();
}
else if(rv){
std::cout<<"estimating features using the resultant vector"<<std::endl;
rec.Estimate_RV_Features();
PointNormalCloudT::Ptr cloud (new PointNormalCloudT);
pcl::PointCloud<pcl::Normal>::Ptr norm_cloud (new pcl::PointCloud<pcl::Normal>);
cloud = rec.getPointNormalCloud();
norm_cloud = rec.getNormalCloud();
pcl::PointCloud<pcl::PointXYZ>::Ptr plaincloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud,*plaincloud);
//pcl::PointXYZ mass_center(rec.rv_centroid.x,rec.rv_centroid.y,rec.rv_centroid.z);
pcl::PointXYZ mass_center(0,0,0);
pcl::PointXYZ kinectZ(0,0,-1);
pcl::PointXYZ res_vec (rec.rv_resultant.normal_x,rec.rv_resultant.normal_y,rec.rv_resultant.normal_z);
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZ> rgb(plaincloud);
//viewer.addPointCloud<pcl::PointXYZ> (plaincloud, rgb, "model_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal> single_color(cloud, double(0), double(255), double(0));
viewer.addPointCloud(cloud,single_color,"sample cloud");
if(_show_axis){
viewer.addLine(mass_center,res_vec,1.0f,0.0f,0.0f,"resultantvector");
viewer.addLine(mass_center,kinectZ,1.0f,1.0f,0.0f,"KinectZ");
}
std::cout<<"resultant vector :"<<res_vec.x<<" i"<<" + "<<res_vec.y<<" j"<<" + "<<res_vec.z<<" k"<<std::endl;
if(show_normals){
std::cout<<"showing 1 in "<<normalsratio<<" normals"<<std::endl;
viewer.addPointCloudNormals<pcl::PointNormal,pcl::Normal>(cloud, norm_cloud,normalsratio, 0.05, "normalscloud");
}
while(!viewer.wasStopped())
viewer.spinOnce();
}
std::cout<<"feature calculation complete"<<std::endl;
ofstream myfile;
if (vfh){
std::stringstream ss;
ss<<"vfh_"<<model_name<<".txt";
myfile.open(ss.str().c_str());
for(size_t k =0 ;k<308;k++){
if(k != 307)
myfile << rec._vfh_features->points[0].histogram[k]<<",";
else
myfile << rec._vfh_features->points[0].histogram[k];
}
std::cout<<"wrote the histogram to file :" <<ss.str()<<std::endl;
myfile.close();
}else if(rv){
std::stringstream ss;
ss<<"rv_"<<model_name<<".txt";
myfile.open(ss.str().c_str());
for(int k = 0;k<181;k++){
if(k != 180)
myfile << rec._rv_features_181[k]<<",";
else
myfile << rec._rv_features_181[k];
}
std::cout<<"wrote the histogram to file: "<< ss.str()<<std::endl;
//myfile.open(ss.c_str());
}
}
static int moment_of_inertia_features(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& inputCluster) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
// Get cloud from publisher
pcl::copyPointCloud(*inputCluster, *cloud);
std::cout << "Hello1" << std::endl; //*
pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
std::cout << "Hello2" << std::endl; //*
feature_extractor.setInputCloud (cloud);
std::cout << "Hello3" << std::endl; //*
feature_extractor.compute ();
std::cout << "Hello4" << std::endl; //*
std::vector <float> moment_of_inertia;
std::vector <float> eccentricity;
pcl::PointXYZ min_point_AABB;
pcl::PointXYZ max_point_AABB;
pcl::PointXYZ min_point_OBB;
pcl::PointXYZ max_point_OBB;
pcl::PointXYZ position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
float major_value, middle_value, minor_value;
Eigen::Vector3f major_vector, middle_vector, minor_vector;
Eigen::Vector3f mass_center;
feature_extractor.getMomentOfInertia (moment_of_inertia);
std::cout << "Hello5" << std::endl; //*
feature_extractor.getEccentricity (eccentricity);
std::cout << "Hello6" << std::endl; //*
feature_extractor.getAABB (min_point_AABB, max_point_AABB);
std::cout << "Hello7" << std::endl; //*
feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
std::cout << "Hello8" << std::endl; //*
feature_extractor.getEigenValues (major_value, middle_value, minor_value);
std::cout << "Hello9" << std::endl; //*
feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
std::cout << "Hello10" << std::endl; //*
feature_extractor.getMassCenter (mass_center);
std::cout << "Hello11" << std::endl; //*
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();
viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");
Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
Eigen::Quaternionf quat (rotational_matrix_OBB);
viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");
while(!viewer->wasStopped()) {
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
int main (int argc, char** argv)
{
if (argc != 3)
return (0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
if (pcl::io::loadPLYFile (argv[1], *cloud) == -1)
return (-1);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::io::loadPLYFile(argv[2], *cloud2);
pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
feature_extractor.setInputCloud (cloud);
feature_extractor.compute ();
std::vector <float> moment_of_inertia;
std::vector <float> eccentricity;
pcl::PointXYZ min_point_AABB;
pcl::PointXYZ max_point_AABB;
pcl::PointXYZ min_point_OBB;
pcl::PointXYZ max_point_OBB;
pcl::PointXYZ position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
float major_value, middle_value, minor_value;
Eigen::Vector3f major_vector, middle_vector, minor_vector;
Eigen::Vector3f mass_center;
feature_extractor.getMomentOfInertia (moment_of_inertia);
feature_extractor.getEccentricity (eccentricity);
feature_extractor.getAABB (min_point_AABB, max_point_AABB);
feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
feature_extractor.getEigenValues (major_value, middle_value, minor_value);
feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
feature_extractor.getMassCenter (mass_center);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addCoordinateSystem (2.0, 0);
viewer->initCameraParameters ();
viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
viewer->addPointCloud<pcl::PointXYZ> (cloud2, "object cloud");
viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");
Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
Eigen::Quaternionf quat (rotational_matrix_OBB);
viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");
while(!viewer->wasStopped())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}