1_7后端优化

news2024/7/4 4:23:59

        后端优化是指将一段时间内相机所有关键帧的位姿、内参、每个点3维坐标作为参数进行优化,得到最优的内、外参;利用的方法主要是Bundle Adjustment。

        所谓Bundle Adjustment可以理解为从任意特征点发射出来的几束光线,它们会在几个相机的成像平面上变成像素或是检测到的特征点。如果我们调整各相机姿态和各特征点的空间位置,使得这些光线最终收束到相机光心,简称BA。

        相机投影模型和BA代价函数

        相机投影过程是从一个世界坐标系中的点p出发,把相机的内外参数和畸变考虑进来,最后投影成像素坐标。具体步骤:

世界坐标     ->      相机坐标系   ->    相机归一化坐标系   -> 成像平面归一化坐标系  ->  像素坐标系

P=[X,Y,Z]  P_{cam}=[X^{'},Y^{'},Z^{'}]^T   P_{c}=[u_{c}, v_{c}, 1] \\ = [\frac{X^{'}}{Z^{'}}, \frac{Y^{'}}{Z^{'}}, 1]^{T}    P_{c}^{'}=[u_{c}^{'},v_{c}^{'},1]            p=[u, v]

转换方式:

          P_{cam}=RP+t        除以Z方向距离       u_{c}^{'}=u_{c}(1+k_{1}r_{c}^{2}+k_{2}r_{c}^{4})\\ v_{c}^{'}=v_{c}(1+k_{1}r_{c}^{2}+k_{2}r_{c}^{4})      u=f_{x}u_{c}^{'}+c_{x}\\ v=f_{y}v_{c}^{'}+c_{y}

        于是,整个过程构建出了观测方程,即z=h(x,y)

        具体,这里的x指代此时相机的位姿,即外参R,t,对应的李群为T,李代数\xi。y为路标,即三维点p,而z为观测数据,即z=[u,v]。将整体观测数据都考虑进行,得到如下代价函数:

\frac{1}{2} \sum_{i=1}^{m}\sum_{j=1}^{n}\left \| e_{ij}^{2} \right \|^{2} =\frac{1}{2} \sum_{i=1}^{m}\sum_{j=1}^{n}\left \| z_{ij}-h(T_{i},p_{j}) \right \|^{2}

        对这个最小二乘进行求解,相当于对位姿和路标同时做了调整,也就是所谓的BA。这里的优化量可分为两部分,一部分是相机姿态、一部分是空间点;

        x_{c}=[\xi_{1},\xi_{2},...,\xi_{m}]^{T}\in R^{6m}\\ x_{p}=[p_{1},p_{2},...,p_{n}]^{T}\in R^{3n}

        于是,增加自变量增量:

        \frac{1}{2}\left \| f(x+\Delta x) \right \|^{2}=\frac{1}{2}\left \| e+F\Delta x_{c} + E \Delta x_{p} \right \|

        最后将转化为线性方程:H\Delta x=g

基于ceres的计算代码,参看视觉slam十四讲:

#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H

#include <iostream>
#include "ceres/ceres.h"
#include "rotation.h"

class SnavelyReprojectionError {
public:
    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;
    }

    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.h
#include <iostream>
#include <ceres/ceres.h>
#include "common.h"
#include "SnavelyReprojectionError.h"

using namespace std;

void SolveBA(BALProblem &bal_problem);

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "usage: bundle_adjustment_ceres bal_data.txt" << endl;
        return 1;
    }

    BALProblem bal_problem(argv[1]);
    bal_problem.Normalize();
    bal_problem.Perturb(0.1, 0.5, 0.5);
    bal_problem.WriteToPLYFile("initial.ply");
    SolveBA(bal_problem);
    bal_problem.WriteToPLYFile("final.ply");

    return 0;
}

void 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.
        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.
        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;
    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";
}

 基于g2o算法:

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel_impl.h>
#include <iostream>

#include "common.h"
#include "sophus/se3.hpp"

using namespace Sophus;
using namespace Eigen;
using namespace std;

/// 姿态和内参的结构
struct PoseAndIntrinsics {
    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) {
        auto 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;
};

/// 位姿加相机内参的顶点,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];
        double r2 = pc.squaredNorm();
        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) {}

    virtual bool write(ostream &out) const {}
};

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) {}

    virtual bool write(ostream &out) const {}
};

class EdgeProjection :
    public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void computeError() override {
        auto v0 = (VertexPoseAndIntrinsics *) _vertices[0];
        auto v1 = (VertexPoint *) _vertices[1];
        auto proj = v0->project(v1->estimate());
        _error = proj - _measurement;
    }

    // use numeric derivatives
    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}

};

void SolveBA(BALProblem &bal_problem);

