int main(int argc,char** argv){
ParameterReader* pr = new ParameterReader("../parameters.txt");
CAMERA_INTRINSIC_PARAMETERS camera;
if(strcmp((pr->getData("camera.cx")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("camera.cy")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("camera.fx")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("camera.fy")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("camera.scale")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("detector")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("descriptor")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("good_match_threshold")).c_str(),"NOT_FOUND")==0){
cout<<"Parameter lost."<<endl;
return -1;
}
camera.cx = atof((pr->getData("camera.cx")).c_str());
camera.cy = atof((pr->getData("camera.cy")).c_str());
camera.fx = atof((pr->getData("camera.fx")).c_str());
camera.fy = atof((pr->getData("camera.fy")).c_str());
camera.scale = atof((pr->getData("camera.scale")).c_str());
int min_good_match = atoi((pr->getData("min_good_match")).c_str());
/*
cv::Mat rgb,depth;
rgb = cv::imread("../pic/color.png");
depth = cv::imread("../pic/depth.png",-1);
PointCloud::Ptr cloud = image2PointCloud(rgb,depth,camera);
cloud->points.clear();
cv::Point3f point;
point.x = 50;
point.y = 100;
point.z = 1200;
cv::Point3f p = point2dTo3d(point,camera);
cout<<"2D point ("<<point.x<<","<<point.y<<") to "<<"3D point ("<<p.x<<","<<p.y<<","<<p.z<<")"<<endl;
*/
double good_match_threshold = atof((pr->getData("good_match_threshold")).c_str());
FRAME frame1,frame2;
frame1.rgb = cv::imread("../pic/color1.png");
frame1.depth = cv::imread("../pic/depth1.png");
frame2.rgb = cv::imread("../pic/color2.png");
frame2.depth = cv::imread("../pic/depth2.png");
string detector = pr->getData("detector");
string descriptor = pr->getData("descriptor");
computeKeyPointsAndDesp(frame1,detector,descriptor);
computeKeyPointsAndDesp(frame2,detector,descriptor);
RESULT_OF_PNP motion = estimateMotion(frame1,frame2,camera,good_match_threshold,min_good_match);
cout<<"Inliers = "<<motion.inliers<<endl;
cout<<"R = "<<motion.rvec<<endl;
cout<<"t = "<<motion.tvec<<endl;
//joinPointCloud(frame1,frame2,camera,motion);
return 0;
}
PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera ){
PointCloud::Ptr newCloud = image2PointCloud(newFrame.rgb,newFrame.depth,camera);
// 合并点云
PointCloud::Ptr output(new PointCloud());
pcl::transformPointCloud(*original,*output,T.matrix());
*newCloud += *output;
// Voxel grid 滤波降采样
static pcl::VoxelGrid<PointT> voxel;
static ParameterReader pd;
double gridsize = atof(pd.getData("voxel_grid").c_str());
voxel.setLeafSize(gridsize,gridsize,gridsize);
voxel.setInputCloud(newCloud);
PointCloud::Ptr tmp(new PointCloud());
voxel.filter(*tmp);
return tmp;
}
int main(int argc, char** args)
{
// check the range of the values in this mesh
ParameterReader pd;
bool use_mesh_file = atof(pd.getData("use_mesh_file").c_str());
if(use_mesh_file)
{
// this works for a single mesh
PangaeaMeshData inputMesh;
std::string input_file = pd.getData("input_mesh_file");
std::string output_file = pd.getData("output_mesh_file");
PangaeaMeshIO::loadfromFile(input_file, inputMesh);
vector<vector<double> > bbox;
getMeshBoundingBox(inputMesh, bbox);
double xrange = bbox[0][1] - bbox[0][0];
double yrange = bbox[1][1] - bbox[1][0];
double zrange = bbox[2][1] - bbox[2][0];
cout << "x_range: " << xrange << endl;
cout << "y_range: " << yrange << endl;
cout << "z_range: " << zrange << endl;
double factor = 400.0 / zrange;
cout << "scaling factor is: " << factor << endl;
// scale the mesh up
scaleMeshUp(inputMesh, factor);
PangaeaMeshIO::writeToFile(output_file, inputMesh);
}else
{
// this works for a heirachy of meshes
PangaeaMeshData inputMesh;
std::string input_format = pd.getData("input_mesh_format");
std::string output_format = pd.getData("output_mesh_format");
std::string input_list_str = pd.getData("input_list");
std::stringstream input_list(input_list_str);
int number; vector<int> input_list_vector;
input_list >> number;
while( !input_list.fail() )
{
input_list_vector.push_back(number);
input_list >> number;
}
double factor = 0;
char buffer[BUFFER_SIZE];
for(int i = 0; i < input_list_vector.size(); ++i)
{
stringstream input_file;
sprintf(buffer, input_format.c_str(), input_list_vector[i]);
input_file << buffer;
//memset(&buffer[0], 0, sizeof(buffer));
stringstream output_file;
sprintf(buffer, output_format.c_str(), input_list_vector[i]);
output_file << buffer;
//memset(&buffer[0], 0, sizeof(buffer));
PangaeaMeshIO::loadfromFile(input_file.str(), inputMesh);
if(factor == 0) // if this is the first frame
{
vector<vector<double> > bbox;
getMeshBoundingBox(inputMesh, bbox);
double xrange = bbox[0][1] - bbox[0][0];
double yrange = bbox[1][1] - bbox[1][0];
double zrange = bbox[2][1] - bbox[2][0];
cout << "x_range: " << xrange << endl;
cout << "y_range: " << yrange << endl;
cout << "z_range: " << zrange << endl;
factor = 400.0 / zrange;
cout << "scaling factor is: " << factor << endl;
}
// scale the mesh up
scaleMeshUp(inputMesh, factor);
PangaeaMeshIO::writeToFile(output_file.str(), inputMesh);
}
}
}