本实验对五个旋转矩阵做平均,利用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;
}