// 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; }
// atomic mean square pose estimation. double refinePoseUsingDepth(Pose3D& pose3d, const std::vector<cv::Point3f>& ref_points, const std::vector<cv::Point3f>& img_points) { std::vector<double> fx; std::vector<double> initial(6); pose_error_3d f(pose3d, ref_points, img_points); LevenbergMarquartMinimizer optimizer; std::fill(stl_bounds(initial), 0); fx.resize(ref_points.size()*3); ntk_dbg_print(f.normalizedOutputNorm(initial), 1); optimizer.minimize(f, initial); optimizer.diagnoseOutcome(1); f.evaluate(initial, fx); foreach_idx(i, fx) { ntk_dbg_print(fx[i], 1); }