TriMesh<FloatType> TriMesh<FloatType>::flatLoopSubdivision(UINT iterations, float minEdgeLength) const
{
TriMeshf result = *this;
for (UINT i = 0; i < iterations; i++)
result = result.flatLoopSubdivision(minEdgeLength);
return result;
}
TriMeshf cylinder(const vec3f& p0, const vec3f& p1, float radius, UINT stacks, UINT slices, const vec4f& color) {
float height = (p1 - p0).length();
TriMeshf result = shapes::cylinder(radius, height, stacks, slices, color);
result.transform(mat4f::translation(p0) * mat4f::face(vec3f::eZ, p1 - p0));
return result;
}
TriMeshf box(const OBBf &obb, const vec4f& color)
{
TriMeshf result = box(bbox3f(vec3f::origin, vec3f(1.0f, 1.0f, 1.0f)), color);
result.transform(obb.getOBBToWorld());
return result;
}
TriMeshf box(const bbox3f &bbox, const vec4f& color)
{
auto extent = bbox.getExtent();
auto center = bbox.getCenter();
TriMeshf result = box(extent.x, extent.y, extent.z, color);
for (auto &v : result.getVertices())
{
v.position += center;
}
return result;
}
MeshEdges Segmentor::getEdges(const TriMeshf& mesh)
{
MeshEdges edges;
const vector<ml::vec3ui>& indices = mesh.getIndices();
for (int i = 0; i < mesh.getIndices().size(); i++)
{
UINT v1 = indices[i].x;
UINT v2 = indices[i].y;
UINT v3 = indices[i].z;
edges.push_back(std::make_pair(v1, v2));
edges.push_back(std::make_pair(v1, v3));
edges.push_back(std::make_pair(v3, v2));
}
return edges;
}
void Vizzer::init(ApplicationData &app)
{
assets.init(app.graphics);
vec3f eye(0.5f, 0.2f, 0.5f);
vec3f worldUp(0.0f, 1.0f, 0.0f);
camera = Cameraf(-eye, eye, worldUp, 60.0f, (float)app.window.getWidth() / app.window.getHeight(), 0.01f, 10000.0f);
camera = Cameraf("-0.774448 1.24485 -1.35404 0.999848 1.80444e-009 -0.0174517 0.0152652 -0.484706 0.874544 -0.00845866 -0.874677 -0.484632 0 1 0 60 1.25 0.01 10000");
//-0.774448 1.24485 -1.35404 0.999848 1.80444e-009 -0.0174517 0.0152652 -0.484706 0.874544 -0.00845866 -0.874677 -0.484632 0 1 0 60 1.25 0.01 10000
font.init(app.graphics, "Calibri");
state.bundler.loadSensorFile(constants::dataDir + "/sensors/sample.sensor");
state.bundler.computeKeypoints();
state.bundler.addCorrespondences(1);
state.bundler.addCorrespondences(2);
state.bundler.addCorrespondences(4);
state.bundler.addCorrespondences(6);
state.bundler.addCorrespondences(12);
state.bundler.addCorrespondences(20);
state.bundler.addCorrespondences(30);
state.bundler.addCorrespondences(40);
state.bundler.addCorrespondences(60);
state.bundler.addCorrespondences(80);
state.bundler.addCorrespondences(100);
state.bundler.solve();
state.bundler.thresholdCorrespondences(0.01);
state.bundler.solve();
//state.bundler.thresholdCorrespondences(0.005);
//state.bundler.solve();
state.bundler.saveKeypointCloud(constants::debugDir + "result.ply");
state.bundler.saveResidualDistribution(constants::debugDir + "residuals.csv");
state.frameCloudsSmall.resize(state.bundler.frames.size());
state.frameCloudsBig.resize(state.bundler.frames.size());
for (auto &frame : iterate(state.bundler.frames))
{
vector<TriMeshf> meshesSmall, meshesBig;
auto makeColoredBox = [](const vec3f ¢er, const vec4f &color, float radius) {
TriMeshf result = ml::Shapesf::box(radius, radius, radius);
result.transform(mat4f::translation(center));
result.setColor(color);
return result;
};
const int stride = 5;
for (auto &p : frame.value.depthImage)
{
vec2i coord((int)p.x, (int)p.y);
const vec3f framePos = frame.value.localPos(coord);
if (!framePos.isValid() || p.x % stride != 0 || p.y % stride != 0)
continue;
meshesSmall.push_back(makeColoredBox(frame.value.frameToWorld * framePos, vec4f(frame.value.colorImage(coord)) / 255.0f, 0.002f));
meshesBig.push_back(makeColoredBox(frame.value.frameToWorld * framePos, vec4f(frame.value.colorImage(coord)) / 255.0f, 0.005f));
}
state.frameCloudsSmall[frame.index] = D3D11TriMesh(app.graphics, Shapesf::unifyMeshes(meshesSmall));
state.frameCloudsBig[frame.index] = D3D11TriMesh(app.graphics, Shapesf::unifyMeshes(meshesBig));
}
state.selectedCamera = 0;
}
vector<UINT> SegmentorFelzenswalb::segment(const TriMeshf& mesh) const
{
size_t N = mesh.getVertices().size();
cout << "Segmenting mesh with " << N << " vertices...";
const MeshEdges& es = getEdges(mesh);
size_t Ne = es.size();
edge* edges = new edge[Ne];
const auto& verts = mesh.getVertices();
for (size_t i = 0; i < Ne; i++)
{
int a = es[i].first;
int b = es[i].second;
edges[i].a = a;
edges[i].b = b;
const vec3f& n1 = verts[a].normal;
const vec3f& n2 = verts[b].normal;
const vec3f& p1 = verts[a].position;
const vec3f& p2 = verts[b].position;
float dx = p2.x - p1.x;
float dy = p2.y - p1.y;
float dz = p2.z - p1.z;
float dd = sqrt(dx * dx + dy * dy + dz * dz);
dx /= dd; dy /= dd; dz /= dd;
float dot = vec3f::dot(n1, n2);
float dot2 = n2.x * dx + n2.y * dy + n2.z * dz;
float ww = 1.0f - dot;
if (dot2 > 0) { ww = ww * ww; } // make it much less of a problem if convex regions have normal difference
if (colorWeight > 0.0f)
{
const vec4f& c1 = verts[a].color;
const vec4f& c2 = verts[b].color;
//const float d = vec3f::dist(c1, c2);
//ww = (1 - m_colorWeight) * ww + m_colorWeight * d;
const vec4f& c1hsl = ml::ColorUtils::rgbToHsl<vec4f>(c1);
const vec4f& c2hsl = ml::ColorUtils::rgbToHsl<vec4f>(c2);
//const vec4f& c1rgb = ml::ColorUtils::hslToRgb<vec4f>(c1hsl);
//const vec4f& c2rgb = ml::ColorUtils::hslToRgb<vec4f>(c2hsl);
// Find difference in H and L, handling wrapping and make range [0,1]
const float hdiff = 2.0f * std::min(fabsf(c1hsl[0] - c2hsl[0]), fabsf(c2hsl[0] - c1hsl[0] + 1));
const float ldiff = 2.0f * std::min(fabsf(c1hsl[2] - c2hsl[2]), fabsf(c2hsl[2] - c1hsl[2] + 1));
const float hldiff = sqrtf(hdiff*hdiff + ldiff*ldiff);
ww = (1.0f - colorWeight) * ww + colorWeight * hldiff;
}
edges[i].w = ww;
}
cout << "graph done... ";
// Segment!
universe* u = segment_graph(static_cast<int>(N), static_cast<int>(Ne), edges, threshold);
cout << u->num_sets() << " initial segs. ";
// Join small segments
for (int j = 0; j < Ne; j++)
{
int a = u->find(edges[j].a);
int b = u->find(edges[j].b);
if ((a != b) && ((u->size(a) < minSegmentVertices) || (u->size(b) < minSegmentVertices))) {
u->join(a, b);
}
}
cout << " Done: " << u->num_sets() << " final segs" << endl;
// Return segment indices
vector<UINT> out(N);
for (int q = 0; q < N; q++)
{
out[q] = u->find(q);
}
delete u;
return out;
}