int main(int argc, char **argv) {

    if (argc != 2) {
        cout << "usage: bundle_adjustment_g2o bal_data.txt" << endl;
        return 1;
    }

    BALProblem bal_problem(argv[1]);
    bal_problem.Normalize();
    bal_problem.Perturb(0.1, 0.5, 0.5);
    bal_problem.WriteToPLYFile("initial.ply");
    SolveBA(bal_problem);
    bal_problem.WriteToPLYFile("final.ply");

    return 0;
}

void 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();

    // pose dimension 9, landmark is 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);

    /// build g2o problem
    const double *observations = bal_problem.observations();
    // vertex
    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;
        v->setId(i);
        v->setEstimate(PoseAndIntrinsics(camera));
        optimizer.addVertex(v);
        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;
        v->setId(i + bal_problem.num_cameras());
        v->setEstimate(Vector3d(point[0], point[1], point[2]));
        // g2o在BA中需要手动设置待Marg的顶点
        v->setMarginalized(true);
        optimizer.addVertex(v);
        vertex_points.push_back(v);
    }

    // edge
    for (int i = 0; i < bal_problem.num_observations(); ++i) {
        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);
    }

    optimizer.initializeOptimization();
    optimizer.optimize(40);

    // set to 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];
    }
}

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/598975.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

寄存器-汇编复习(2)

通过阅读本文小节内容&#xff0c;可以清楚的明白汇编承接的能力和机器语言&#xff0c;高级语言之间的表达关系。文中虽然讨论16位cpu&#xff0c;最新的64或以后的128理论都一样的&#xff0c;类推就好了。 继续将 通用寄存器-汇编复习(1)_luozhonghua2000的博客-CSDN博客 …

很多人打商标的主意,悄悄埋伏

很多人在打商标的主意&#xff0c;等着抢劫呢 等你的品牌结了果实&#xff0c;然后出手勒索 趣讲大白话&#xff1a;鬼子进村&#xff0c;打枪的不要 【趣讲信息科技183期】 **************************** 有些公司申请几千件商标 有公司一个月申请100件 春江水暖贼先知 有一条…

WSL2网络配置

WSL2越来越好用了&#xff0c;但是在windows下使用clash时&#xff0c;配置WSL2的网络很麻烦&#xff0c;通常使用设置环境变量export ALL_PROXY"http://127.0.0.1:8000"的方式有很大的弊端&#xff0c;例如pip和conda就不会走代理。 经过我亲身体验&#xff0c;最好…

基于小脑模型神经网络的轨迹跟踪研究(Matlab代码实现)

&#x1f4a5;&#x1f4a5;&#x1f49e;&#x1f49e;欢迎来到本博客❤️❤️&#x1f4a5;&#x1f4a5; &#x1f3c6;博主优势&#xff1a;&#x1f31e;&#x1f31e;&#x1f31e;博客内容尽量做到思维缜密&#xff0c;逻辑清晰&#xff0c;为了方便读者。 ⛳️座右铭&a…

模拟退火算法(附简单案例及详细matlab源码)

作者&#xff1a;非妃是公主 专栏&#xff1a;《智能优化算法》 博客地址&#xff1a;https://blog.csdn.net/myf_666 个性签&#xff1a;顺境不惰&#xff0c;逆境不馁&#xff0c;以心制境&#xff0c;万事可成。——曾国藩 文章目录 专栏推荐序一、概论二、物理退火1. 加温…

MyBatis - CRUD 操作

文章目录 1.环境配置1.1 导入相关依赖1.2 基本配置1.3 数据模型 2.基于 XML 开发2.1 创建 Mapper 接口2.2 创建 XML 映射文件2.3 insert2.4 select2.5 delete2.6 update2.7 编写单元测试 3.基于注解开发3.1 常用注解3.2 创建 Mapper 接口 MyBatis 支持通过 XML 和注解两种方式来…

chatgpt赋能python:Python运行程序没反应怎么办?

Python运行程序没反应怎么办&#xff1f; Python作为一种高级编程语言&#xff0c;已经成为了很多开发者的首选语言。然而&#xff0c;在使用Python编写程序时&#xff0c;有时候会出现运行程序却没有任何反应的情况。这是什么原因导致的呢&#xff1f;本文将为大家介绍Python…

单例模式的饿/懒汉模式

目录 1. 什么是单例模式2. 饿汉模式2.1 饿汉模式概念2.2 饿汉模式代码 3. 懒汉模式3.1 懒汉模式概念3.2 单线程情况下的懒汉模式3.3 单例模式的写法(保证线程安全) 4. wait 和 sleep 的区别 1. 什么是单例模式 保证某个类在程序中只存在一份实例&#xff0c;而不会创建多个实例…

Apache Kafka - 跨集群数据镜像 MirrorMaker

