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