C++ ModelFile类代码示例

ModelFile ComparableModels::ColorOriginalModel(const double allowance)
{
	if(!Valid_)
	{
		std::cout << "Cannot run ColorOriginalModel() before creating the comparable models.\n";
		exit(-1);
	}
	
	std::vector<Color<unsigned char> > ModelColors(Model.NumPoints(), Colors::Grey((unsigned char)122));
	
	if(!ModelTree.isValid())
	{
		ModelTree.CreateTree(Model.getCoords());
	}
	
	for(unsigned int i = 0; i < MatchingModelPoints.size(); i++)
	{
		vgl_point_3d<double> TestPoint = MatchingModelPoints[i].getCoord();
		
		//unsigned int ind = ModelTree.ClosestPointIndex(TestPoint);
		std::vector<unsigned int> indices = ModelTree.IndicesWithinRadius(.05, TestPoint);
		for(unsigned int counter = 0; counter < indices.size(); counter++)
		{
			unsigned int ind = indices[counter];
			if((ModelColors[ind] == Colors::Red()) || (ModelColors[ind] == Colors::Green()))
			{
				//this point has already been assigned a color
				continue;
			}
			//std::cout << "The point closest to " << TestPoint << " is " << ind << " " << Model.getCoord(ind) << std::endl;
			
			double s = (GetMatchingWorldCoord(i) - Scanner.ScanParams.getLocation()).length();
			double m = (GetMatchingModelPoint(i) - Scanner.ScanParams.getLocation()).length();
			
			if(s > (m + allowance))
			{
				ModelColors[ind] = Colors::Red();
			}
			else
			{
				ModelColors[ind] = Colors::Green();
			}
		}	
		
		//std::cout << "Colored point " << ind << ModelColors[ind] << endl;
	}
	
	ModelFile ColoredModel = Model;
	ColoredModel.setColors(ModelColors);
	
	return ColoredModel;
}

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();
}

/*
 *  Reimport
 *      Reloads the model data (such as the size of it)
 */
void CAbstractStreaming::Reimport(ModelFile& file, bool bLock)
{
    if(bLock)
    {
        // Lock just to be sure we're not in concurrency with the streaming thread
        scoped_lock xlock(this->cs);
        // Reprocess the file data
        file.ProcessFileData();
    }
    else
    {
        file.ProcessFileData();
    }
}

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 ShapeModel::load(const string& path, bool bin)
{
    if (bin){
        ModelFile mf;
        mf.openFile(path.data(), "rb");
        this->loadFromFile(mf);
        mf.closeFile();
    }
    else {
        ModelFileAscii mf;
        mf.openFile(path.data(), "r");
        this->loadFromFile(mf);
        mf.closeFile();
    }
}

void ShapeModel::save(const string& path, bool bin)
{
    if (bin){
        ModelFile mf;
        mf.openFile(path.data(), "wb");
        this->saveToFile(mf);
        mf.closeFile();
    }
    else {
        ModelFileAscii mf;
        mf.openFile(path.data(), "w");
        this->saveToFile(mf);
        mf.closeFile();
    }
}

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();
	
}

/*
 *  Import
 *      Imports a model into @index
 */
void CAbstractStreaming::Import(ModelFile& file, int index)
{
    imgPlugin->Log("Importing model file for index %d at \"%s\"", index, file.path.c_str());
    
    file.MakeSureHasProcessed();
    this->imports.emplace(index, file);
    
}

void TestAgglomerativeClusterData(ModelFile &Model)
{
	std::vector<OrientedPoint> ModelPoints = Model.getPoints();
	Agglomerative A(ModelPoints, 0.7);
	A.PerformClustering();
	
	WriteData(A, Model);
	
	std::cout << "There are : " << Tools::UniqueElements(A.getPointLabels()).size() << " clusters." << std::endl;

}

void ASMModel::saveToFile(ModelFile& file)
{
    ShapeModel::saveToFile(file);
    
    file.writeInt(localFeatureRad);
    file.writeInt(ns);
    
    int i,j;
    int rows, cols;
    file.writeInt(rows = iCovarG[0][0].rows);
    file.writeInt(cols = iCovarG[0][0].cols);
    for (i=0;i<=pyramidLevel;i++){
        for (j=0;j<nMarkPoints;j++){
            for (int ii=0;ii<rows;ii++)
                for (int jj=0;jj<cols;jj++)
                    file.writeReal(iCovarG[i][j](ii, jj));
        }
    }
    
    file.writeInt(rows = meanG[0][0].rows);
    file.writeInt(cols = meanG[0][0].cols);
    for (i=0;i<=pyramidLevel;i++){
        for (j=0;j<nMarkPoints;j++){
            for (int ii=0;ii<rows;ii++)
                for (int jj=0;jj<cols;jj++)
                    file.writeReal(meanG[i][j](ii, jj));
        }
    }
}

void ShapeInfo::writeToFile(ModelFile &f) const {
    f.writeInt(nContours);
    for (int i=0;i<nContours+1;i++)
        f.writeInt(contourStartInd[i]);
    for (int i=0;i<nContours;i++)
        f.writeInt(contourIsClosed[i]);
    f.writeInt(pointInfo.size());
    for (size_t i=0;i<pointInfo.size();i++){
        f.writeInt(pointInfo[i].connectFrom);
        f.writeInt(pointInfo[i].connectTo);
        f.writeInt(pointInfo[i].type);
        f.writeInt(pointInfo[i].pathId);
    }
}

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);
}

void ShapeInfo::readFromFile(ModelFile &f){
    f.readInt(nContours);
    int t;
    contourStartInd.resize(nContours+1);
    for (int i=0;i<nContours+1;i++)
        contourStartInd[i] = f.readInt(t);
    contourIsClosed.resize(nContours);
    for (int i=0;i<nContours;i++)
        contourIsClosed[i] = f.readInt(t);
    pointInfo.resize(f.readInt(t));
    for (size_t i=0;i<pointInfo.size();i++){
        f.readInt(pointInfo[i].connectFrom);
        f.readInt(pointInfo[i].connectTo);
        f.readInt(pointInfo[i].type);
        f.readInt(pointInfo[i].pathId);
    }
}

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;
}