Mesh Mesh::fromMap(const Map &depth, const Map &intensities, bool centralizeLoadedMesh)
{
if ((depth.w != intensities.w) || (depth.h != intensities.h))
throw FACELIB_EXCEPTION("incompatibile input map sizes");
std::map<std::pair<int,int>, int> coordToIndex;
VectorOfPoints points;
Colors colors;
int index = 0;
for (int y = 0; y < depth.h; y++)
{
for (int x = 0; x < depth.w; x++)
{
if (depth.isSet(x,y))
{
if (!intensities.isSet(x,y)) throw FACELIB_EXCEPTION("intensities point is not valid");
points.push_back(cv::Point3d(x, depth.h-y-1, depth.get(x,y)));
uchar intensity = intensities.get(x, y);
colors.push_back(cv::Vec3b(intensity, intensity, intensity));
coordToIndex[std::pair<int,int>(x,y)] = index;
index++;
}
}
}
Mesh mesh = Mesh::fromPointcloud(points, centralizeLoadedMesh, false);
mesh.colors = colors;
// triangles
for (int y = 0; y < depth.h; y++)
{
for (int x = 0; x < depth.w; x++)
{
if (depth.isSet(x,y) &&
depth.isValidCoord(x, y+1) && depth.isSet(x, y+1) &&
depth.isValidCoord(x+1, y+1) && depth.isSet(x+1, y+1))
{
mesh.triangles.push_back(cv::Vec3i(coordToIndex[std::pair<int,int>(x,y)], coordToIndex[std::pair<int,int>(x,y+1)], coordToIndex[std::pair<int,int>(x+1,y+1)]));
}
if (depth.isSet(x,y) &&
depth.isValidCoord(x+1, y+1) && depth.isSet(x+1, y+1) &&
depth.isValidCoord(x+1, y) && depth.isSet(x+1, y))
{
mesh.triangles.push_back(cv::Vec3i(coordToIndex[std::pair<int,int>(x,y)], coordToIndex[std::pair<int,int>(x+1,y+1)], coordToIndex[std::pair<int,int>(x+1,y)]));
}
}
}
return mesh;
}
XMeansClustering::VectorOfPoints XMeansClustering::GetPointsWithLabel(const unsigned int label)
{
VectorOfPoints points;
std::vector<unsigned int> indicesWithLabel = GetIndicesWithLabel(label);
for(unsigned int i = 0; i < indicesWithLabel.size(); i++)
{
points.push_back(this->Points[indicesWithLabel[i]]);
}
return points;
}
Mesh Mesh::fromXYZ(const std::string &filename, bool centralizeLoadedMesh)
{
std::ifstream in(filename);
if (!in.is_open())
throw FACELIB_EXCEPTION("Can't open file " + filename);
double x,y,z;
VectorOfPoints points;
while (in >> x >> y >> z)
{
cv::Point3d p(x, y, z);
points.push_back(p);
}
return Mesh::fromPointcloud(points, centralizeLoadedMesh);
}
Mesh Mesh::fromMap(const Map &depth, const MapConverter &converter)
{
int w = depth.w;
int h = depth.h;
VectorOfPoints points;
for (int y = 0; y < h; y++)
{
for (int x = 0; x < w; x++)
{
bool ok;
cv::Point3d p = converter.MapToMeshCoords(depth, cv::Point2d(x, y), &ok);
if (ok) points.push_back(p);
}
}
return Mesh::fromPointcloud(points);
}
Mesh Mesh::fromPointcloud(const VectorOfPoints &pointcloud, bool centralizeLoadedMesh, bool calculateTriangles)
{
Mesh m;
int n = pointcloud.size();
m.pointsMat = Matrix(n, 3);
for (int i = 0; i < n; i++)
{
const cv::Point3d &p = pointcloud.at(i);
m.pointsMat(i, 0) = p.x;
m.pointsMat(i, 1) = p.y;
m.pointsMat(i, 2) = p.z;
}
if (calculateTriangles) m.calculateTriangles();
m.recalculateMinMax();
if (centralizeLoadedMesh) m.centralize();
return m;
}
void XMeansClustering::SplitClusters()
{
assert(this->Points.size() > 0);
VectorOfPoints newClusterCenters;
for(unsigned int clusterId = 0; clusterId < this->ClusterCenters.size(); ++clusterId)
{
// Generate a random direction
PointType randomUnitVector = EigenHelpers::RandomUnitVector<PointType>(this->Points[0].size());
// Get the bounding box of the points that belong to this cluster
PointType minCorner;
PointType maxCorner;
EigenHelpers::GetBoundingBox(this->Points, minCorner, maxCorner);
// Scale the unit vector by the size of the region
PointType splitVector = randomUnitVector * (maxCorner - minCorner) / 2.0f;
PointType childCenter1 = this->ClusterCenters[clusterId] + splitVector;
PointType childCenter2 = this->ClusterCenters[clusterId] + splitVector;
// Compute the BIC of the original model
float BIC_parent;
// Compute the BIC of the new (split) model
float BIC_children;
// If the split was useful, keep it
if(BIC_children < BIC_parent)
{
newClusterCenters.push_back(childCenter1);
newClusterCenters.push_back(childCenter2);
}
else
{
newClusterCenters.push_back(this->ClusterCenters[clusterId]);
}
}
this->ClusterCenters = newClusterCenters;
}
Mesh Mesh::fromOBJ(const std::string &filename, bool centralizeLoadedMesh)
{
std::ifstream in(filename);
if (!in.is_open())
throw FACELIB_EXCEPTION("Can't open file " + filename);
VectorOfPoints points;
Triangles triangles;
std::string line;
while (std::getline(in, line))
{
if (line.empty()) continue;
if (line[0] == 'v')
{
Poco::StringTokenizer tokens(line, " ");
double x = Poco::NumberParser::parseFloat(tokens[1]);
double y = Poco::NumberParser::parseFloat(tokens[2]);
double z = Poco::NumberParser::parseFloat(tokens[3]);
points.push_back(cv::Point3d(x,y,z));
}
else if (line[0] == 'f')
{
Poco::StringTokenizer tokens(line, " ");
int t1 = Poco::NumberParser::parse(tokens[1]) - 1;
int t2 = Poco::NumberParser::parse(tokens[2]) - 1;
int t3 = Poco::NumberParser::parse(tokens[3]) - 1;
triangles.push_back(cv::Vec3i(t1, t2, t3));
}
}
Mesh result = Mesh::fromPointcloud(points, centralizeLoadedMesh, false);
result.triangles = triangles;
return result;
}
void MeshMassPropertiesTests::testClosedTetrahedronMesh() {
// given a tetrahedron as a closed mesh of four tiangles
// verify MeshMassProperties computes the right nubers
// these numbers from the Tonon paper:
VectorOfPoints points;
points.push_back(btVector3(8.33220f, -11.86875f, 0.93355f));
points.push_back(btVector3(0.75523f, 5.00000f, 16.37072f));
points.push_back(btVector3(52.61236f, 5.00000f, -5.38580f));
points.push_back(btVector3(2.00000f, 5.00000f, 3.00000f));
btScalar expectedVolume = 1873.233236f;
btMatrix3x3 expectedInertia;
expectedInertia[0][0] = 43520.33257f;
expectedInertia[1][1] = 194711.28938f;
expectedInertia[2][2] = 191168.76173f;
expectedInertia[1][2] = -4417.66150f;
expectedInertia[2][1] = -4417.66150f;
expectedInertia[0][2] = 46343.16662f;
expectedInertia[2][0] = 46343.16662f;
expectedInertia[0][1] = -11996.20119f;
expectedInertia[1][0] = -11996.20119f;
btVector3 expectedCenterOfMass = 0.25f * (points[0] + points[1] + points[2] + points[3]);
VectorOfIndices triangles;
pushTriangle(triangles, 0, 2, 1);
pushTriangle(triangles, 0, 3, 2);
pushTriangle(triangles, 0, 1, 3);
pushTriangle(triangles, 1, 2, 3);
// compute mass properties
MeshMassProperties mesh(points, triangles);
// verify
QCOMPARE_WITH_ABS_ERROR(mesh._volume, expectedVolume, acceptableRelativeError * expectedVolume);
QCOMPARE_WITH_ABS_ERROR(mesh._centerOfMass, expectedCenterOfMass, acceptableAbsoluteError);
QCOMPARE_WITH_RELATIVE_ERROR(mesh._inertia, expectedInertia, acceptableRelativeError);
// test again, but this time shift the points so that the origin is definitely OUTSIDE the mesh
btVector3 shift = points[0] + expectedCenterOfMass;
for (int i = 0; i < (int)points.size(); ++i) {
points[i] += shift;
}
expectedCenterOfMass = 0.25f * (points[0] + points[1] + points[2] + points[3]);
// compute mass properties
mesh.computeMassProperties(points, triangles);
// verify
// QCOMPARE_WITH_ABS_ERROR(mesh._volume, expectedVolume, acceptableRelativeError * expectedVolume);
// QCOMPARE_WITH_ABS_ERROR(mesh._centerOfMass, expectedCenterOfMass, acceptableAbsoluteError);
// QCOMPARE_WITH_RELATIVE_ERROR(mesh._inertia, expectedInertia, acceptableRelativeError);
}
void MeshMassPropertiesTests::testOpenTetrahedonMesh() {
// given the simplest possible mesh (open, with one triangle)
// verify MeshMassProperties computes the right nubers
// these numbers from the Tonon paper:
VectorOfPoints points;
points.push_back(btVector3(8.33220f, -11.86875f, 0.93355f));
points.push_back(btVector3(0.75523f, 5.00000f, 16.37072f));
points.push_back(btVector3(52.61236f, 5.00000f, -5.38580f));
points.push_back(btVector3(2.00000f, 5.00000f, 3.00000f));
btScalar expectedVolume = 1873.233236f;
btMatrix3x3 expectedInertia;
expectedInertia[0][0] = 43520.33257f;
expectedInertia[1][1] = 194711.28938f;
expectedInertia[2][2] = 191168.76173f;
expectedInertia[1][2] = -4417.66150f;
expectedInertia[2][1] = -4417.66150f;
expectedInertia[0][2] = 46343.16662f;
expectedInertia[2][0] = 46343.16662f;
expectedInertia[0][1] = -11996.20119f;
expectedInertia[1][0] = -11996.20119f;
// test as an open mesh with one triangle
VectorOfPoints shiftedPoints;
shiftedPoints.push_back(points[0] - points[0]);
shiftedPoints.push_back(points[1] - points[0]);
shiftedPoints.push_back(points[2] - points[0]);
shiftedPoints.push_back(points[3] - points[0]);
VectorOfIndices triangles;
pushTriangle(triangles, 1, 2, 3);
btVector3 expectedCenterOfMass = 0.25f * (shiftedPoints[0] + shiftedPoints[1] + shiftedPoints[2] + shiftedPoints[3]);
// compute mass properties
MeshMassProperties mesh(shiftedPoints, triangles);
// verify
// (expected - actual) / expected > e ==> expected - actual > e * expected
QCOMPARE_WITH_ABS_ERROR(mesh._volume, expectedVolume, acceptableRelativeError * expectedVolume);
QCOMPARE_WITH_ABS_ERROR(mesh._centerOfMass, expectedCenterOfMass, acceptableAbsoluteError);
QCOMPARE_WITH_RELATIVE_ERROR(mesh._inertia, expectedInertia, acceptableRelativeError);
}
Mesh Mesh::fromABS(const std::string &filename, const std::string &texture, bool centralizeLoadedMesh)
{
cv::Mat_<cv::Vec3b> image;
if (!texture.empty())
image = cv::imread(texture);
std::ifstream in(filename);
if (!in.is_open())
throw FACELIB_EXCEPTION("can't open file " + filename);
std::string line;
int mapHeight;
in >> mapHeight;
std::getline(in, line);
int mapwidth;
in >> mapwidth;
std::getline(in, line);
std::getline(in, line);
int total = mapwidth*mapHeight;
std::vector<int> flags(total);
std::vector<double> xPoints(total);
std::vector<double> yPoints(total);
std::vector<double> zPoints(total);
for (int i = 0; i < total; i++)
in >> (flags[i]);
for (int i = 0; i < total; i++)
in >> (xPoints[i]);
for (int i = 0; i < total; i++)
in >> (yPoints[i]);
for (int i = 0; i < total; i++)
in >> (zPoints[i]);
VectorOfPoints points;
Colors colors;
for (int i = 0; i < total; i++)
{
if (flags[i])
{
cv::Point3d p;
p.x = xPoints[i];
p.y = yPoints[i];
p.z = zPoints[i];
points.push_back(p);
if (!texture.empty())
{
int x = i % 640;
int y = i / 640;
colors.push_back(image(y, x));
}
}
}
Mesh mesh = Mesh::fromPointcloud(points, centralizeLoadedMesh);
mesh.colors = colors;
return mesh;
}
void MeshMassPropertiesTests::testBoxAsMesh() {
// verify that a mesh box produces the same mass properties as the analytic box.
// build a box:
// /
// y
// /
// 6-------------------------7
// /| /|
// / | / |
// / 2----------------------/--3
// / / / /
// | 4-------------------------5 / --x--
// z | / | /
// | |/ |/
// 0 ------------------------1
btScalar x(5.0f);
btScalar y(3.0f);
btScalar z(2.0f);
VectorOfPoints points;
points.reserve(8);
points.push_back(btVector3(0.0f, 0.0f, 0.0f));
points.push_back(btVector3(x, 0.0f, 0.0f));
points.push_back(btVector3(0.0f, y, 0.0f));
points.push_back(btVector3(x, y, 0.0f));
points.push_back(btVector3(0.0f, 0.0f, z));
points.push_back(btVector3(x, 0.0f, z));
points.push_back(btVector3(0.0f, y, z));
points.push_back(btVector3(x, y, z));
VectorOfIndices triangles;
pushTriangle(triangles, 0, 1, 4);
pushTriangle(triangles, 1, 5, 4);
pushTriangle(triangles, 1, 3, 5);
pushTriangle(triangles, 3, 7, 5);
pushTriangle(triangles, 2, 0, 6);
pushTriangle(triangles, 0, 4, 6);
pushTriangle(triangles, 3, 2, 7);
pushTriangle(triangles, 2, 6, 7);
pushTriangle(triangles, 4, 5, 6);
pushTriangle(triangles, 5, 7, 6);
pushTriangle(triangles, 0, 2, 1);
pushTriangle(triangles, 2, 3, 1);
// compute expected mass properties analytically
btVector3 expectedCenterOfMass = 0.5f * btVector3(x, y, z);
btScalar expectedVolume = x * y * z;
btMatrix3x3 expectedInertia;
computeBoxInertia(expectedVolume, btVector3(x, y, z), expectedInertia);
// compute the mass properties using the mesh
MeshMassProperties mesh(points, triangles);
// verify
QCOMPARE_WITH_ABS_ERROR(mesh._volume, expectedVolume, acceptableRelativeError * expectedVolume);
QCOMPARE_WITH_ABS_ERROR(mesh._centerOfMass, expectedCenterOfMass, acceptableAbsoluteError);
// test this twice: _RELATIVE_ERROR doesn't test zero cases (to avoid divide-by-zero); _ABS_ERROR does.
QCOMPARE_WITH_ABS_ERROR(mesh._inertia, expectedInertia, acceptableAbsoluteError);
QCOMPARE_WITH_RELATIVE_ERROR(mesh._inertia, expectedInertia, acceptableRelativeError);
// These two macros impl this:
// for (int i = 0; i < 3; ++i) {
// for (int j = 0; j < 3; ++j) {
// if (expectedInertia [i][j] == btScalar(0.0f)) {
// error = mesh._inertia[i][j] - expectedInertia[i][j]; // COMPARE_WITH_ABS_ERROR
// if (fabsf(error) > acceptableAbsoluteError) {
// std::cout << __FILE__ << ":" << __LINE__ << " ERROR : inertia[" << i << "][" << j << "] off by "
// << error << " absolute"<< std::endl;
// }
// } else {
// error = (mesh._inertia[i][j] - expectedInertia[i][j]) / expectedInertia[i][j]; // COMPARE_WITH_RELATIVE_ERROR
// if (fabsf(error) > acceptableRelativeError) {
// std::cout << __FILE__ << ":" << __LINE__ << " ERROR : inertia[" << i << "][" << j << "] off by "
// << error << std::endl;
// }
// }
// }
// }
}
