C++ ModelFile::Write方法代码示例

void ComparableModels::WriteMatchingPoints(const std::string &WorldFilename, const std::string &ModelFilename)
{
	if(UsedWorldPoints.size() == 0)
	{
		std::cout << "There are no comparable points!" << std::endl;
		exit(-1);
	}
	
	//world
	std::vector<OrientedPoint> WorldPoints;
	for(unsigned int i = 0; i < UsedWorldPoints.size(); i++)
	{
		WorldPoints.push_back(World.Points_[UsedWorldPoints[i]]);
	}
	ModelFile WorldOutput;
	WorldOutput.setPoints(WorldPoints);
	WorldOutput.Init();
	WorldOutput.Write(WorldFilename);

	//model
	ModelFile ModelOutput;
	ModelOutput.setPoints(MatchingModelPoints);
	ModelOutput.Init();
	ModelOutput.Write(ModelFilename);

}

void PerformScan(ModelFile &Scene, const LidarScanner &Scanner, const std::string &OutputFilename)
{
	LidarScan Scan = Scanner.AcquireScene(Scene);
		
	ModelFile Model;
	Scan.CreateModelFile(Model);
	Model.Write(OutputFilename);
	Scan.WritePTX("Scan.ptx");
	Scan.WriteBinaryHitImage("HitImage.png");
	Scan.WriteImage("DepthImage.png");
	std::cout << "Hits: " << Scan.NumHits() << std::endl;
	
}

void WriteData(Agglomerative &A, ModelFile &Model)
{

	std::vector<unsigned int> PointLabels = A.getPointLabels();
	
	std::vector<unsigned int> UniqueLabels = Tools::UniqueElements(PointLabels);
	std::vector<Color<unsigned char> > Colors = Spectrum(UniqueLabels.size());

	for(unsigned int label = 0; label < UniqueLabels.size(); label++)
	{
		for(unsigned int p = 0; p < Model.NumPoints(); p++)
		{
			if(PointLabels[p] == UniqueLabels[label])
				Model.Points_[p].setColor(Colors[label]);
		}
	}
	
	Model.Write("Clusters.vtp");
	
	vsl_b_ofstream outputPDM("PointDistanceMatrix.bin");
	vsl_b_write(outputPDM, A.PointDistanceMatrix);
	outputPDM.close();
	
	vbl_array_2d<double> CDMValue(PointLabels.size(), PointLabels.size());
	vbl_array_2d<bool> CDMValid(PointLabels.size(), PointLabels.size());
	for(unsigned int i = 0; i < PointLabels.size(); i++)
	{
		for(unsigned int j = 0; j < PointLabels.size(); j++)
		{
			CDMValue(i,j) = A.ClusterDistanceMatrix(i,j).Value;
			CDMValid(i,j) = A.ClusterDistanceMatrix(i,j).Valid;
		}
	}
	vsl_b_ofstream outputCDMValue("ClusterDistanceMatrixValue.bin");
	vsl_b_write(outputCDMValue, CDMValue);
	outputCDMValue.close();
	
	vsl_b_ofstream outputCDMValid("ClusterDistanceMatrixValid.bin");
	vsl_b_write(outputCDMValid, CDMValid);
	outputCDMValid.close();
	/*
	vsl_b_ofstream outputCDM("ClusterDistanceMatrix.bin");
	vsl_b_write(outputCDM, A.ClusterDistanceMatrix);
	outputCDM.close();
	*/
	
	vsl_b_ofstream outputPL("PointLabels.bin");
	vsl_b_write(outputPL, PointLabels);
	outputPL.close();
}

