BA后端优化
第七次作业的第2题BAL-dataset与书中所给示例代码类似，于是就学习了一下，让我写肯定写不出来。。。
整体思路如下：
文件结构：
.
├── bundle_adjustment_ceres.cpp
├── bundle_adjustment_g2o.cpp
├── cmake
│ ├── CeresConfig.cmake.in
│ ├── FindBLAS.cmake
│ ├── FindCSparse.cmake
│ ├── FindEigen3.cmake
│ ├── FindG2O.cmake
│ └── FindLAPACK.cmake
├── CMakeLists.txt
├── common.cpp
├── common.h
├── problem-16-22106-pre.txt
├── random.h
├── rotation.h
└── SnavelyReprojectionError.h1.通用部分
（1）读取数据
1）了解读入文件的格式
//第一部分 header
<相机数量> <3D点数量> <观测2D点数量>
//第二部分 某个相机观测某个点得到像素点坐标
<相机索引_1> <3D点索引_1> <观测2D点_x_1> <观测2D点_y_1>
...
<相机索引_1> <3D点索引_1> <观测2D点_x_1> <观测2D点_y_1>
//第三部分相机参数(9) 点参数(3)
<相机参数_1--1>
<相机参数_1--2>
...
<相机参数_1--8>
<相机参数_1--9>
...
...
<相机参数_n--1>
<相机参数_n--2>
...
<相机参数_n--8>
<相机参数_n--9>
<3D点_1_x>
<3D点_1_y>
<3D点_1_z>
...
<3D点_m_x>
<3D点_m_y>
<3D点_m_z>2）fscanf()函数
fscanf()函数的头文件是<stdio.h>，函数原型 为 int fscanf(FILE *stream, const char *format, [argument…]);
其功能为根据数据格式(format)从输入流(stream)中写入数据(argument)；
【参数】stream为文件指针，format为格式化字符串，argument 为格式化控制符对应的参数。
从文件指针stream指向的文件中，按format中对应的控制格式读取数据，并存储在agars对应的变量中
void FscanfOrDie(FILE *fptr, const char *format, T *value) {
//从stream流中连续读取能匹配format格式的字符串到参数列表中对应的变量里
int num_scanned = fscanf(fptr, format, value);
if (num_scanned != 1)
std::cerr << "Invalid UW data file. ";
}3）构造函数
1读出文件第一行相机数，点的个数以及观测个数
2读出哪一个相机[0,15]观测哪一个点[0,22105]得到观测的像素坐标值[x,y]
总观测数：83718
3参数依次读取
BALProblem::BALProblem(const std::string &filename, bool use_quaternions) {
FILE *fptr = fopen(filename.c_str(), "r"); //打开只读文件
if (fptr == NULL) {
std::cerr << "Error: unable to open file " << filename;
return;
};
// This wil die horribly on invalid files. Them's the breaks.
FscanfOrDie(fptr, "%d", &num_cameras_);
FscanfOrDie(fptr, "%d", &num_points_);
FscanfOrDie(fptr, "%d", &num_observations_);
std::cout << "Header: " << num_cameras_
<< " " << num_points_
<< " " << num_observations_;
point_index_ = new int[num_observations_];
camera_index_ = new int[num_observations_];
observations_ = new double[2 * num_observations_]; //观测的像素点
num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
parameters_ = new double[num_parameters_];
for (int i = 0; i < num_observations_; ++i) {
FscanfOrDie(fptr, "%d", camera_index_ + i);
FscanfOrDie(fptr, "%d", point_index_ + i);
for (int j = 0; j < 2; ++j) {
FscanfOrDie(fptr, "%lf", observations_ + 2 * i + j);
}
}
for (int i = 0; i < num_parameters_; ++i) {
FscanfOrDie(fptr, "%lf", parameters_ + i);
}
fclose(fptr);
use_quaternions_ = use_quaternions;
if (use_quaternions) {
// Switch the angle-axis rotations to quaternions.
num_parameters_ = 10 * num_cameras_ + 3 * num_points_;
double *quaternion_parameters = new double[num_parameters_];
double *original_cursor = parameters_;
double *quaternion_cursor = quaternion_parameters;
for (int i = 0; i < num_cameras_; ++i) {
AngleAxisToQuaternion(original_cursor, quaternion_cursor); //将角轴转化为四元数
quaternion_cursor += 4;
original_cursor += 3;
for (int j = 4; j < 10; ++j) {
*quaternion_cursor++ = *original_cursor++;
}
}
// Copy the rest of the points.
for (int i = 0; i < 3 * num_points_; ++i) {
*quaternion_cursor++ = *original_cursor++;
}
// Swap in the quaternion parameters.
delete[]parameters_;
parameters_ = quaternion_parameters;
}
}（2）处理数据
1）Normalize归一化
1求中位数
double Median(std::vector<double> *data) {
int n = data->size();
std::vector<double>::iterator mid_point = data->begin() + n / 2;
std::nth_element(data->begin(), mid_point, data->end());
return *mid_point;
}2更新tmp3绝对标准差 = 个别测定值 - 多次测定平均值之差
void BALProblem::Normalize() {
// Compute the marginal median of the geometry
std::vector<double> tmp(num_points_);
Eigen::Vector3d median;
double *points = mutable_points();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < num_points_; ++j) {
tmp[j] = points[3 * j + i];
}
median(i) = Median(&tmp);
}
for (int i = 0; i < num_points_; ++i) {
VectorRef point(points + 3 * i, 3);
tmp[i] = (point - median).lpNorm<1>();
}
const double median_absolute_deviation = Median(&tmp);
// Scale so that the median absolute deviation of the resulting
// reconstruction is 100
const double scale = 100.0 / median_absolute_deviation;
// X = scale * (X - median)
for (int i = 0; i < num_points_; ++i) {
VectorRef point(points + 3 * i, 3);
point = scale * (point - median);
}
double *cameras = mutable_cameras();
double angle_axis[3];
double center[3];
for (int i = 0; i < num_cameras_; ++i) {
double *camera = cameras + camera_block_size() * i;
CameraToAngelAxisAndCenter(camera, angle_axis, center);
// center = scale * (center - median)
VectorRef(center, 3) = scale * (VectorRef(center, 3) - median);
AngleAxisAndCenterToCamera(angle_axis, center, camera);
}
} 4对于相机的归一化，需要将文件中的相机有关数据提取出轴角，以及相机中心点在世界坐标系下的坐标
其中 ，所以
R为旋转矩阵，若想通过轴角得到R，需使用罗德里格斯公式：
所以
void BALProblem::CameraToAngelAxisAndCenter(const double *camera,
double *angle_axis,
double *center) const {
VectorRef angle_axis_ref(angle_axis, 3);
if (use_quaternions_) {
QuaternionToAngleAxis(camera, angle_axis);
} else {
angle_axis_ref = ConstVectorRef(camera, 3);
}
// c = -R't
Eigen::VectorXd inverse_rotation = -angle_axis_ref;
AngleAxisRotatePoint(inverse_rotation.data(), //data()函数返回一个指向矩阵或向量的首地址的指针
camera + camera_block_size() - 6,
center);
VectorRef(center, 3) *= -1.0;
}template<typename T>
// 求转换后的点
inline void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
const T theta2 = DotProduct(angle_axis, angle_axis);
if (theta2 > T(std::numeric_limits<double>::epsilon())) {
const T theta = sqrt(theta2);
const T costheta = cos(theta);
const T sintheta = sin(theta);
const T theta_inverse = 1.0 / theta;
const T w[3] = {angle_axis[0] * theta_inverse,
angle_axis[1] * theta_inverse,
angle_axis[2] * theta_inverse};
// Explicitly inlined evaluation of the cross product for
// performance reasons.
/*const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
w[2] * pt[0] - w[0] * pt[2],
w[0] * pt[1] - w[1] * pt[0] };*/
T w_cross_pt[3];
CrossProduct(w, pt, w_cross_pt);
const T tmp = DotProduct(w, pt) * (T(1.0) - costheta);
// (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);
result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
} else {
T w_cross_pt[3];
CrossProduct(angle_axis, pt, w_cross_pt);
result[0] = pt[0] + w_cross_pt[0];
result[1] = pt[1] + w_cross_pt[1];
result[2] = pt[2] + w_cross_pt[2];
}
}2）Purturb添加噪声
1产生随机数
#ifndef RAND_H
#define RAND_H
#include <math.h>
#include <stdlib.h>
inline double RandDouble()
{
double r = static_cast<double>(rand()); //sttatic_cast转换运算符
return r / RAND_MAX;
}
inline double RandNormal()
{
double x1, x2, w;
do{
x1 = 2.0 * RandDouble() - 1.0;
x2 = 2.0 * RandDouble() - 1.0;
w = x1 * x1 + x2 * x2;
}while( w >= 1.0 || w == 0.0);
w = sqrt((-2.0 * log(w))/w);
return x1 * w;
}
#endif // random.h2构建噪声函数
void PerturbPoint3(const double sigma, double *point) { //添加方差为sigma的高斯噪声
for (int i = 0; i < 3; ++i)
point[i] += RandNormal() * sigma;
}3给相机旋转，平移以及点的坐标添加噪声
void BALProblem::Perturb(const double rotation_sigma,
const double translation_sigma,
const double point_sigma) {
assert(point_sigma >= 0.0);
assert(rotation_sigma >= 0.0);
assert(translation_sigma >= 0.0);
double *points = mutable_points();
if (point_sigma > 0) {
for (int i = 0; i < num_points_; ++i) {
PerturbPoint3(point_sigma, points + 3 * i);
}
}
for (int i = 0; i < num_cameras_; ++i) {
double *camera = mutable_cameras() + camera_block_size() * i;
double angle_axis[3];
double center[3];
// Perturb in the rotation of the camera in the angle-axis
// representation
CameraToAngelAxisAndCenter(camera, angle_axis, center);
if (rotation_sigma > 0.0) {
PerturbPoint3(rotation_sigma, angle_axis);
}
AngleAxisAndCenterToCamera(angle_axis, center, camera);
if (translation_sigma > 0.0)
PerturbPoint3(translation_sigma, camera + camera_block_size() - 6);
}
}（3）输出数据
1）写入普通文件
1fprinft()函数
2memcpy()函数 void *memcpy(void *str1, const void *str2, size_t n)
从存储区 str2 复制 n 个字节到存储区 str1
void BALProblem::WriteToFile(const std::string &filename) const {
FILE *fptr = fopen(filename.c_str(), "w"); //打开可写文件
if (fptr == NULL) {
std::cerr << "Error: unable to open file " << filename;
return;
}
fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_);
for (int i = 0; i < num_observations_; ++i) {
fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]);
for (int j = 0; j < 2; ++j) {
fprintf(fptr, " %g", observations_[2 * i + j]);
}
fprintf(fptr, "\n");
}
// 输出相机坐标到文件
for (int i = 0; i < num_cameras(); ++i) {
double angleaxis[9];
if (use_quaternions_) {
//OutPut in angle-axis format.
QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis);
memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double));
} else {
memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double));
}
for (int j = 0; j < 9; ++j) {
fprintf(fptr, "%.16g\n", angleaxis[j]);
}
}
const double *points = parameters_ + camera_block_size() * num_cameras_;
for (int i = 0; i < num_points(); ++i) {
const double *point = points + i * point_block_size();
for (int j = 0; j < point_block_size(); ++j) {
fprintf(fptr, "%.16g\n", point[j]);
}
}
fclose(fptr);
}（2）写入ply文件
void BALProblem::WriteToPLYFile(const std::string &filename) const {
std::ofstream of(filename.c_str());
of << "ply"
<< '\n' << "format ascii 1.0"
<< '\n' << "element vertex " << num_cameras_ + num_points_
<< '\n' << "property float x"
<< '\n' << "property float y"
<< '\n' << "property float z"
<< '\n' << "property uchar red"
<< '\n' << "property uchar green"
<< '\n' << "property uchar blue"
<< '\n' << "end_header" << std::endl;
// Export extrinsic data (i.e. camera centers) as green points.
double angle_axis[3];
double center[3];
for (int i = 0; i < num_cameras(); ++i) {
const double *camera = cameras() + camera_block_size() * i;
CameraToAngelAxisAndCenter(camera, angle_axis, center);
of << center[0] << ' ' << center[1] << ' ' << center[2]
<< " 0 255 0" << '\n';
}
// Export the structure (i.e. 3D Points) as white points.
const double *points = parameters_ + camera_block_size() * num_cameras_;
for (int i = 0; i < num_points(); ++i) {
const double *point = points + i * point_block_size();
for (int j = 0; j < point_block_size(); ++j) {
of << point[j] << ' ';
}
of << " 255 255 255\n";
}
of.close();
}2.ceres_BA
（1）构建代价函数重载（）运算符
（2）构建寻优问题
（3）配置并运行求解器
#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H
#include <iostream>
#include "ceres/ceres.h"
#include "rotation.h"
//定义误差类，作用为在类中重载()运算符，写出误差的计算方法
class SnavelyReprojectionError {
public:
//1构建代价函数重载（）运算符
//构造函数，将观测坐标读进来
SnavelyReprojectionError(double observation_x, double observation_y) : observed_x(observation_x),
observed_y(observation_y) {}
//此处定义了一个模板()函数，定义误差计算方法，重投影误差，预测值减去观测值的误差
template<typename T>
bool operator()(const T *const camera,
const T *const point,
T *residuals) const {
// camera[0,1,2] are the angle-axis rotation
T predictions[2];
CamProjectionWithDistortion(camera, point, predictions);
// 计算误差，实质上就是重投影误差模型
residuals[0] = predictions[0] - T(observed_x);
residuals[1] = predictions[1] - T(observed_y);
return true;
}
// camera : 9 dims array
// [0-2] : angle-axis rotation
// [3-5] : translateion
// [6-8] : camera parameter, [6] focal length, [7-8] second and forth order radial distortion
// point : 3D location.
// predictions : 2D predictions with center of the image plane.
template<typename T>
static inline bool CamProjectionWithDistortion(const T *camera, const T *point, T *predictions) {
// Rodrigues' formula
T p[3];
AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Compute the center fo distortion
// 归一化平面，去除深度
T xp = -p[0] / p[2];
T yp = -p[1] / p[2];
// Apply second and fourth order radial distortion
const T &l1 = camera[7];
const T &l2 = camera[8];
// 径向畸变
T r2 = xp * xp + yp * yp;
T distortion = T(1.0) + r2 * (l1 + l2 * r2);
// 投影得到像素坐标
const T &focal = camera[6];
predictions[0] = focal * distortion * xp;
predictions[1] = focal * distortion * yp;
return true;
}
//2构建寻优问题
static ceres::CostFunction *Create(const double observed_x, const double observed_y) {
/*
创建一个自动求导的匿名对象
自动求导模板参数：误差类型(即代价函数模型) ，输入维度， 输出维度(有几个输出写几个)
参数：代价函数模型的匿名对象
*/
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
new SnavelyReprojectionError(observed_x, observed_y)));
}
private:
double observed_x;
double observed_y;
};
#endif // SnavelyReprojection.hvoid SolveBA(BALProblem &bal_problem) {
//读出路标和相机的维度
const int point_block_size = bal_problem.point_block_size();
const int camera_block_size = bal_problem.camera_block_size();
//读出路标和相机数据的首位置
double *points = bal_problem.mutable_points();
double *cameras = bal_problem.mutable_cameras();
// Observations is 2 * num_observations long array observations
// [u_1, u_2, ... u_n], where each u_i is two dimensional, the x
// and y position of the observation.
const double *observations = bal_problem.observations();
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
ceres::CostFunction *cost_function;
// Each Residual block takes a point and a camera as input
// and outputs a 2 dimensional Residual
cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);
// If enabled use Huber's loss function.
//定义problem->AddResidualBlock()函数中需要的Huber核函数
ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);
// Each observation corresponds to a pair of a camera and a point
// which are identified by camera_index()[i] and point_index()[i]
// respectively.
//定义problem->AddResidualBlock()函数中需要的待估计参数
double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];
double *point = points + point_block_size * bal_problem.point_index()[i];
problem.AddResidualBlock(cost_function, loss_function, camera, point);
}
// show some information here ...
std::cout << "bal problem file loaded..." << std::endl;
std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
<< bal_problem.num_points() << " points. " << std::endl;
std::cout << "Forming " << bal_problem.num_observations() << " observations. " << std::endl;
std::cout << "Solving ceres BA ... " << endl;
//设定求解办法，SPARSE_SCHUR会让ceres实时求解过程为：先对路标部分进行Schur边缘化，以加速的方式求解此问题。
ceres::Solver::Options options;
options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
}3.g2o_BA
（1）构建相机参数的结构
/// 姿态和内参的结构
struct PoseAndIntrinsics { //包含 R T f k1 k2
PoseAndIntrinsics() {}
/// set from given data address
explicit PoseAndIntrinsics(double *data_addr) {
rotation = SO3d::exp(Vector3d(data_addr[0], data_addr[1], data_addr[2]));
translation = Vector3d(data_addr[3], data_addr[4], data_addr[5]);
focal = data_addr[6];
k1 = data_addr[7];
k2 = data_addr[8];
}
/// 将估计值放入内存
void set_to(double *data_addr) {
Vector3d r = rotation.log();
for (int i = 0; i < 3; ++i) data_addr[i] = r[i];
for (int i = 0; i < 3; ++i) data_addr[i + 3] = translation[i];
data_addr[6] = focal;
data_addr[7] = k1;
data_addr[8] = k2;
}
SO3d rotation;
Vector3d translation = Vector3d::Zero();
double focal = 0;
double k1 = 0, k2 = 0;
};（2）定义顶点
因为顶点分为相机与观测点，所以需要两个顶点类。
/// 位姿加相机内参的顶点，9维，前三维为so3，接下去为t, f, k1, k2
class VertexPoseAndIntrinsics : public g2o::BaseVertex<9, PoseAndIntrinsics> { //优化变量维度 数据类型
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexPoseAndIntrinsics() {}
virtual void setToOriginImpl() override { //顶点初始化
_estimate = PoseAndIntrinsics();
}
virtual void oplusImpl(const double *update) override { //顶点估计值更新
_estimate.rotation = SO3d::exp(Vector3d(update[0], update[1], update[2])) * _estimate.rotation;
_estimate.translation += Vector3d(update[3], update[4], update[5]);
_estimate.focal += update[6];
_estimate.k1 += update[7];
_estimate.k2 += update[8];
}
/// 根据估计值投影一个点
Vector2d project(const Vector3d &point) {
Vector3d pc = _estimate.rotation * point + _estimate.translation;
pc = -pc / pc[2]; //归一化平面 BAL数据集说明
double r2 = pc.squaredNorm();
//去畸变 1 + k1*r2 + k2*r2*r2
double distortion = 1.0 + r2 * (_estimate.k1 + _estimate.k2 * r2);
return Vector2d(_estimate.focal * distortion * pc[0], //像素平面
_estimate.focal * distortion * pc[1]);
}
virtual bool read(istream &in) {return false;}
virtual bool write(ostream &out) const {return false;}
};
class VertexPoint : public g2o::BaseVertex<3, Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexPoint() {}
virtual void setToOriginImpl() override { //顶点初始化
_estimate = Vector3d(0, 0, 0);
}
virtual void oplusImpl(const double *update) override { //顶点估计值更新
_estimate += Vector3d(update[0], update[1], update[2]);
}
virtual bool read(istream &in) {return false;}
virtual bool write(ostream &out) const {return false;}
};（3）定义边
class EdgeProjection :
//<>内：2：误差项维数，Vector2d：输入数据的变量形式，与边相连的两点
public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
virtual void computeError() override {
VertexPoseAndIntrinsics *pose = static_cast<VertexPoseAndIntrinsics*> (_vertices[0]);
VertexPoint *point = static_cast<VertexPoint*> (_vertices[1]);
Vector2d proj = pose->project(point->estimate());
_error = proj - _measurement; //误差项
}
// 没有实现linearizeOplus（） 的重写时， g2o会自动提供一个数值计算的雅克比
// use numeric derivatives
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
};（4）构建图优化
1）实例化optimizer优化器，打开调试输出
// pose dimension 9, landmark is 3 T 6维 + point 3维 ，观测点 point 3维
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType; //块求解器
typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType; //线性求解器
// use LM
auto solver = new g2o::OptimizationAlgorithmLevenberg( //总求解器
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; //建立稀疏矩阵优化器
optimizer.setAlgorithm(solver); //设置求解器
optimizer.setVerbose(true); //打开调试输出2）实例化顶点
vector<VertexPoseAndIntrinsics *> vertex_pose_intrinsics;
vector<VertexPoint *> vertex_points;
for (int i = 0; i < bal_problem.num_cameras(); ++i) { //相机位姿顶点
VertexPoseAndIntrinsics *v = new VertexPoseAndIntrinsics();
double *camera = cameras + camera_block_size * i;
PoseAndIntrinsics estimate (camera);
v->setId(i);
v->setEstimate(estimate); //设置初始估计值
optimizer.addVertex(v); //增加pose顶点
vertex_pose_intrinsics.push_back(v);
}
for (int i = 0; i < bal_problem.num_points(); ++i) { //观测点顶点
VertexPoint *v = new VertexPoint();
double *point = points + point_block_size * i;
Eigen::Vector3d estimate (point[0], point[1], point[2]);
v->setId(i + bal_problem.num_cameras());
v->setEstimate(estimate);
// g2o在BA中需要手动设置待Marg的顶点
v->setMarginalized(true); //手动设置需要边缘化的顶点
optimizer.addVertex(v); //增加point顶点
vertex_points.push_back(v);
}3）实例化边
// edge
for (int i = 0; i < bal_problem.num_observations(); ++i) { //g2o的边是误差
EdgeProjection *edge = new EdgeProjection;
edge->setVertex(0, vertex_pose_intrinsics[bal_problem.camera_index()[i]]);
edge->setVertex(1, vertex_points[bal_problem.point_index()[i]]);
edge->setMeasurement(Vector2d(observations[2 * i + 0], observations[2 * i + 1])); //设置观测值
edge->setInformation(Matrix2d::Identity());
edge->setRobustKernel(new g2o::RobustKernelHuber()); //设置鲁棒核函数
optimizer.addEdge(edge);
}4）设置优化器参数，进行优化
optimizer.initializeOptimization();
optimizer.optimize(40);5）存储优化后的值
// set to bal problem 将优化后的值 储存到bal_problem
for (int i = 0; i < bal_problem.num_cameras(); ++i) {
double *camera = cameras + camera_block_size * i;
auto vertex = vertex_pose_intrinsics[i];
auto estimate = vertex->estimate();
estimate.set_to(camera);
}
for (int i = 0; i < bal_problem.num_points(); ++i) {
double *point = points + point_block_size * i;
auto vertex = vertex_points[i];
for (int k = 0; k < 3; ++k)
point[k] = vertex->estimate()[k];
}
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
