77 lines
2.8 KiB
C++
77 lines
2.8 KiB
C++
#include "Optimizer.h"
|
|
#include <glog/logging.h>
|
|
bool Optimizer::OptimizerB(const std::vector<Pose> &slams, const std::vector<Pose> >s, g2o::SE3Quat &B)
|
|
{
|
|
std::vector<Pose> slam_diff;
|
|
std::vector<Pose> gt_diff;
|
|
double time_interval = 1.0;
|
|
int i = 0;
|
|
int j = i + 1;
|
|
|
|
while (j < slams.size())
|
|
{
|
|
if (slams[j].ts - slams[i].ts >= time_interval)
|
|
{
|
|
Pose p;
|
|
p.ts = slams[j].ts;
|
|
p.Twb = slams[i].Twb.inverse() * slams[j].Twb;
|
|
slam_diff.push_back(p);
|
|
Pose q;
|
|
q.ts = gts[j].ts;
|
|
q.Twb = gts[i].Twb.inverse() * gts[j].Twb;
|
|
gt_diff.push_back(q);
|
|
i++;
|
|
}
|
|
j++;
|
|
}
|
|
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
|
|
typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
|
|
BlockSolverType::LinearSolverType *linear_solver = new LinearSolverType();
|
|
BlockSolverType *block_solver = new BlockSolverType(linear_solver);
|
|
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(block_solver);
|
|
g2o::SparseOptimizer optimizer; // 定义图模型
|
|
optimizer.setAlgorithm(solver);
|
|
optimizer.setVerbose(false);
|
|
g2o::VertexSE3Expmap *v1 = new g2o::VertexSE3Expmap();
|
|
v1->setId(0);
|
|
v1->setFixed(false);
|
|
optimizer.addVertex(v1);
|
|
|
|
const Eigen::Matrix<double, 6, 6> matlambda = Eigen::Matrix<double, 6, 6>::Identity();
|
|
|
|
for (int i = 0; i < gt_diff.size(); i++) // 添加边
|
|
{
|
|
Pose gt = gt_diff[i];
|
|
Pose slam = slam_diff[i];
|
|
MyunaryEdgeSE3_B *e = new MyunaryEdgeSE3_B(gt.Twb);
|
|
e->setId(i);
|
|
e->setVertex(0, v1);
|
|
e->setMeasurement(slam.Twb);
|
|
e->information() = matlambda;
|
|
optimizer.addEdge(e);
|
|
}
|
|
|
|
std::vector<std::vector<int>> rot_axis{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
|
|
for (auto tx : {0.0, -0.04, -0.02, 0.02, 0.04})
|
|
{
|
|
for (auto ty : {0.0, -0.04, -0.02, 0.02, 0.04})
|
|
{
|
|
for (auto tz : {0.0, -0.04, -0.02, 0.02, 0.04})
|
|
{
|
|
for (auto axis : rot_axis)
|
|
{
|
|
for (auto angle : {0.0, -M_PI / 2, M_PI / 2, M_PI})
|
|
{
|
|
Eigen::Quaterniond q(cos(angle / 2), axis[0] * sin(angle / 2), axis[1] * sin(angle / 2), axis[2] * sin(angle / 2));
|
|
Eigen::Vector3d t(tx, ty, tz);
|
|
g2o::SE3Quat init_B(q, t);
|
|
v1->setEstimate(init_B);
|
|
LOG(INFO) << "start optimization B";
|
|
LOG(INFO) << "init_B: "
|
|
<< "quat: " << v1->estimate().rotation() << ", trans: " << v1->estimate().translation().transpose();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} |