（二）使用gtsam做多个旋转的平均

本实验对五个旋转矩阵做平均，利用gtsam的先验信息对矩阵求平均，优化器采用DOGLEG

#include <random>
#include <gtsam/base/Vector.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/PriorFactor.h>

#define degree (M_PI/180)
int main()
{
    using gtsam::symbol_shorthand::R;
    constexpr int N = 5;
    gtsam::Rot3 rot[N] = {
        gtsam::Rot3::RzRyRx(0, 0,( 40)*degree),
        gtsam::Rot3::RzRyRx(0, 0,( 40)*degree),
        gtsam::Rot3::RzRyRx(0, 0,( 60)*degree),
        gtsam::Rot3::RzRyRx(0, 0,( 60)*degree),
        gtsam::Rot3::RzRyRx(0, 0,( 60)*degree),
    };
    
    gtsam::Values initial;

    gtsam::NonlinearFactorGraph graph;
    for (int i = 0; i < N; ++i) {
    
        auto cov = gtsam::noiseModel::Isotropic::Sigma(3,1);
        graph.addPrior<gtsam::Rot3>(R(0), rot[i], cov);
    }

    initial.insert(R(0), gtsam::Rot3::RzRyRx(0.1*degree, 0.2* degree, (0)*degree));

    gtsam::DoglegOptimizer opt(graph, initial);

    auto res = opt.optimize();
    res.print("final res");

    gtsam::Rot3 finalRot = res.at<gtsam::Rot3>(R(0));
    std::cout <<"final rot:"<< finalRot.xyz().transpose() / degree << "\n";

    std::system("pause");
    return 0;
}