之前章节总结了轴角以及李群李代数参数化，简单介绍了下四元数参数化，本节补充下四元数表示的位姿，如何利用视觉3D到2D重投影误差的残差及雅可比，完整代码如下

https://gitee.com/zl_vslam/slam_optimizer/blob/master/3d_optimize/ch5/pose_estimation_3d2d.cpphttps://gitee.com/zl_vslam/slam_optimizer/blob/master/3d_optimize/ch5/pose_estimation_3d2d.cpp

上述旋转点的公式展开为如下

由公式可得，注意这里的符号略微不同，但不影响结果

注意这个是一个3x4的矩阵

再来推导下方法二的雅可比：点坐标相对于四元数的雅可比矩阵

简单重排下

代码使用了更简洁的方法。首先构造一个中间矩阵：

再看下理论基础

实现过程中，往往直接使用ceres-solver自带的切空间处理方法，因此这里简单介绍下

ceres::QuaternionManifold* quaternion_manifold = new ceres::QuaternionManifold(); // w, x, y, z

这里Eigen版或者非Eigen版差别是四元数顺序不同，其它基本一致，因此只介绍一个即可，定义如下

// Implements the manifold for a Hamilton quaternion as defined in
// https://en.wikipedia.org/wiki/Quaternion. Quaternions are represented as
// unit norm 4-vectors, i.e.
//
// q = [q0; q1; q2; q3], |q| = 1
//
// is the ambient space representation.
//
//   q0  scalar part.
//   q1  coefficient of i.
//   q2  coefficient of j.
//   q3  coefficient of k.
//
// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
//
// The tangent space is R^3, which relates to the ambient space through the
// Plus and Minus operations defined as:
//
// Plus(x, delta) = [cos(|delta|); sin(|delta|) * delta / |delta|] * x
//    Minus(y, x) = to_delta(y * x^{-1})
//
// where "*" is the quaternion product and because q is a unit quaternion
// (|q|=1), q^-1 = [q0; -q1; -q2; -q3]
//
// and to_delta( [q0; u_{3x1}] ) = u / |u| * atan2(|u|, q0)
class CERES_EXPORT QuaternionManifold final : public Manifold {
 public:
  int AmbientSize() const override { return 4; }
  int TangentSize() const override { return 3; }

  bool Plus(const double* x,
            const double* delta,
            double* x_plus_delta) const override;
  bool PlusJacobian(const double* x, double* jacobian) const override;
  bool Minus(const double* y,
             const double* x,
             double* y_minus_x) const override;
  bool MinusJacobian(const double* x, double* jacobian) const override;
};

实现如下

这里的加和减区别不大，因此介绍其中之一即可，可以看到实现主要在基类中，主要Eigen版跟非Eigen版的顺序

上述代码的解释如下

四元数乘法

特殊情况处理

完整的表达方式

再来看雅可比相关部分

提取有效部分即为上面的雅可比矩阵

结果展示：

注意局部参数化我们直接使用ceres-solver内部的接口，因此外部雅可比只需要算到对四元数的导数，而不需要算局部参数化的雅可比

    bool Solve(const std::vector<Eigen::Vector3d>& landmarks,
               const std::vector<Eigen::Vector2d>& bearings,
               Eigen::Vector3d& initial_translation,
               Eigen::Quaterniond& initial_rotation) {
        
        if (landmarks.size() != bearings.size() || landmarks.empty()) {
            std::cerr << "Error: landmarks and bearings must have same non-zero size" << std::endl;
            return false;
        }
        
        // 将初始旋转转换为四元数（确保是单位四元数）
        Eigen::Quaterniond q = initial_rotation.normalized();
        
        // 优化变量
        double translation[3] = {initial_translation.x(), initial_translation.y(), initial_translation.z()};
        double quaternion[4] = {q.x(), q.y(), q.z(), q.w()}; // Eigen使用 [x,y,z,w] 格式
        
        // 创建问题
        ceres::Problem problem;
        problem.AddParameterBlock(translation, 3);

        // 添加四元数流形约束
        // ceres::EigenQuaternionManifold* quaternion_manifold = new ceres::EigenQuaternionManifold(); // w, x, y, z
        ceres::QuaternionManifold* quaternion_manifold = new ceres::QuaternionManifold(); // x, y, z, w
        problem.AddParameterBlock(quaternion, 4, quaternion_manifold);

        std::vector<Eigen::Vector3d> opt_landmarks = landmarks;
        // 为每个观测添加残差块（使用解析导数）
        for (size_t i = 0; i < landmarks.size(); ++i) {
            ceres::CostFunction* cost_function = 
                new AnalyticReprojectionError(landmarks[i], bearings[i], fx_, fy_, cx_, cy_);
            
            problem.AddResidualBlock(cost_function, 
                                   nullptr, // 使用默认的损失函数（L2范数）
                                   translation, 
                                   quaternion);

        }
        
        // 设置求解选项
        ceres::Solver::Options options;
        
        options.linear_solver_type = ceres::DENSE_SCHUR;
        options.minimizer_progress_to_stdout = false;
        options.max_num_iterations = 1000;
        options.function_tolerance = 1e-8;
        options.gradient_tolerance = 1e-12;
        options.parameter_tolerance = 1e-10;

        // 暂时禁用梯度检查以测试基本功能
        // options.check_gradients = false;
        // options.gradient_check_relative_precision = 1e-6;

        // 使用更保守的优化策略
        options.trust_region_strategy_type = ceres::DOGLEG;
        // options.dogleg_type = ceres::TRADITIONAL_DOGLEG;

        // 求解
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);
        
        std::cout << summary.BriefReport() << std::endl;
        
        std::cout << "Initial cost: " << summary.initial_cost << std::endl;
        std::cout << "Final cost: " << summary.final_cost << std::endl;
        
        // 输出优化结果
        if (summary.IsSolutionUsable()) {
            // 更新结果
            initial_translation = Eigen::Vector3d(translation[0], translation[1], translation[2]);
            
            // 注意：Eigen四元数构造函数的参数顺序是 (w, x, y, z)
            initial_rotation = Eigen::Quaterniond(quaternion[3], quaternion[0], quaternion[1], quaternion[2]);
            initial_rotation.normalize();
            
            std::cout << "Optimization successful!" << std::endl;
            std::cout << "Final translation: " << initial_translation.transpose() << std::endl;
            std::cout << "Final rotation quaternion: " << initial_rotation.coeffs().transpose() << std::endl;
            
            // 计算旋转矩阵和欧拉角（可选）
            Eigen::Matrix3d R_final = initial_rotation.toRotationMatrix();
            std::cout << "Final rotation matrix:\n" << R_final << std::endl;
            
            return true;
        } else {
            std::cerr << "Optimization failed!" << std::endl;
            return false;
        }
    }

主流程如上所示，主要就是利用四元数求的导数，来优化pnp的位姿，结果如下

如图所示，opencv结果展示的旋转和平移，平移部分几乎和我们的算的一摸一样，我们把旋转矩阵转换为四元数来看看结果

代入旋转矩阵

与我们的结果几乎一摸一样，足以证明算法的正确性