void TestRotation(const ModelFile &World, const ModelFile &OriginalModel, const LidarScanner &Scanner, const char Which)
{
	stringstream ProbFileStream;
	ProbFileStream << "ProbR" << Which << ".txt";
	string ProbFile = ProbFileStream.str();
	ofstream fout(ProbFile.c_str());
	
	int counter = 0;
	
	//double step = PI/20.0;
	//for(double angle = -PI; angle <= PI; angle += PI/20.0)
	for(double angle = -PI/4.0; angle <= PI/4.0; angle += PI/100.0)
	{
		cout << "angle = " << angle << endl;
		ModelFile Model = OriginalModel;
		cout << "Center of mass: " << Model.getCenterOfMass() << endl;
		
		vnl_double_3x3 R = MakeRotation(Which, angle);
		Model.RotateAroundCenter(R);
		//Transformation Trans(R);
		//Model.Transform(Trans);
		
		
		stringstream ModelFilename;
		ModelFilename << "R" << Which << Tools::ZeroPad(counter, 3) << ".vtp";
		Model.Write(ModelFilename.str());
		
		//create the comparable models
		ComparableModels Comparable(World, Model, Scanner);
		
		double P = CalculateProbability(Comparable, .1);
		
		stringstream ModelPointsFilename;
		stringstream WorldPointsFilename;
		ModelPointsFilename << "ModelPointsR" << Which << Tools::ZeroPad(counter, 3) << ".vtp";
		WorldPointsFilename << "WorldPointsR" << Which << Tools::ZeroPad(counter, 3) << ".vtp";
		Comparable.Model.Write(ModelPointsFilename.str());
		Comparable.World.Write(WorldPointsFilename.str());

		fout << angle << " " << P << endl;
		counter++;
	}

	fout.close();
	
}

int main(int argc, char *argv[])
{
	std::string WithTrianglesFilename, WithoutTrianglesFilename, OutputFilename;
	
	po::options_description desc("Allowed options");
	desc.add_options()
			("help", "Help message.")
			("withtris", po::value<std::string>(&WithTrianglesFilename), "Set file with triangles.")
			("withouttris", po::value<std::string>(&WithoutTrianglesFilename), "Set file without triangles.")
			("output", po::value<std::string>(&OutputFilename), "Set output file.")
			;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);
	
	if(vm.count("help"))
	{
		std::cout << desc << std::endl;
		exit(-1);
	}
	
	CheckRequiredArgs(vm);

	ModelFile ModelWithTriangles;
	ModelWithTriangles.Read(WithTrianglesFilename);
	ModelWithTriangles.Init();
	
	ModelFile ModelWithoutTriangles;
	ModelWithoutTriangles.Read(WithoutTrianglesFilename);
	ModelWithoutTriangles.Init();
	
	if(ModelWithTriangles.NumPoints() != ModelWithoutTriangles.NumPoints())
	{
		std::cout << "The two models must have identical point lists!" << std::endl;
		exit(-1);
	}
	
	std::vector<vector<unsigned int> > VertexLists = ModelWithTriangles.getVertexLists();
	ModelWithoutTriangles.setVertexLists(VertexLists);
	
	ModelWithoutTriangles.Write(OutputFilename);
	
	return 0;
}

void KeepShell(ModelFile &Model, const vector<LidarScanner> &Scanners, const string &OutputFilename)
{
	vector<bool> KeepTri(Model.NumTriangles(), false);
	
	for(unsigned i = 0; i < Scanners.size(); i++)
	//for(unsigned i = 0; i < 1; i++)
	{
		LidarScanner Scanner = Scanners[i];
		LidarScan Scan = Scanner.AcquireScene(Model);
		for(unsigned int t = 0; t < Scanner.ScanParams.getNumThetaPoints(); t++)
		{
			for(unsigned int p = 0; p < Scanner.ScanParams.getNumPhiPoints(); p++)
			{
				LidarPoint LP = Scan.getPoint(t, p);
				if(LP.getHit() == true)
				{
					//cout << "Hit Tri: " << LP.HitTri_ << " (out of " << Model.NumTriangles() << ")" << endl;
					KeepTri[LP.HitTri_] = true;
				}
			}
		}
	}
	
	vector<vector<unsigned int> > TriList = Model.getVertexLists();
	
	vector<vector<unsigned int> > NewTriList;
	for(unsigned int i = 0; i < TriList.size(); i++)
	{
		if(KeepTri[i])
			NewTriList.push_back(TriList[i]);
	}
	
	ModelFile OutputModel = Model;
	OutputModel.setVertexLists(NewTriList);
	
	OutputModel.Write(OutputFilename);
}

