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