void test_renderer(Mesh& mesh)
{
MeshRenderer renderer(800, 600);
renderer.setMesh(mesh);
Pose3D pose;
pose.parseAvsFile("mesh_pose.avs");
cv::Mat4b color_image (Size(800,600));
cv::Mat1f depth;
TimeCount tc_render("Rendering 100 times", 1);
for (int i = 0; i < 100; ++i)
{
pose.applyTransformAfter(cv::Vec3f(0.02f, 0, 0), cv::Vec3f(0, 0, 0));
TimeCount tc_render_one("Rendering", 1);
renderer.setMesh(mesh);
float near = 0.3f, far = 5.f;
renderer.setPose(pose, &near, &far);
renderer.renderToImage(color_image, 0);
tc_render_one.stop();
}
tc_render.stop();
// color_image = renderer.colorBuffer();
imwrite("debug_color.png", color_image);
depth = renderer.depthBuffer();
normalize(depth, depth, 0, 255, cv::NORM_MINMAX);
imwrite("debug_depth.png", Mat1b(depth));
}
// atomic mean square pose estimation.
double rms_optimize_3d(Pose3D& pose3d,
const std::vector<Point3f>& ref_points,
const std::vector<Point3f>& img_points)
{
std::vector<double> fx;
std::vector<double> initial(6);
reprojection_error_3d f(pose3d, ref_points, img_points);
LevenbergMarquartMinimizer optimizer;
std::fill(stl_bounds(initial), 0);
fx.resize(ref_points.size()*3);
optimizer.minimize(f, initial);
optimizer.diagnoseOutcome();
f.evaluate(initial, fx);
double error = f.outputNorm(initial);
pose3d.applyTransformAfter(Vec3f(initial[3],initial[4],initial[5]), cv::Vec3f(initial[0], initial[1], initial[2]));
return error;
}