Пример #1
0
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;
}
Пример #2
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;
}
Пример #3
0
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);
        }
        
    }


}