# Program Listing for File unit3-param.h¶

Return to documentation for file (`algorithms/ceres-error-terms/include/ceres-error-terms/parameterization/unit3-param.h`)

```#ifndef CERES_ERROR_TERMS_PARAMETERIZATION_UNIT3_PARAM_H_
#define CERES_ERROR_TERMS_PARAMETERIZATION_UNIT3_PARAM_H_

#include <Eigen/Core>
#include <ceres/ceres.h>

#include <maplab-common/quaternion-math.h>
#include "ceres-error-terms/parameterization/quaternion-param-eigen.h"

namespace ceres_error_terms {

// Implementation according to
// Bloesch et al - IEKF-based Visual-Inertial Odometry using Direct Photometric
// Feedback
// We use a quaternion q to represent a unit vector

namespace Unit3 {
const int kLocalSize = 2;
const int kGlobalSize = 4;

inline void GetFromVector(
const Eigen::Vector3d& vector, Eigen::Quaterniond* unit_vector) {
*unit_vector = vector.norm() < 1e-12 ? Eigen::Quaterniond::Identity()
: Eigen::Quaterniond::FromTwoVectors(
Eigen::Vector3d(0, 0, 1), vector);
}

// The unit vector is given by the R(q) * e_z
// The tangent space(basis) is given by R(q) * e_x and R * e_y
inline void GetNormalVectorAndBasis(
const Eigen::Quaterniond& q, Eigen::Vector3d* normal_vector,
Eigen::Matrix<double, 3, 2>* basis) {
const Eigen::Matrix3d R = q.toRotationMatrix();
if (normal_vector != nullptr) {
*normal_vector = R.block<3, 1>(0, 2);
}
if (basis != nullptr) {
*basis = R.block<3, 2>(0, 0);
}
}

inline Eigen::Vector3d GetNormalVector(const Eigen::Quaterniond& q) {
return q.toRotationMatrix().block<3, 1>(0, 2);
}

inline Eigen::Vector3d GetBasis1(const Eigen::Quaterniond& q) {
return q.toRotationMatrix().block<3, 1>(0, 0);
}

inline Eigen::Vector3d GetBasis2(const Eigen::Quaterniond& q) {
return q.toRotationMatrix().block<3, 1>(0, 1);
}

inline Eigen::Matrix<double, 3, 2> GetBasis(const Eigen::Quaterniond& q) {
return q.toRotationMatrix().block<3, 2>(0, 0);
}

// TODO(burrimi): Modify to also allow Eigen::Map types.
// Rotates the unit1 vector with the error respecting the manifold structure:
// unit2 = unit1 boxplus error
inline bool Plus(
const Eigen::Quaterniond& unit1, const Eigen::Vector2d& error,
Eigen::Quaterniond* unit2) {
CHECK_NOTNULL(unit2);
*unit2 = common::eigen_quaternion_helpers::ExpMap(
error(0) * GetBasis1(unit1) + error(1) * GetBasis2(unit1)) *
unit1;
return true;
}

// TODO(burrimi): Modify to also allow Eigen::Map types.
// Calculates the shortest connection respecting the manifold structure:
// error = unit1 boxminus unit2
inline bool Minus(
const Eigen::Quaterniond& unit1, const Eigen::Quaterniond& unit2,
Eigen::Vector2d* error) {
CHECK_NOTNULL(error);
const Eigen::Vector3d error_angle = common::eigen_quaternion_helpers::LogMap(
Eigen::Quaterniond::FromTwoVectors(
GetNormalVector(unit2), GetNormalVector(unit1)));
*error = GetBasis(unit2).transpose() * error_angle;
return true;
}
}  // namespace Unit3

class Unit3Parameterization : public ceres::LocalParameterization {
public:
// The representation for Jacobian computed by this object.
typedef Eigen::Matrix<
double, Unit3::kGlobalSize, Unit3::kLocalSize, Eigen::RowMajor>
Jacobian;
typedef Eigen::Matrix<
double, Unit3::kLocalSize, Unit3::kGlobalSize, Eigen::RowMajor>
LiftJacobian;

virtual ~Unit3Parameterization() {}
virtual bool Plus(
const double* x, const double* delta, double* x_plus_delta) const;
virtual bool ComputeJacobian(const double* x, double* jacobian) const;
virtual int GlobalSize() const {
return Unit3::kGlobalSize;
}
virtual int LocalSize() const {
return Unit3::kLocalSize;
}

// Calculates the difference on the manifold:
// error = unit1 boxminus unit2
bool Minus(const double* x, const double* y, double* x_minus_y) const;

bool ComputeLiftJacobian(const double* x, double* jacobian) const;

private:
};

}  // namespace ceres_error_terms

#endif  // CERES_ERROR_TERMS_PARAMETERIZATION_UNIT3_PARAM_H_
```