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