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;
}
/*
* Reimport
* Imports a new model into @index
*/
void CAbstractStreaming::Reimport(ModelFile& file, int index)
{
scoped_lock xlock(this->cs);
auto it = this->imports.find(index);
if(it != this->imports.end())
it->second = file;
else
this->imports.emplace(index, file);
file.ProcessFileData();
}
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;
}
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 ShapeModel::saveToFile(ModelFile &file)
{
file.writeInt(pyramidLevel);
file.writeInt(nMarkPoints);
file.writeInt(nTrain);
file.writeInt(nShapeParams);
file.writeReal(searchYOffset);
file.writeReal(searchXOffset);
file.writeReal(searchWScale);
file.writeReal(searchHScale);
file.writeReal(searchStepAreaRatio);
file.writeReal(searchScaleRatio);
file.writeReal(searchInitXOffset);
file.writeReal(searchInitYOffset);
file.writePCA(pcaShape);
for (int i=0;i<nMarkPoints*2;i++)
file.writeReal(meanShape(i, 0));
// Info for BTSM
file.writeReal(sigma2);
file.writePCA(pcaFullShape);
shapeInfo.writeToFile(file);
////! Mean shape after aligning
//ShapeVec meanShape;
}
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();
}
void ShapeModel::loadFromFile(ModelFile &file)
{
printf("Loading Shape model from file...\n");
file.readInt(pyramidLevel);
file.readInt(nMarkPoints);
file.readInt(nTrain);
file.readInt(nShapeParams);
file.readReal(searchYOffset);
file.readReal(searchXOffset);
file.readReal(searchWScale);
file.readReal(searchHScale);
file.readReal(searchStepAreaRatio);
file.readReal(searchScaleRatio);
file.readReal(searchInitXOffset);
file.readReal(searchInitYOffset);
// PCA shape model
file.readPCA(pcaShape);
meanShape.create(nMarkPoints*2, 1);
for (int i=0;i<nMarkPoints*2;i++)
file.readReal(meanShape(i, 0));
// Info for BTSM
file.readReal(sigma2);
file.readPCA(pcaFullShape);
shapeInfo.readFromFile(file);
}
int main(int argc, char* argv[])
{
std::cout << "Enter main." << std::endl;
std::string InputFile, OutputFile;
double ScannerLocX, ScannerLocY, ScannerLocZ;
double ForwardX, ForwardY, ForwardZ;
unsigned int NumPoints;
double ThetaMin, ThetaMax, PhiMin, PhiMax, minmax;
std::cout << "Options:" << std::endl;
po::options_description desc("Allowed options");
desc.add_options()
("help", "Help message.")
("input", po::value<std::string>(&InputFile), "Set input file")
("output", po::value<std::string>(&OutputFile), "Set output file")
("ScannerLocX", po::value<double>(&ScannerLocX)->default_value(0.0), "Set scanner location (x)")
("ScannerLocY", po::value<double>(&ScannerLocY)->default_value(0.0), "Set scanner location (y)")
("ScannerLocZ", po::value<double>(&ScannerLocZ)->default_value(0.0), "Set scanner location (z)")
("ForwardX", po::value<double>(&ForwardX)->default_value(0.0), "Set forward (x)")
("ForwardY", po::value<double>(&ForwardY)->default_value(0.0), "Set forward (y)")
("ForwardZ", po::value<double>(&ForwardZ)->default_value(0.0), "Set forward (z)")
("NumPoints", po::value<unsigned int>(&NumPoints)->default_value(100), "Set number of points (one side of the grid, assuming a square grid)")
("PhiMin", po::value<double>(&PhiMin)->default_value(-10.0), "Set min phi angle")
("PhiMax", po::value<double>(&PhiMax)->default_value(10.0), "Set max phi angle")
("ThetaMin", po::value<double>(&ThetaMin)->default_value(-10.0), "Set min theta angle")
("ThetaMax", po::value<double>(&ThetaMax)->default_value(10.0), "Set max theta angle")
("minmax", po::value<double>(&minmax), "Set max theta angle")
;
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
po::notify(vm); //assign the variables (if they were specified)
if(vm.count("help"))
{
std::cout << desc << std::endl;
return 1;
}
CheckRequiredArgs(vm);
std::cout << std::endl << std::endl;
if(!vm.count("minmax"))
{
//convert degrees to radians
PhiMin = deg2rad(PhiMin);
PhiMax = deg2rad(PhiMax);
ThetaMin = deg2rad(ThetaMin);
ThetaMax = deg2rad(ThetaMax);
}
else
{
PhiMin = deg2rad(-minmax);
PhiMax = deg2rad(minmax);
ThetaMin = deg2rad(-minmax);
ThetaMax = deg2rad(minmax);
}
/////////////////////////////////////////////////////
//setup model
ModelFile Scene;
Scene.Read(InputFile);
Scene.Init();
//setup scanner
LidarScanner Scanner(true); //use octree
Scene.BuildOctree();
vgl_point_3d<double> Pos(ScannerLocX, ScannerLocY, ScannerLocZ);
vgl_vector_3d<double> Dir(ForwardX, ForwardY, ForwardZ);
Scanner.Orient(Pos, Dir);
Scanner.ScanParams.setNumPhiPoints(NumPoints);
Scanner.ScanParams.setNumThetaPoints(NumPoints);
Scanner.ScanParams.setPhiMin(PhiMin);
Scanner.ScanParams.setPhiMax(PhiMax);
Scanner.ScanParams.setThetaMin(ThetaMin);
Scanner.ScanParams.setThetaMax(ThetaMax);
//scan
PerformScan(Scene, Scanner, OutputFile);
return 0;
}
void ASMModel::loadFromFile(ModelFile& file)
{
ShapeModel::loadFromFile(file);
printf("Loading ASM model from file...\n");
//! parameter k for ASM
file.readInt(localFeatureRad);
file.readInt(ns);
int i,j;
int rows, cols;
file.readInt(rows);
file.readInt(cols);
iCovarG.resize(pyramidLevel+1);
for (i=0;i<=pyramidLevel;i++){
iCovarG[i].resize(nMarkPoints);
for (j=0;j<nMarkPoints;j++){
iCovarG[i][j].create(rows, cols);
for (int ii=0;ii<rows;ii++)
for (int jj=0;jj<cols;jj++)
file.readReal(iCovarG[i][j](ii, jj));
}
}
file.readInt(rows);
file.readInt(cols);
meanG.resize(pyramidLevel+1);
for (i=0;i<=pyramidLevel;i++){
meanG[i].resize(nMarkPoints);
for (j=0;j<nMarkPoints;j++){
meanG[i][j].create(rows, cols);
for (int ii=0;ii<rows;ii++)
for (int jj=0;jj<cols;jj++)
file.readReal(meanG[i][j](ii, jj));
}
}
// Prepare BTSM pyramid
double curSigma2 = 0;
for (i=0; i<pcaFullShape->eigenvalues.rows; i++){
curSigma2 += pcaFullShape->eigenvalues.at<double>(i, 0);
}
printf("sssssssssig: %g\n", curSigma2);
// Layer 2, 5 parameter
for (i=0; i<5 && i<pcaFullShape->eigenvalues.rows; i++){
curSigma2 -= pcaFullShape->eigenvalues.at<double>(i, 0);
}
sigma2Pyr[2] = curSigma2 / (nMarkPoints*2-4);
printf("sssssssssig: %g\n", curSigma2);
pcaPyr[2].eigenvalues = pcaFullShape->eigenvalues.rowRange(0, i);
pcaPyr[2].eigenvectors = pcaFullShape->eigenvectors.rowRange(0, i);
pcaPyr[2].mean = pcaFullShape->mean;
// Layer 1, 20 parameter
for (; i<20 && i<pcaFullShape->eigenvalues.rows; i++){
curSigma2 -= pcaFullShape->eigenvalues.at<double>(i, 0);
}
sigma2Pyr[1] = curSigma2 / (nMarkPoints*2-4);
pcaPyr[1].eigenvalues = pcaFullShape->eigenvalues.rowRange(0, i);
pcaPyr[1].eigenvectors = pcaFullShape->eigenvectors.rowRange(0, i);
pcaPyr[1].mean = pcaFullShape->mean;
/*sigma2Pyr[2] = sigma2Pyr[1] = */sigma2Pyr[0] = sigma2;
/*pcaPyr[2] = pcaPyr[1]= */pcaPyr[0] = *pcaShape;
}
