int main( int argc, char *argv[] ) {
if(argc<=1){
std::cout<<"usage: "<<argv[0]<<" <octoMap.bt>"<<std::endl;
exit(0);
}
octomap::OcTree *tree = NULL;
tree = new octomap::OcTree(0.05);
//read in octotree
tree->readBinary(argv[1]);
std::cout<<"read in tree, "<<tree->getNumLeafNodes()<<" leaves "<<std::endl;
double x,y,z;
tree->getMetricMin(x,y,z);
octomap::point3d min(x,y,z);
//std::cout<<"Metric min: "<<x<<","<<y<<","<<z<<std::endl;
tree->getMetricMax(x,y,z);
octomap::point3d max(x,y,z);
//std::cout<<"Metric max: "<<x<<","<<y<<","<<z<<std::endl;
bool unknownAsOccupied = true;
unknownAsOccupied = false;
float maxDist = 1.0;
//- the first argument ist the max distance at which distance computations are clamped
//- the second argument is the octomap
//- arguments 3 and 4 can be used to restrict the distance map to a subarea
//- argument 5 defines whether unknown space is treated as occupied or free
//The constructor copies data but does not yet compute the distance map
DynamicEDTOctomap distmap(maxDist, tree, min, max, unknownAsOccupied);
//This computes the distance map
distmap.update();
//This is how you can query the map
octomap::point3d p(5.0,5.0,0.6);
//As we don't know what the dimension of the loaded map are, we modify this point
p.x() = min.x() + 0.3 * (max.x() - min.x());
p.y() = min.y() + 0.6 * (max.y() - min.y());
p.z() = min.z() + 0.5 * (max.z() - min.z());
octomap::point3d closestObst;
float distance;
distmap.getDistanceAndClosestObstacle(p, distance, closestObst);
std::cout<<"\n\ndistance at point "<<p.x()<<","<<p.y()<<","<<p.z()<<" is "<<distance<<std::endl;
if(distance < distmap.getMaxDist())
std::cout<<"closest obstacle to "<<p.x()<<","<<p.y()<<","<<p.z()<<" is at "<<closestObst.x()<<","<<closestObst.y()<<","<<closestObst.z()<<std::endl;
//if you modify the octree via tree->insertScan() or tree->updateNode()
//just call distmap.update() again to adapt the distance map to the changes made
delete tree;
return 0;
}
int main( int , char** ) {
//we build a sample map
int sizeX, sizeY, sizeZ;
sizeX=100;
sizeY=100;
sizeZ=100;
bool*** map;
map = new bool**[sizeX];
for(int x=0; x<sizeX; x++){
map[x] = new bool*[sizeY];
for(int y=0; y<sizeY; y++){
map[x][y] = new bool[sizeZ];
for(int z=0; z<sizeZ; z++){
if(x<2 || x > sizeX-3 || y < 2 || y > sizeY-3 || z<2 || z > sizeZ-3)
map[x][y][z] = 1;
else
map[x][y][z] = 0;
}
}
}
map[51][45][67] = 1;
map[50][50][68] = 1;
// create the EDT object and initialize it with the map
int maxDistInCells = 20;
DynamicEDT3D distmap(maxDistInCells*maxDistInCells);
distmap.initializeMap(sizeX, sizeY, sizeZ, map);
//compute the distance map
distmap.update();
// now perform some updates with random obstacles
int numPoints = 20;
for (int frame=1; frame<=10; frame++) {
std::cout<<"\n\nthis is frame #"<<frame<<std::endl;
std::vector<IntPoint3D> newObstacles;
for (int i=0; i<numPoints; i++) {
double x = 2+rand()/(double)RAND_MAX*(sizeX-4);
double y = 2+rand()/(double)RAND_MAX*(sizeY-4);
double z = 2+rand()/(double)RAND_MAX*(sizeZ-4);
newObstacles.push_back(IntPoint3D(x,y,z));
}
// register the new obstacles (old ones will be removed)
distmap.exchangeObstacles(newObstacles);
//update the distance map
distmap.update();
//retrieve distance at a point
float dist = distmap.getDistance(30,67,33);
int distSquared = distmap.getSQCellDistance(30,67,33);
std::cout<<"distance at 30,67,33: "<< dist << " squared: "<< distSquared << std::endl;
if(distSquared == maxDistInCells*maxDistInCells)
std::cout<<"we hit a cell with d = dmax, distance value is clamped."<<std::endl;
//retrieve closest occupied cell at a point
IntPoint3D closest = distmap.getClosestObstacle(30,67,33);
if(closest.x == DynamicEDT3D::invalidObstData)
std::cout<<"we hit a cell with d = dmax, no information about closest occupied cell."<<std::endl;
else
std::cout<<"closest occupied cell to 30,67,33: "<< closest.x<<","<<closest.y<<","<<closest.z<<std::endl;
}
std::cout<<"\n\nthis is the last frame"<<std::endl;
// now remove all random obstacles again.
std::vector<IntPoint3D> empty;
distmap.exchangeObstacles(empty);
distmap.update();
//retrieve distance at a point
float dist = distmap.getDistance(30,67,33);
int distSquared = distmap.getSQCellDistance(30,67,33);
std::cout<<"distance at 30,67,33: "<< dist << " squared: "<< distSquared << std::endl;
if(distSquared == maxDistInCells*maxDistInCells)
std::cout<<"we hit a cell with d = dmax, distance value is clamped."<<std::endl;
//retrieve closest occupied cell at a point
IntPoint3D closest = distmap.getClosestObstacle(30,67,33);
if(closest.x == DynamicEDT3D::invalidObstData)
std::cout<<"we hit a cell with d = dmax, no information about closest occupied cell."<<std::endl;
else
std::cout<<"closest occupied cell to 30,67,33: "<< closest.x<<","<<closest.y<<","<<closest.z<<std::endl;
return 0;
}
int main() {
//testTensors();
//system("pause");
//return 0;
vector<BlendShape> shapes;
const int nShapes = 150; // 150 identity
const int nExprs = 47; // 46 expressions + 1 neutral
const int nVerts = 11510; // 11510 vertices for each mesh
const string path = "C:\\Users\\Peihong\\Desktop\\Data\\FaceWarehouse_Data_0\\";
const string foldername = "Tester_";
const string bsfolder = "Blendshape";
const string filename = "shape.bs";
shapes.resize(nShapes);
for(int i=0;i<nShapes;i++) {
stringstream ss;
ss << path << foldername << (i+1) << "\\" << bsfolder + "\\" + filename;
shapes[i].read(ss.str());
}
int nCoords = nVerts * 3;
// create an order 3 tensor for the blend shapes
Tensor3<float> t(nShapes, nExprs, nCoords);
// fill in the data
for(int i=0;i<shapes.size();i++) {
const BlendShape& bsi = shapes[i];
for(int j=0;j<bsi.expressionCount();j++) {
const BlendShape::shape_t& bsij = bsi.expression(j);
for(int k=0, cidx = 0;k<nVerts;k++, cidx+=3) {
const BlendShape::vert_t& v = bsij[k];
t(i, j, cidx) = v.x;
t(i, j, cidx+1) = v.y;
t(i, j, cidx+2) = v.z;
}
}
}
cout << "Tensor assembled." << endl;
// create deformation map
Tensor2<float> distmap(nShapes, nVerts);
for(int i=0;i<shapes.size();i++) {
const BlendShape::shape_t& bsi0 = shapes[i].expression(0);
const BlendShape& bsi = shapes[i];
for(int j=1;j<bsi.expressionCount();j++) {
const BlendShape::shape_t& bsij = bsi.expression(j);
for(int k=0, cidx = 0;k<nVerts;k++, cidx+=3) {
const BlendShape::vert_t& v0 = bsi0[k];
const BlendShape::vert_t& v = bsij[k];
float dx = v.x - v0.x, dy = v.y - v0.y, dz = v.z - v0.z;
distmap(i, k) += sqrt(dx*dx+dy*dy+dz*dz);
}
}
}
distmap.write("distmap.txt");
// perform svd to get core tensor
cout << "Performing SVD on the blendshapes ..." << endl;
int ms[2] = {0, 1}; // only the first two modes
int ds[2] = {150, 47}; // pick 50 for identity and 25 for expression
vector<int> modes(ms, ms+2);
vector<int> dims(ds, ds+2);
auto comp2 = t.svd(modes, dims);
cout << "SVD done." << endl;
auto tcore = std::get<0>(comp2);
auto tus = std::get<1>(comp2);
cout << "writing core tensor ..." << endl;
tcore.write("blendshape_core.tensor");
cout << "writing U tensors ..." << endl;
for(int i=0;i<tus.size();i++) {
tus[i].write("blendshape_u_" + toString(ms[i]) + ".tensor");
}
cout << "Validation begins ..." << endl;
Tensor3<float> tin;
tin.read("blendshape_core.tensor");
cout << "Core tensor dimensions = "
<< tin.dim(0) << "x"
<< tin.dim(1) << "x"
<< tin.dim(2) << endl;
float maxDiffio = 0;
for(int i=0;i<tin.dim(0);i++) {
for(int j=0;j<tin.dim(1);j++) {
for(int k=0;k<tin.dim(2);k++) {
maxDiffio = std::max(fabs(tin(i, j, k) - tcore(i, j, k)), maxDiffio);
}
}
}
cout << "Max difference io = " << maxDiffio << endl;
tin = tin.modeProduct(tus[0], 0).modeProduct(tus[1], 1);
cout << "Dimensions = "
<< tin.dim(0) << "x"
<< tin.dim(1) << "x"
<< tin.dim(2) << endl;
float maxDiff = 0;
for(int i=0;i<tin.dim(0);i++) {
for(int j=0;j<tin.dim(1);j++) {
for(int k=0;k<tin.dim(2);k++) {
maxDiff = std::max(fabs(tin(i, j, k) - t(i, j, k)), maxDiff);
}
}
}
cout << "Max difference = " << maxDiff << endl;
cout << "done" << endl;
system("pause");
return 0;
}