文章目录 概述跨集群数据镜像的原理MirrorMaker配置小结 概述 在分布式系统中&#xff0c;数据镜像是一项重要的功能&#xff0c;它可以将数据从一个集群复制到另一个集群&#xff0c;以保证数据的高可用性和容错性。Apache Kafka是一个流处理平台&#xff0c;它提供了一种跨集…

程序设计综合实习(C语言):学生成绩单制作

一、目的 1&#xff0e;掌握结构体变量及数组的定义、赋值、初始化、输入、输出 2&#xff0e;结构体数组的操作。 二、实习环境 Visual Stdio 2022 三、实习内容、步骤与要求 1&#xff0e;定义一个结构体数组&#xff0c;存放10个学生的学号&#xff0c;姓名&#xff0c;三…

Linux 设备树文件手动编译的 shell 脚本

前言 前面通过 Makefile 实现手动编译 Linux 设备树 dts 源文件及其 设备树依赖 dtsi、.h 头文件&#xff0c;如何写成一个 shell 脚本&#xff0c;直接编译呢&#xff1f; 其实就是 把 Makefile 重新编写为 shell 脚本即可 编译设备树 shell 脚本 脚本内容如下&#xff1a…

【六一 iKun】Happy LiuYi, iKuns

六一了&#xff0c;放松下。 Python iKun from turtle import * screensize(1000,1000) speed(6)#把衣服画出来&#xff0c;从右肩膀开始#领子 penup() goto(-141,-179) pensize(3) fillcolor("black") pencolor("black") begin_fill() pendown() left(1)…

【Python实战】Python采集高校信息

前言 大家好,我们今天来爬取某站的高校名单,把其高校名单,成员和内容数获取下来,不过,我们发现这个网站比我们平时多了一个验证,下面看看我是怎么解决的。 环境使用 python 3.9pycharm模块使用 requests模块介绍 requests requests是一个很实用的Python HTTP客户端…

【线性dp必学四道题】线性dp四道经典例题【最长上升子序列】、【最长公共子序列】、【最长公共上升子序列(maxv的由来)】【最长公共子串】

【最长上升子序列】、【最长公共子序列】、【最长公共上升子序列】 最长上升子序列f[i] 表示以i结尾的最长子序列 最长公共子序列f[i][j] 表示 a前i 和 b前j个 最长公共长度 最长公共上升子序列f[i][j]代表所有a[1 ~ i]和b[1 ~ j]中以b[j]结尾的公共上升子序列的集合 最长公共子…

Spring Boot如何实现分布式追踪和监控

Spring Boot如何实现分布式追踪和监控 在分布式系统中&#xff0c;由于服务数量的增加和服务之间的相互调用&#xff0c;会出现跨服务的请求链路较长&#xff0c;难以追踪问题和定位性能瓶颈等问题。因此&#xff0c;分布式追踪和监控变得越来越重要。本文将介绍如何使用 Spri…

怎样一元钱部署自己的AI网站

前段时间我开发了一个简洁的AI问答网站&#xff0c;好多朋友感兴趣&#xff0c;因此我将网站代码在github上开源&#xff0c;并编写此教程&#xff0c;帮助大家快速部署自己的AI网站&#xff0c;会编程的朋友们也可在此基础上定制开发。 前提条件&#xff1a;有自己的ChatGPT账…

AI实战营:开源计算机视觉神器OpenMMLab

目录 OpenMMLab概述 OpenMMLab各开源算法库详细介绍 OpenMMLab开源生态 OpenMMLab概述 部署框架&#xff1a;MMdeploy 算法框架&#xff1a;MMPretrain预训练多模态、MMDetection目标检测、MMDetection3D目标检测、MMRotate旋转目标检测、MMSegmentation语义分割、MMPose姿…

操作系统_进程

操作系统_进程 1 冯•诺依曼体系结构2 操作系统&#xff08;Operator System&#xff09;2.1 设计OS的目的2.2 OS的定位 3 进程3.1 什么是进程&#xff1f;3.2 查看进程3.3 通过fork创建进程使用 if 进行分流如何杀死一个进程 3.4 进程状态R - 运行状态S - 浅睡眠状态D - 磁盘休…

Java中关于ConditionObject的signal()方法的分析

代码块的展示 isHeldExclusively()这个仅持有锁资源的方法&#xff0c;在ReentrantLock中重写进行判断&#xff0c;要是没有持有锁资源那么会返回false&#xff0c;就会出现直接抛异常IllegalMonitorStateException&#xff08;非法监视器状态异常&#xff09;获取排在Conditi…

zookeeper相关,安装,认识......

目录 Zookeeper 1 第一章 Zookeeper简介... 1 1.1 Zookeeper概述和功能... 1 1.2 Zookeper安装... 1 1.3 Zookeper数据模型... 3 第二章 Zookeeper命令操作... 4 1.1 Zookeeper Client 4 1.2 Zookeeper JavaClient 6 第三章 集群角色... 28 Zookeeper 第一章 Zookeepe…