Backend Global Graph Optimization: Pose Parameterization and Jacobian Solution Scheme

Date: November 05, 2021

Pose Parameterization and Jacobian Solution for Backend Global Graph Optimization

Implementation Based on Quaternion Rotation and Translation Vector Representation with Automatic Differentiation

Assuming for time $i$ , the vehicle pose is represented as $x_{i} = [p_{i}^{T}, q_{i}^{T}]^{T}$ , where $p_{i}$ denotes the three-dimensional position vector and $q_{i}$ is a unit quaternion representing the vehicle orientation. For two time instances $i$ and $j$ , the residual term corresponding to the relative pose between vehicles is defined as

r_{i j} = [\begin{matrix} R (q_{i})^{T} (p_{j} - p_{i}) - {\hat{p}}_{i j} \\ 2 \cdot v e c ((q_{i}^{- 1} q_{j}) {\hat{q}}_{i j}^{- 1}) \end{matrix}]

where $v e c ()$ denotes the vector part of a quaternion, specifically $[q_{x}, q_{y}, q_{z}]^{T}$ , and $R (q)$ represents the rotation matrix corresponding to quaternion $q$ . The corresponding code implementation is in the Functor class EdgePoseError, used in the Ceres CostFunction class for computing the residual vector and its corresponding Jacobian.

	template <typename T>
	bool operator()(const T* const position_i, const T* const orientation_i, 
					const T* const position_j, const T* const orientation_j, 
					T* residual_ptr) const
	{
		Eigen::Map<const Eigen::Matrix<T,3,1>> p_i(position_i);
		Eigen::Map<const Eigen::Quaternion<T>> q_i(orientation_i);

		Eigen::Map<const Eigen::Matrix<T,3,1>> p_j(position_j);
		Eigen::Map<const Eigen::Quaternion<T>> q_j(orientation_j);

		Eigen::Quaternion<T> q_i_inv=q_i.conjugate();
		Eigen::Quaternion<T> q_ij=q_i_inv*q_j;
		Eigen::Matrix<T,3,1> p_ij=q_i_inv*(p_j-p_i);

		Eigen::Quaternion<T> q_ij_meas(pose_ij_meas_.linear().template cast<T>());
		Eigen::Quaternion<T> delta_q=q_ij_meas*q_ij.conjugate();

		Eigen::Map<Eigen::Matrix<T,6,1>> residual(residual_ptr);
		Eigen::Map<Eigen::Matrix<T,3,1>> residual_trs(residual_ptr);
		Eigen::Map<Eigen::Matrix<T,3,1>> residual_rot(residual_ptr+3);

		residual_trs=p_ij-pose_ij_meas_.translation().template cast<T>();
		residual_rot=T(2.0)*delta_q.vec();
		residual.applyOnTheLeft(sqrt_information_.template cast<T>());

		return true;
	}

The Jacobian solving utilizes the automatic differentiation method provided by Ceres.

	static ceres::CostFunction* Create(const Eigen::Isometry3d& pose_ij,const Matrix6d& sqrt_information)
	{
		return new ceres::AutoDiffCostFunction<EdgePoseError,6,3,4,3,4>
			(new EdgePoseError(pose_ij,sqrt_information));
	}

Implementation Based on Pose Lie Algebra Parameterization and Analytical Differentiation

In this approach, the vehicle pose is represented in a minimal parameter form, i.e., $ξ_{i} \in R^{6}$ . The corresponding transformation matrix for the three-dimensional vehicle pose is represented by $T_{i} \in SE (3)$ .

ξ_{i}^{\land} \in se (3)

T_{i} = \exp (ξ_{i}^{\land}) \in SE (3)

The relative pose transformation between two nodes can be represented as

\begin{aligned} δ ξ_{i j} & = \log (δ T_{i j})^{\lor} = \log (T_{i}^{- 1} T_{j})^{\lor} \\ = \log {(\exp (- ξ_{i}^{\land}) \exp (ξ_{j}^{\land}))}^{\lor} \end{aligned}

