Exemplos de LevenbergMarquartMinimizer em C++ (Cpp)

Exemplo n.º 1

Arquivo: relative_pose_estimator.cpp Projeto: gpodevijn/nestk

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

Exemplo n.º 2

Arquivo: ScannerBlock.cpp Projeto: CNCBASHER/rgbdemo

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