001 gtsam/examples CameraResectioning.cpp

xlsemmmmmm

已于 2022-10-23 22:00:55 修改

阅读量141

点赞数

分类专栏：因子图优化文章标签： c++

于 2022-10-23 21:55:39 首次发布

本文链接：https://blog.csdn.net/weixin_43848456/article/details/127481839

版权

因子图优化专栏收录该内容

27 篇文章 3 订阅

订阅专栏

001 CameraResectioning.cpp

一、已知
二、未知(待求)
三、类
四、主函数

说明：未知姿态的一个因素，由测量图像中已知3D点的投影产生

一、已知

3D点
2D投影
相机内参

二、未知(待求)

相机位姿

三、类

class ResectioningFactor: public NoiseModelFactor1<Pose3> {
  typedef NoiseModelFactor1<Pose3> Base;

  Cal3_S2::shared_ptr K_; ///< camera's intrinsic parameters
  Point3 P_;              ///< 3D point on the calibration rig
  Point2 p_;              ///< 2D measurement of the 3D point

public:

  /// Construct factor given known point P and its projection p
  ResectioningFactor(const SharedNoiseModel& model, const Key& key,
      const Cal3_S2::shared_ptr& calib, const Point2& p, const Point3& P) :
      Base(model, key), K_(calib), P_(P), p_(p) {
  }

  /// evaluate the error
  Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
      boost::none) const override {
    PinholeCamera<Cal3_S2> camera(pose, *K_);//内参和相机位姿构造针孔相机模型
    return camera.project(P_, H, boost::none, boost::none) - p_;//返回3D->2D -p的计算重投影误差
  }
};

四、主函数

int main(int argc, char* argv[]) {
  /* read camera intrinsic parameters */
  Cal3_S2::shared_ptr calib(new Cal3_S2(1, 1, 0, 50, 50));

  /* 1. create graph */
  NonlinearFactorGraph graph;

  /* 2. add factors to the graph */
  // add measurement factors
  SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5));
  boost::shared_ptr<ResectioningFactor> factor;
  graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(55, 45), Point3(10, 10, 0));
  graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(45, 45), Point3(-10, 10, 0));
  graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(45, 55), Point3(-10, -10, 0));
  graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(55, 55), Point3(10, -10, 0));

  /* 3. Create an initial estimate for the camera pose */
  Values initial;
  initial.insert(X(1),
      Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2)));

  /* 4. Optimize the graph using Levenberg-Marquardt*/
  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
  result.print("Final result:\n");

  return 0;
}