The residual of the corresponding edge is

\begin{aligned} r_{i j} & = \log (δ T_{i j}^{- 1} T_{i}^{- 1} T_{j})^{\lor} \\ = \log (\exp (- ξ_{i j}^{\land}) \exp (- ξ_{i}^{\land}) \exp (ξ_{j}^{\land}))^{\lor} \end{aligned}

The Jacobian matrix is calculated by

\begin{aligned} \frac{\partial r_{i j}}{\partial ξ_{i}} = - J^{- 1} (r_{i j}) A d (T_{j}^{- 1}) \\ \frac{\partial r_{i j}}{\partial ξ_{j}} = J^{- 1} (r_{i j}) A d (T_{j}^{- 1}) \end{aligned}

In the implementation approach, define the cost function class EdgePoseSE3CostFunction, with the corresponding code for residual and Jacobian matrix calculation as follows.

	virtual bool Evaluate(double const* const* parameters,
						  double *residuals, double **jacobians) const
	{
		Sophus::SE3 pose_i = Sophus::SE3::exp(Vector6d(parameters[0]));
		Sophus::SE3 pose_j = Sophus::SE3::exp(Vector6d(parameters[1]));
		Sophus::SE3 estimate = pose_i.inverse() * pose_j;

		Eigen::Map<Vector6d> residual(residuals);
		residual = sqrt_information_ * ((measurment_.inverse() * estimate).log());

		if(jacobians)
		{
			if(jacobians[0]) {
				Eigen::Map<Eigen::Matrix<double,6,6, Eigen::RowMajor>> jacobian_i(jacobians[0]);
				Matrix6d J = JRInv(Sophus::SE3::exp(residual));
				jacobian_i = (-J) * pose_j.inverse().Adj();
				jacobian_i = sqrt_information_*(-J) * pose_j.inverse().Adj();
			}
			if(jacobians[1]){
				Eigen::Map<Eigen::Matrix<double,6,6, Eigen::RowMajor>> jacobian_j(jacobians[1]);
				Matrix6d J = JRInv(Sophus::SE3::exp(residual));
				jacobian_j = J * pose_j.inverse().Adj();
				jacobian_j = sqrt_information_*J * pose_j.inverse().Adj();
			}
		}
		return true;
	}

Automatic Differentiation Algorithm Based on Dual Numbers and Jets

Dual Numbers and Jets

Dual numbers extend real numbers by introducing a differential unit $ϵ$ with the property $ϵ^{2} = 0$ . A dual number is defined as:

a + v ϵ

where it consists of two parts: the real part $a$ and the differential part $v$ .

Based on the definition of dual numbers, it is possible to compute exact differentials of functions without explicitly providing the differential form. Specifically, for any continuously differentiable function $f (x)$ , considering the Taylor expansion of $f (x + ϵ)$ around $x$ , we have:

\begin{aligned} f (x + ϵ) & = f (x) + D f (x) ϵ + D^{2} f (x) \frac{ϵ^{2}}{2!} + D^{3} f (x) \frac{ϵ^{3}}{3!} + \dots \\ = f (x) + D f (x) ϵ \end{aligned}

Next, extend real numbers to $n$ differential units $ϵ_{i}, i = 1, \dots, n$ , satisfying $\forall i, j : ϵ_{i} ϵ_{j} = 0$ . Define a Jet as

x = a + \sum_{j} v_{j} ϵ_{j}

where it includes a real part $a$ and an $n$ -dimensional differential part $v$ , represented as

x = a + v

The Taylor expansion of $f (a + v)$ yields

f (a + v) = f (a) + D f (a) v

For a multivariable function $f : R^{n} \to R^{m}$ , the Taylor expansion at $x_{i} = a_{i} + v_{i}, \forall i = 1, \dots, n$ is given by

f (x_{1}, \dots, x_{n}) = f (a_{1}, \dots, a_{n}) + \sum_{i} D_{i} f (a_{1}, \dots, a_{n}) v_{i}

