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