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