Setting $v_{i} = e_{i}$ , the basis vector for each dimension, we have

f (x_{1}, \dots, x_{n}) = f (a_{1}, \dots, a_{n}) + \sum_{i} D_{i} f (a_{1}, \dots, a_{n}) ϵ_{i}

Based on this, the Jacobian at a given value can be obtained according to the coefficients corresponding to each differential component $ϵ_{i}$ .

Ceres Implementation

The Ceres nonlinear optimization library provides an exact derivative solution based on dual numbers and Jets, known as Automatic Differentiation. First, the Jet structure needs to be defined.

template<int N> struct Jet {
  double a;
  Eigen::Matrix<double, 1, N> v;
};

template<int N> Jet<N> operator+(const Jet<N>& f, const Jet<N>& g) {
  return Jet<N>(f.a + g.a, f.v + g.v);
}

template<int N> Jet<N> operator-(const Jet<N>& f, const Jet<N>& g) {
  return Jet<N>(f.a - g.a, f.v - g.v);
}

template<int N> Jet<N> operator*(const Jet<N>& f, const Jet<N>& g) {
  return Jet<N>(f.a * g.a, f.a * g.v + f.v * g.a);
}

template<int N> Jet<N> operator/(const Jet<N>& f, const Jet<N>& g) {
  return Jet<N>(f.a / g.a, f.v / g.a - f.a * g.v / (g.a * g.a));
}

template <int N> Jet<N> exp(const Jet<N>& f) {
  return Jet<T, N>(exp(f.a), exp(f.a) * f.v);
}

// This is a simple implementation for illustration purposes, the
// actual implementation of pow requires careful handling of a number
// of corner cases.
template <int N>  Jet<N> pow(const Jet<N>& f, const Jet<N>& g) {
  return Jet<N>(pow(f.a, g.a),
                g.a * pow(f.a, g.a - 1.0) * f.v +
                pow(f.a, g.a) * log(f.a); * g.v);
}

Next, taking the Rat43 problem as an example, define the functor class as follows.

struct Rat43CostFunctor {
  Rat43CostFunctor(const double x, const double y) : x_(x), y_(y) {}

  template <typename T>
  bool operator()(const T* parameters, T* residuals) const {
    const T b1 = parameters[0];
    const T b2 = parameters[1];
    const T b3 = parameters[2];
    const T b4 = parameters[3];
    residuals[0] = b1 * pow(1.0 + exp(b2 -  b3 * x_), -1.0 / b4) - y_;
    return true;
  }

  private:
    const double x_;
    const double y_;
};

CostFunction* cost_function =
      new AutoDiffCostFunction<Rat43CostFunctor, 1, 4>(
        new Rat43CostFunctor(x, y));

Unlike the implementation of numerical differentiation algorithms, the operator() definition here uses a template form.

template <typename T> bool operator()(const T* parameters, T* residuals) const;

With this definition, the Jet type can be passed into the Rat43CostFunctor function to compute the Jacobian matrix of the error function with respect to the variables.

Performance Comparison

The Ceres nonlinear optimization library provides three methods for Jacobian computation: Analytic Derivatives, Numeric Derivatives, and Automatic Derivatives.

Taking the Rat43 problem as an example, the performance comparison of these three differentiation methods is shown in Table 3.1. Here, Rat43AutomaticDiff represents the Automatic Differentiation solution, Rat43Analytic and Rat43AnalyticOptimized are the Analytic Derivatives methods, whose implementation details can be found in Ceres - Analytic Derivatives. Rat43NumericDiffForward, Rat43NumericDiffCentral, and Rat43NumericDiffRidders represent the Numeric Derivatives methods, with their implementation details available at Ceres - Numeric Derivatives.

CostFunction	Time (ns)
Rat43Analytic	255
Rat43AnalyticOptimized	92
Rat43NumericDiffForward	262
Rat43NumericDiffCentral	517
Rat43NumericDiffRidders	3760
Rat43AutomaticDiff	129

