第六章

[ztayou]

我看错误是来自于 CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}这串代码 不知道为什么？

[ztayou]

CurveFittingEdge* edge = new CurveFittingEdge(x_data[i]);设置断点的时候显示这个错误不知道怎么解决了

[gaoxiang12]

看上去某个地方出现了空指针。
可以查看一下call stack里的函数，看哪个变量是空的或者越界了。

[ztayou]

是不是这个resize 出错了啊 这里怎么改啊 老师

[ztayou]

[ztayou]

导致_val出现了空指针是不是~ 改怎么解决啊 ，老师

[ztayou]

我这个是在windows下 VS2019

[gaoxiang12]

第一张图里的this已经是空指针了，你应该再往前追溯问题原因。
你修改过代码吗？能否把完整代码贴上来？

[ztayou]

我没有改过代码~

[ztayou]

#include
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include
#include
#pragma warning (default:4716)
using namespace std;

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // 重置
    virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0;
}

// 更新
virtual void oplusImpl(const double* update) override {
    _estimate += Eigen::Vector3d(update);
}

// 存盘和读盘：留空
virtual bool read(istream& in) {}

virtual bool write(ostream& out) const {}

};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}

// 计算曲线模型误差
virtual void computeError() override {
    const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
}

// 计算雅可比矩阵
virtual void linearizeOplus() override {
    const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
}

virtual bool read(istream& in) {}

virtual bool write(ostream& out) const {}

public:
double _x; // x 值， y 值为 _measurement
};

int main() {
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器

vector<double> x_data, y_data;      // 数据
for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
    cout << x_data[i] << "," << y_data[i] << endl;
}
/*for (int i = 0; i < N; i++)
{
    cout << x_data[i] << "," << y_data[i] << endl;
}*/

// 构建图优化，先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型

// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer;     // 图模型
optimizer.setAlgorithm(solver);   // 设置求解器
optimizer.setVerbose(true);       // 打开调试输出

// 往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(ae, be, ce));
v->setId(0);
optimizer.addVertex(v);

// 往图中增加边
for (int i = 0; i < N; i++) {
    CurveFittingEdge* edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    edge->setVertex(0, v);                // 设置连接的顶点
    edge->setMeasurement(y_data[i]);      // 观测数值
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆
    optimizer.addEdge(edge);
}

// 执行优化
cout << "start optimization" << endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

// 输出优化值
Eigen::Vector3d abc_estimate = v->estimate();
cout << "estimated model: " << abc_estimate.transpose() << endl;

return 0;

}

[ztayou]

高老师 我想问一个纠结了很久的问题，求单应矩阵的时候，如果我用的四对匹配点，这四对匹配点是来自于空间中地面上四个点与对应的像素坐标的匹配关系。这四对匹配点是比较准确的那种。求出来的单应矩阵该怎么得到旋转矩阵和平移，是用分解法么？还是怎么做？ 真的谢谢你。

…

------------------&nbsp;原始邮件&nbsp;------------------ 发件人: "gaoxiang12/slambook" ***@***.***&gt;; 发送时间:&nbsp;2021年4月11日(星期天) 中午12:36 ***@***.***&gt;; ***@***.******@***.***&gt;; 主题:&nbsp;Re: [gaoxiang12/slambook] 第六章 (#280) 第一张图里的this已经是空指针了，你应该再往前追溯问题原因。 你修改过代码吗？能否把完整代码贴上来？ — You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub, or unsubscribe.