OKVIS 外參優化項（相機和 IMU 之間的 pose），和 Relative 一樣，都是外參的優化正則項。
Reprojection Error 中也會優化相機的外參，Reprojection 用到相機的外參。Relative error 和 Pose error 一樣，在優化時，實際是優化的正則項。OKVIS 需要手工輸入相機和 IMU 之間的 pose，這裏前提假設是輸入的 pose 還可以，PoseError 是保證優化後相機和 IMU 之間的 pose 和系統開始前手工輸入的 pose 差別儘量小。也就是說相機和 IMU 之間的 pose 是可以在線優化的。如果確認輸入的外參足夠準，也就是標定足夠準，並且在系統運行過程中，pose 不會變化，在優化時，也可以把相機和 IMU 之間的 pose 設置固定。
誤差位置
在第一幀執行的時候添加 PoseError，這部分在 Estimator.cpp 裏
// depending on whether or not this is the very beginning, we will add priors or relative terms to the last state:
if (statesMap_.size() == 1) {
// let's add a prior
Eigen::Matrix<double,6,6> information = Eigen::Matrix<double,6,6>::Zero();
information(5,5) = 1.0e8; information(0,0) = 1.0e8; information(1,1) = 1.0e8; information(2,2) = 1.0e8;
std::shared_ptr<ceres::PoseError > poseError(new ceres::PoseError(T_WS, information));
/*auto id2= */ mapPtr_->addResidualBlock(poseError,NULL,poseParameterBlock);
//mapPtr_->isJacobianCorrect(id2,1.0e-6);
// sensor states
for (size_t i = 0; i < extrinsicsEstimationParametersVec_.size(); ++i) {
double translationStdev = extrinsicsEstimationParametersVec_.at(i).sigma_absolute_translation;
double translationVariance = translationStdev*translationStdev;
double rotationStdev = extrinsicsEstimationParametersVec_.at(i).sigma_absolute_orientation;
double rotationVariance = rotationStdev*rotationStdev;
if(translationVariance>1.0e-16 && rotationVariance>1.0e-16){
const okvis::kinematics::Transformation T_SC = *multiFrame->T_SC(i);
std::shared_ptr<ceres::PoseError > cameraPoseError(
new ceres::PoseError(T_SC, translationVariance, rotationVariance));
// add to map
mapPtr_->addResidualBlock(
cameraPoseError,
NULL,
mapPtr_->parameterBlockPtr(
states.sensors.at(SensorStates::Camera).at(i).at(CameraSensorStates::T_SCi).id));
//mapPtr_->isJacobianCorrect(id,1.0e-6);
}
else {
mapPtr_->setParameterBlockConstant(
states.sensors.at(SensorStates::Camera).at(i).at(CameraSensorStates::T_SCi).id);
}
}對於 sensor 繫到 W 系 pose error 信息矩陣：
對於 camera 繫到 sensor 系 pose error 分情況而定。
// Construct with measurement and information matrix.
PoseError::PoseError(const okvis::kinematics::Transformation & measurement,
const Eigen::Matrix<double, 6, 6> & information) {
setMeasurement(measurement);
setInformation(information);
}
// Construct with measurement and variance.
PoseError::PoseError(const okvis::kinematics::Transformation & measurement,
double translationVariance, double rotationVariance) {
setMeasurement(measurement);
information_t information;
information.setZero();
information.topLeftCorner<3, 3>() = Eigen::Matrix3d::Identity() * 1.0
/ translationVariance;
information.bottomRightCorner<3, 3>() = Eigen::Matrix3d::Identity() * 1.0
/ rotationVariance;
setInformation(information);
}
// Set the information.
void PoseError::setInformation(const information_t & information) {
information_ = information;
covariance_ = information.inverse();
// perform the Cholesky decomposition on order to obtain the correct error weighting
Eigen::LLT<information_t> lltOfInformation(information_);
squareRootInformation_ = lltOfInformation.matrixL().transpose();
}
// This evaluates the error term and additionally computes the Jacobians.
bool PoseError::Evaluate(double const* const * parameters, double* residuals,
double** jacobians) const {
return EvaluateWithMinimalJacobians(parameters, residuals, jacobians, NULL);
}// This evaluates the error term and additionally computes
// the Jacobians in the minimal internal representation.
bool PoseError::EvaluateWithMinimalJacobians(double const* const * parameters,
double* residuals,
double** jacobians,
double** jacobiansMinimal) const {
// compute error
okvis::kinematics::Transformation T_WS(
Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]),
Eigen::Quaterniond(parameters[0][6], parameters[0][3], parameters[0][4],
parameters[0][5]));
// delta pose
okvis::kinematics::Transformation dp = measurement_ * T_WS.inverse();
// get the error
Eigen::Matrix<double, 6, 1> error;
const Eigen::Vector3d dtheta = 2 * dp.q().coeffs().head<3>();
error.head<3>() = measurement_.r() - T_WS.r();
error.tail<3>() = dtheta;
// weigh it
Eigen::Map<Eigen::Matrix<double, 6, 1> > weighted_error(residuals);
weighted_error = squareRootInformation_ * error;誤差部分
優化的變量 sensor 繫到世界系轉換矩陣：
變量的估計值：
// compute Jacobian...
if (jacobians != NULL) {
if (jacobians[0] != NULL) {
Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor> > J0(
jacobians[0]);
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> J0_minimal;
J0_minimal.setIdentity();
J0_minimal *= -1.0;
J0_minimal.block<3, 3>(3, 3) = -okvis::kinematics::plus(dp.q())
.topLeftCorner<3, 3>();
J0_minimal = (squareRootInformation_ * J0_minimal).eval();
// pseudo inverse of the local parametrization Jacobian:
Eigen::Matrix<double, 6, 7, Eigen::RowMajor> J_lift;
PoseLocalParameterization::liftJacobian(parameters[0], J_lift.data());
// hallucinate Jacobian w.r.t. state
J0 = J0_minimal * J_lift;
if (jacobiansMinimal != NULL) {
if (jacobiansMinimal[0] != NULL) {
Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor> > J0_minimal_mapped(
jacobiansMinimal[0]);
J0_minimal_mapped = J0_minimal;
}
}
}
}雅各比部分