Some Tips

Since this is a large sparse problem (well large for DENSE_QR anyways), one way to solve this problem is to set Solver::Options::linear_solver_type to SPARSE_NORMAL_CHOLESKY and call Solve(). And while this is a reasonable thing to do, bundle adjustment problems have a special sparsity structure that can be exploited to solve them much more efficiently. Ceres provides three specialized solvers (collectively known as Schur-based solvers) for this task. The example code uses the simplest of them DENSE_SCHUR.

Problem by default takes ownership of the cost_function, loss_function and local_parameterization pointers. These objects remain live for the life of the Problem. If the user wishes to keep control over the destruction of these objects, then they can do this by setting the corresponding enums in the Problem::Options struct.

Covariance Calculation and Propagation

Covariance Estimation Method Based on Hessian Matrix

For the linear least squares problem, assuming the objective function is

E (\hat{X}) = (Y - M \hat{X})^{T} (Y - M \hat{X})

The optimal estimate of variable $\hat{X}$ and its covariance is given by

\begin{aligned} \hat{X} = (M^{T} M)^{- 1} M^{T} Y \\ C (\hat{X}) = (M^{T} M)^{- 1} σ^{2} \end{aligned}

The above equation provides an unbiased estimate of $σ^{2}$ , where $N$ is the number of observation data, and $\dim (\cdot)$ denotes the dimensionality of the variables.

s^{2} = \frac{E_{m i n} (\hat{X})}{N - \dim (X)}

Taking the second derivative of the above equation yields the Hessian matrix:

\begin{aligned} H = \frac{d E^{2} (\hat{X})}{d {\hat{X}}^{2}} = 2 M^{T} M \\ \to M^{T} M = \frac{1}{2} H \end{aligned}

Substituting the covariance estimation result, we obtain

C (\hat{X}) = (\frac{1}{2} H)^{- 1} σ^{2}

For the point cloud ICP scan matching method, its optimization objective is

E (ξ) = \sum_{i = 1}^{N} ‖ e_{i} ‖^{2} = \sum_{i = 1}^{N} ‖ R p_{i} + t - p_{i}^{'} ‖^{2}

The first derivative of solving $E (ξ)$ with respect to $ξ$ gives

\frac{\partial E (ξ)}{\partial ξ} = 2 \sum_{i = 1}^{N} {\frac{\partial e_{i}}{\partial ξ}}^{T} e_{i}

where

\frac{\partial e_{i}}{\partial ξ} = [\begin{matrix} I & - p_{i}^{\land} \end{matrix}]

Based on this, solving the second derivative of $E (ξ)$ with respect to $ξ$ yields the Hessian matrix

H = \frac{\partial^{2} E}{\partial ξ^{2}} = 2 \sum_{i = 1}^{N} [\begin{matrix} I & p_{i}^{\land T} \\ p_{i}^{\land} & p_{i}^{\land} p_{i}^{\land T} \end{matrix}]

Implementation

Eigen::Matrix<float,6,6> ICPRegistration::GetCovariance()
{
//	CloudData::CLOUD_PTR cloud;
//	cloud=icp_ptr_->getInputSource();
	Eigen::Matrix<float,6,6> hessian;
	hessian.setZero();
	for(int i=0;i<input_source_->size();i++)
	{
		Eigen::Vector3f p(input_source_->at(i).x,input_source_->at(i).y,input_source_->at(i).z);
		Eigen::Matrix3f p_skew=Converter::toSkewSym(p);
		hessian.block<3,3>(0,0)+=Eigen::Matrix3f::Identity();
		hessian.block<3,3>(0,3)+=-p_skew;
		hessian.block<3,3>(3,0)+=p_skew;
		hessian.block<3,3>(3,3)+=-p_skew*p_skew;
	}
	return hessian.inverse() * GetFitnessScore() / (input_source_->size()-6);
}

code:

孙沁璇