本文详细介绍了Okvis程序的关键组件，包括同步应用、估计器类以及Ceres相关的误差和参数化模块。Estimator类用于配置和管理传感器，执行Ceres优化，实现位姿和地标点的添加与删除。同时，文中涵盖了ImuError和MarginalizationError类，分别涉及IMU误差计算和边缘化策略。通过对源码的解读，读者可以深入理解Okvis的视觉惯性同步定位与建图算法。
前言
本文参考各网站大佬的各种总结讲解，在此致谢。
针对程序我自己不明白不清楚的部分等，做注释。
仅供学习，如果有错误的地方，请大家指点指点。
文章目录
- 前言
- 一、okvis程序整体预览_1
- 1. okvis/okvis_apps/src/okvis_app_synchronous.cpp
- 2. okvis/okvis_ceres/src/estimator.cpp
- 3. okvis/okvis_ceres/src/HomogeneousPointError.cpp
- 4. okvis/okvis_ceres/src/HomogeneousPointLocalParameterization.cpp
- 5. okvis/okvis_ceres/src/HomogeneousPointParameterBlock.cpp
- 6. okvis/okvis_ceres/src/IdProvider.cpp
- 7. okvis/okvis_ceres/src/ImuError.cpp
- 8. okvis/okvis_ceres/src/MarginalizationError.cpp
- 9. okvis/okvis_ceres/src/PoseError.cpp
- 10. okvis/okvis_ceres/src/PoseLocalParameterization.cpp
- 总结
一、okvis程序整体预览_1
1. okvis/okvis_apps/src/okvis_app_synchronous.cpp
（1）class PoseViewer
绘图类，PoseViewer类的主要作用是画出小黑窗口，实时显示运动轨迹，在该类中，可以定义输出数据流savetrajectory（ofstream savetrajectory），将轨迹保存在“results.txt”中。
（2）int main(int argc, char **argv)
main函数
2. okvis/okvis_ceres/src/estimator.cpp
（1）namespace okvis
okvis Main namespace of this package.
okvis包的主命名空间。
// Constructor if a ceres map is already available.
// 如果ceres地图已经可用，使用构造函数
Estimator::Estimator(std::shared_ptr<okvis::ceres::Map> mapPtr)
// The default constructor. 默认构造函数
Estimator::Estimator()
// 析构函数
Estimator::~Estimator()
// Add a camera to the configuration. Sensors can only be added and never removed.
// 添加摄像机到配置中。传感器只能添加，不能移除。
int Estimator::addCamera(const ExtrinsicsEstimationParameters & extrinsicsEstimationParameters)
// Add an IMU to the configuration.
// 添加IMU到配置中
int Estimator::addImu(const ImuParameters & imuParameters)
// Remove all cameras from the configuration
// 从配置中移除所有相机
void Estimator::clearCameras()
// Remove all IMUs from the configuration.
// 从配置中移除所有IMU
void Estimator::clearImus()
// Add a pose to the state.
// 状态添加函数 // 添加位姿状态
bool Estimator::addStates(okvis::MultiFramePtr multiFrame,
const okvis::ImuMeasurementDeque & imuMeasurements, bool asKeyframe)
// Add a landmark.
// 添加地标点
bool Estimator::addLandmark(uint64_t landmarkId, const Eigen::Vector4d & landmark)
// Remove an observation from a landmark.
// 从地标移除观测
bool Estimator::removeObservation(::ceres::ResidualBlockId residualBlockId)
// Remove an observation from a landmark, if available.
// 如果可以，从地标中移除观测
bool Estimator::removeObservation(uint64_t landmarkId, uint64_t poseId,size_t camIdx, size_t keypointIdx)
// 模板类
template<class T>
// Applies the dropping/marginalization strategy according to the RSS'13/IJRR'14 paper.
// 根据RSS'13/IJRR'14的论文采用下降/边缘化策略。
// The new number of frames in the window will be numKeyframes+numImuFrames.
// 窗口中的新帧数 = numKeyframes + numImuFrames
// numKeyframes=5
// numImuFrames=3
bool Estimator::applyMarginalizationStrategy(size_t numKeyframes, size_t numImuFrames, okvis::MapPointVector& removedLandmarks)
// Prints state information to buffer.
// 将状态信息打印到缓冲区
void Estimator::printStates(uint64_t poseId, std::ostream & buffer)
// Initialise pose from IMU measurements. For convenience as static.
// 从IMU观测中恢复初始位姿 从IMU观测中初始化位姿
bool Estimator::initPoseFromImu(const okvis::ImuMeasurementDeque & imuMeasurements, okvis::kinematics::Transformation & T_WS)
// Start ceres optimization.
// 开始ceres优化
// 迭代次数
// 线程数量
#ifdef USE_OPENMP
void Estimator::optimize(size_t numIter, size_t numThreads, bool verbose)
#else
void Estimator::optimize(size_t numIter, size_t /*numThreads*/,
bool verbose) // avoid warning since numThreads unused
#warning openmp not detected, your system may be slower than expected
#endif
// Set a time limit for the optimization process.
// 为优化过程设定一个时间限制
bool Estimator::setOptimizationTimeLimit(double timeLimit, int minIterations)
// getters
// Get a specific landmark.
bool Estimator::getLandmark(uint64_t landmarkId, MapPoint& mapPoint)
// Checks whether the landmark is initialized.
// 检查路标是否初始化
bool Estimator::isLandmarkInitialized(uint64_t landmarkId)
// Get a copy of all the landmarks as a PointMap.
size_t Estimator::getLandmarks(PointMap & landmarks)
// Get a copy of all the landmark in a MapPointVector. This is for legacy support.
// Use getLandmarks(okvis::PointMap&) if possible.
size_t Estimator::getLandmarks(MapPointVector & landmarks)
// Get pose for a given pose ID.
// 获取给定位姿ID的位姿
bool Estimator::get_T_WS(uint64_t poseId, okvis::kinematics::Transformation & T_WS)
// Feel free to implement caching for them...
// Get speeds and IMU biases for a given pose ID.
// 获取给定位姿ID的速度和IMU偏差
bool Estimator::getSpeedAndBias(uint64_t poseId, uint64_t imuIdx, okvis::SpeedAndBias & speedAndBias)
// Get camera states for a given pose ID.
// 获取给定位姿ID的相机状态
bool Estimator::getCameraSensorStates( uint64_t poseId, size_t cameraIdx, okvis::kinematics::Transformation & T_SCi)
// Get the ID of the current keyframe.
// 获取当前关键帧的ID
uint64_t Estimator::currentKeyframeId()
// Get the ID of an older frame.
// 获得比当前帧早age的帧的ID
uint64_t Estimator::frameIdByAge(size_t age)
// Get the ID of the newest frame added to the state.
// 获取添加到状态的最新帧的ID
uint64_t Estimator::currentFrameId()
// Checks if a particular frame is still in the IMU window
// 检测帧是否仍在IMU的窗口中
bool Estimator::isInImuWindow(uint64_t frameId)
// Set pose for a given pose ID.
bool Estimator::set_T_WS(uint64_t poseId, const okvis::kinematics::Transformation & T_WS)
// Set the speeds and IMU biases for a given pose ID.
bool Estimator::setSpeedAndBias(uint64_t poseId, size_t imuIdx, const okvis::SpeedAndBias & speedAndBias)
// Set the transformation from sensor to camera frame for a given pose ID.
bool Estimator::setCameraSensorStates( uint64_t poseId, size_t cameraIdx, const okvis::kinematics::Transformation & T_SCi)
// Set the homogeneous coordinates for a landmark.
// 设置路标标的齐次坐标
bool Estimator::setLandmark( uint64_t landmarkId, const Eigen::Vector4d & landmark)
// Set the landmark initialization state
// 设置路标初始化状态
void Estimator::setLandmarkInitialized(uint64_t landmarkId, bool initialized)
// private stuff
// getters
bool Estimator::getGlobalStateParameterBlockPtr(
uint64_t poseId, int stateType,
std::shared_ptr<ceres::ParameterBlock>& stateParameterBlockPtr)
template<class PARAMETER_BLOCK_T>
bool Estimator::getGlobalStateParameterBlockAs(
uint64_t poseId, int stateType,
PARAMETER_BLOCK_T & stateParameterBlock)
template<class PARAMETER_BLOCK_T>
bool Estimator::getGlobalStateEstimateAs(
uint64_t poseId, int stateType,
typename PARAMETER_BLOCK_T::estimate_t & state)
bool Estimator::getSensorStateParameterBlockPtr(
uint64_t poseId, int sensorIdx, int sensorType, int stateType,
std::shared_ptr<ceres::ParameterBlock>& stateParameterBlockPtr)
template<class PARAMETER_BLOCK_T>
bool Estimator::getSensorStateParameterBlockAs(
uint64_t poseId, int sensorIdx, int sensorType, int stateType,
PARAMETER_BLOCK_T & stateParameterBlock)
template<class PARAMETER_BLOCK_T>
bool Estimator::getSensorStateEstimateAs(
uint64_t poseId, int sensorIdx, int sensorType, int stateType,
typename PARAMETER_BLOCK_T::estimate_t & state)
template<class PARAMETER_BLOCK_T>
bool Estimator::setGlobalStateEstimateAs(
uint64_t poseId, int stateType,
const typename PARAMETER_BLOCK_T::estimate_t & state)
3. okvis/okvis_ceres/src/HomogeneousPointError.cpp
在okvis中实现的ceres相关功能的ceres命名空间
4. okvis/okvis_ceres/src/HomogeneousPointLocalParameterization.cpp
在okvis中实现的ceres相关功能的ceres命名空间
5. okvis/okvis_ceres/src/HomogeneousPointParameterBlock.cpp
在okvis中实现的ceres相关功能的ceres命名空间
6. okvis/okvis_ceres/src/IdProvider.cpp
提供id
// get a unique new ID.
// 获得一个唯一的新ID
uint64_t instance::newId()
// Get the last generated ID.
// 获取最后生成的ID
uint64_t instance::currentId()
7. okvis/okvis_ceres/src/ImuError.cpp
// Construct with measurements and parameters.
// 初始IMU的误差
// imuMeasurements为从上一帧到当前帧之间所有的IMU观测值
// imuParameters为IMU的参数信息
ImuError::ImuError(const okvis::ImuMeasurementDeque & imuMeasurements,
const okvis::ImuParameters & imuParameters,
const okvis::Time& t_0, const okvis::Time& t_1)
// Propagates pose, speeds and biases with given IMU measurements.
// 预积分位置,速度和偏差
int ImuError::redoPreintegration(const okvis::kinematics::Transformation& /*T_WS*/,
const okvis::SpeedAndBias & speedAndBiases)
// Propagates pose, speeds and biases with given IMU measurements.
// 利用IMU观测值更新pose、speeds、biase。
// 注意到这里propagation的covariance和jacobian均为0，仅仅用于预测，对特征点检测提供先验的T_WC：
// t_start为上一帧的时间戳，t_end为当前帧的时间戳
// T_WS、speedAndBiases为上一帧的位姿、速度和偏差
int ImuError::propagation(const okvis::ImuMeasurementDeque & imuMeasurements,
const okvis::ImuParameters & imuParams,
okvis::kinematics::Transformation& T_WS,
okvis::SpeedAndBias & speedAndBiases,
const okvis::Time & t_start,
const okvis::Time & t_end, covariance_t* covariance,
jacobian_t* jacobian)
// This evaluates the error term and additionally computes the Jacobians.
// 这计算了误差项并计算了雅可比矩阵
bool ImuError::Evaluate(double const* const * parameters, double* residuals,
double** jacobians)
// This evaluates the error term and additionally computes
// the Jacobians in the minimal internal representation.
// 计算误差式和计算最小雅各比矩阵
// parameters表示相对位姿(平移向量和四元数)
// residuals为残差
// 雅克比矩阵
// 最小雅克比矩阵
bool ImuError::EvaluateWithMinimalJacobians(double const* const * parameters,
double* residuals,
double** jacobians,
double** jacobiansMinimal)
8. okvis/okvis_ceres/src/MarginalizationError.cpp
// Set the underlying okvis::ceres::Map.
// 设置底层okvis::ceres::Map
void MarginalizationError::setMap(Map& map)
// Add some residuals to this marginalisation error. This means, they will get linearised.
// 边缘化误差中添加残差块。这意味着，它们会被线性化
bool MarginalizationError::addResidualBlocks(
const std::vector< ::ceres::ResidualBlockId> & residualBlockIds,
const std::vector<bool> & keepResidualBlocks)
// Add some residuals to this marginalisation error. This means, they will get linearised.
// 边缘化误差中添加残差块
// 将残差residualBlockId对应的数据块添加进信息容器parameterBlockInfos_和parameterBlockId2parameterBlockInfoIdx_中
// 计算残差residualBlockId与其数据块对应的雅各比,并利用雅各比计算海瑟矩阵,找到合适的位置插入H_,b_中
// 如果keep为false,则从优化地图中移除该残差
bool MarginalizationError::addResidualBlock(
::ceres::ResidualBlockId residualBlockId, bool keep)
// Checks the internal datastructure (debug)
// 检查数据结构
void MarginalizationError::check()
// Call this in order to (re-)add this error term after whenever it had been modified.
// 得到数据块~
void MarginalizationError::getParameterBlockPtrs(
std::vector<std::shared_ptr<okvis::ceres::ParameterBlock> >& parameterBlockPtrs)
// Marginalise out a set of parameter blocks.
// 边缘化所有的数据块
// 通过舒尔消元更新H_和b_矩阵
// 从parameterBlockInfos_和parameterBlockId2parameterBlockInfoIdx_删除待边缘化数据块的元素
bool MarginalizationError::marginalizeOut(
const std::vector<uint64_t>& parameterBlockIds,
const std::vector<bool> & keepParameterBlocks)
9. okvis/okvis_ceres/src/PoseError.cpp
// This evaluates the error term and additionally computes
// the Jacobians in the minimal internal representation.
// 求取最小雅各比矩阵
// parameters表示位姿（四元数和平移）
// residuals表示残差
// jacobians表示雅各比矩阵
// jacobiansMinimal表示最小雅各比矩阵
bool PoseError::EvaluateWithMinimalJacobians(double const* const * parameters,
double* residuals,
double** jacobians,
double** jacobiansMinimal)
10. okvis/okvis_ceres/src/PoseLocalParameterization.cpp
// Computes the minimal difference between a variable x and a perturbed variable x_plus_delta.
// 计算变量x和扰动变量x_plus_delta之间的最小差值
bool PoseLocalParameterization::Minus(const double* x,
const double* x_plus_delta,
double* delta)
// Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
bool PoseLocalParameterization::ComputeLiftJacobian(const double* x,
double* jacobian)
// Computes the minimal difference between a variable x and a perturbed variable x_plus_delta.
bool PoseLocalParameterization::minus(const double* x,
const double* x_plus_delta,
double* delta)
// The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
bool PoseLocalParameterization::plusJacobian(const double* x,
double* jacobian)
// Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
// 残差error对measurement求导
bool PoseLocalParameterization::liftJacobian(const double* x,
double* jacobian)
// The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
bool PoseLocalParameterization::ComputeJacobian(const double* x,
double* jacobian)
// Generalization of the addition operation,
// x_plus_delta = Plus(x, delta)
// with the condition that Plus(x, 0) = x.
bool PoseLocalParameterization3d::Plus(const double* x, const double* delta,
double* x_plus_delta)
// Computes the minimal difference between a variable x and a perturbed variable x_plus_delta.
bool PoseLocalParameterization3d::Minus(const double* x,
const double* x_plus_delta,
double* delta)
// Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
bool PoseLocalParameterization3d::ComputeLiftJacobian(const double* x,
double* jacobian)
// The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
bool PoseLocalParameterization3d::plusJacobian(const double* x,
double* jacobian)
// Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
bool PoseLocalParameterization3d::liftJacobian(const double* x,
double* jacobian)
// The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
bool PoseLocalParameterization3d::ComputeJacobian(const double* x,
double* jacobian)
// Generalization of the addition operation,
// x_plus_delta = Plus(x, delta)
// with the condition that Plus(x, 0) = x.
bool PoseLocalParameterization4d::Plus(const double* x, const double* delta,
double* x_plus_delta)
// Computes the minimal difference between a variable x and a perturbed variable x_plus_delta.
bool PoseLocalParameterization4d::Minus(const double* x,
const double* x_plus_delta,
double* delta)
// Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
bool PoseLocalParameterization4d::ComputeLiftJacobian(const double* x,
double* jacobian)
// The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
bool PoseLocalParameterization4d::plusJacobian(const double* x,
double* jacobian)
// Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
bool PoseLocalParameterization4d::liftJacobian(const double* x,
double* jacobian)
// The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
bool PoseLocalParameterization4d::ComputeJacobian(const double* x,
double* jacobian)
// Generalization of the addition operation,
// x_plus_delta = Plus(x, delta)
// with the condition that Plus(x, 0) = x.
bool PoseLocalParameterization2d::Plus(const double* x, const double* delta,
double* x_plus_delta)
// Computes the minimal difference between a variable x and a perturbed variable x_plus_delta.
bool PoseLocalParameterization2d::Minus(const double* x,
const double* x_plus_delta,
double* delta)
// Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
bool PoseLocalParameterization2d::ComputeLiftJacobian(const double* x,
double* jacobian)
// The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
bool PoseLocalParameterization2d::plusJacobian(const double* x,
double* jacobian)
// Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
bool PoseLocalParameterization2d::liftJacobian(const double* x,
double* jacobian)
// The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
bool PoseLocalParameterization2d::ComputeJacobian(const double* x,
double* jacobian)
注：后面部分未看完，后续补一下注释和总结
总结
待总结
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