int main(int argc, char *argv[])
{
  std::string SceneFilename, ModelFilename;
  double mismatch;

  //parse arguments
  po::options_description desc("Allowed options");
  desc.add_options()
                  ("help", "Help message.")
                  ("scene", po::value<std::string>(&SceneFilename), "Set scene file")
                  ("model", po::value<std::string>(&ModelFilename), "Set model file")
                  ("mismatch", po::value<double>(&mismatch), "Set mismatch.")
                  ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if(vm.count("help"))
  {
    std::cout << desc << std::endl;
    exit(-1);
  }

  CheckRequiredArgs(vm);

  ModelFile Scene;
  Scene.Read(SceneFilename);
  Scene.Init();

  ModelFile Model;
  Model.Read(ModelFilename);
  Model.Init();

  if(Scene.NumPoints() != Model.NumPoints() )
  {
    std::cout << "Number of points do not match (points should already be correspondences.)!" << std::endl;
    exit(-1);
  }

  vgl_point_3d<double> ScannerLocation;
  bool IsScan = Scene.getScannerLocation(ScannerLocation);
  std::vector<Color<unsigned char> > Colors;

  for(unsigned int i = 0; i < Scene.NumPoints(); i++)
  {
    double s = (Scene.getCoord(i) - ScannerLocation).length();
    double m = (Model.getCoord(i) - ScannerLocation).length();
    if(s > (m + mismatch))
    {
      Colors.push_back(Colors::Red());
    }
    else
    {
      Colors.push_back(Colors::Green());
    }
  }

  ModelFile SceneColored;
  SceneColored.setCoords(Scene.getCoords());
  SceneColored.setColors(Colors);
  SceneColored.Write("SceneColored.vtp");

  ModelFile ModelColored;
  ModelColored.setCoords(Model.getCoords());
  ModelColored.setColors(Colors);
  ModelColored.Write("ModelColored.vtp");


  return 0;
}

void TestTranslation(const ModelFile &World, const ModelFile &OriginalModel, const LidarScanner &Scanner, const char Which)
{
	stringstream ProbFileStream;
	ProbFileStream << "ProbT" << Which << ".txt";
	string ProbFile = ProbFileStream.str();
	ofstream fout(ProbFile.c_str());
	
	int counter = 0;
	
	//for(double var = -1.0; var <= 1.0; var += .1) //coarse
	for(double var = -0.2; var <= 0.2; var += .01) //fine
	//double var = -.2;
	{
		cout << "var = " << var << endl;
		ModelFile Model = OriginalModel;
		
		
		vgl_vector_3d<double> Translation;
		
		if(Which == 'x')
			Translation = vgl_vector_3d<double> (var, 0.0, 0.0);
		else if(Which == 'y')
			Translation = vgl_vector_3d<double> (0.0, var, 0.0);
		else if(Which == 'z')
			Translation = vgl_vector_3d<double> (0.0, 0.0, var);
		else
			assert(0);
		
		Transformation Trans(Translation);
		Model.Transform(Trans);
		stringstream ModelFilename;
		Model.Write("Model.vtp");
		
		ModelFilename << "T" << Which << Tools::ZeroPad(counter, 3) << ".vtp";
		Model.Write(ModelFilename.str());
		
		//create the comparable models
		ModelFile CompatWorld, CompatModel;
		ComparableModels Comparable(World, Model, Scanner);
		
		Comparable.Model.Write("CompatModel.vtp");
		Comparable.World.Write("CompatWorld.vtp");

		
		double P = CalculateProbability(Comparable, .1);
		
		Comparable.Model.Write("CompatModelColored.vtp");
		Comparable.World.Write("CompatWorldColored.vtp");
		
		stringstream ModelPointsFilename;
		stringstream WorldPointsFilename;
		ModelPointsFilename << "ModelPointsT" << Which << Tools::ZeroPad(counter, 3) << ".vtp";
		WorldPointsFilename << "WorldPointsT" << Which << Tools::ZeroPad(counter, 3) << ".vtp";
		Comparable.Model.Write(ModelPointsFilename.str());
		Comparable.World.Write(WorldPointsFilename.str());

		
		fout << var << " " << P << endl;
		counter++;
	}

	fout.close();
	
}