gtsam的一个简单示例

最新推荐文章于 2024-06-17 22:13:25 发布

我绕过山腰雨声敲敲

最新推荐文章于 2024-06-17 22:13:25 发布

阅读量209

点赞数

文章标签： c++ c语言开发语言

本文链接：https://blog.csdn.net/qq_41568034/article/details/129463241

版权

#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Values.h>

int main()
{
  // Define variables
  gtsam::Pose3 pose1(gtsam::Rot3::Ypr(0.1, 0.2, 0.3), gtsam::Point3(1.0, 2.0, 3.0));
  gtsam::Pose3 pose2(gtsam::Rot3::Ypr(-0.1, 0.4, -0.5), gtsam::Point3(2.0, 3.0, 4.0));

  // Define factors
  gtsam::noiseModel::Diagonal::shared_ptr noiseModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.1, 0.1, 0.1));
  gtsam::BetweenFactor<gtsam::Pose3> factor(1, 2, gtsam::Pose3(gtsam::Rot3::Ypr(0.2, 0.3, 0.4), gtsam::Point3(1.0, 1.0, 1.0)), noiseModel);

  // Create factor graph and add factors
  gtsam::NonlinearFactorGraph graph;
  graph.add(factor);

  // Define initial values
  gtsam::Values initial;
  initial.insert(1, pose1);
  initial.insert(2, pose2);

  // Define optimizer
  gtsam::GaussNewtonOptimizer optimizer(graph, initial);

  // Optimize the graph
  gtsam::Values result = optimizer.optimize();

  // Get optimized values
  gtsam::Pose3 optimized_pose1 = result.at<gtsam::Pose3>(1);
  gtsam::Pose3 optimized_pose2 = result.at<gtsam::Pose3>(2);

  return 0;
}