lsb_slam Tracking线程 SE3Tracking 欧式变换矩阵跟踪参考帧 加权高斯牛顿优化算法WLM 最小二乘优化 归一化方差的光度误差函数 偏导数雅克比矩阵J 线性方程组LDLT求解
2024-04-18 13:45:25
github
算法推导
LSD-SLAM的Tracking是算法框架中三大部分之一。其主要实现函数为SlamSystem::trackFrame
这个函数的代码主要分为如下几个步骤实现：
- 构造新的图像帧：把当前图像构建为新的图像帧
- 更新参考帧：如果当前参考帧不是最近的关键帧，则更新参考帧
- 初始化位姿：把上一帧与参考帧的位姿当做初始位姿
- SE3求解：调用SE3Tracker计算当前帧和参考帧间的位姿变换
- 判断是否跟踪失败：根据跟踪的像素点个数多少以及跟踪质量来判断
- 关键帧筛选：通过计算得分确定是否构造新的关键帧
接下来主要介绍SE3求解
欧式变换矩阵 R,t 跟踪求解 SE3Tracker.cpp
这里 是 SE3跟踪求解 欧式变换矩阵求解 * LSD-SLAM在位姿估计中采用了直接法，也就是通过最小化光度误差来求解两帧图像间的位姿变化。* 并且采用了LM算法迭代求解。* 误差函数 E(se3) = SUM((Iref(pi) - I(W(pi,Dref(pi),se3))^2)* (参考帧下的像素值-参考帧3d点变换到当前帧下的像素值)^2 * 也就是 所有匹配点 的光度误差和* LSD-SLAM在求解两帧图像间的SE3变换主要在SE3Tracker类中进行实现。
求解步骤:
* 1. 首先在参考帧下根据深度信息计算出3d点云* 2. 其次，计算参考帧下的点变换到当前帧下的亚像素值，进而计算初始 光度匹配误差err* 3. 再次，利用误差对单个3d点的导数信息计算单个误差的可信程度，进行加权后方差归一化，* 得到误差置信度权重ｗ，为了减少外点（outliers）对算法的影响* 4. 然后，利用误差函数对误差向量求导数，求得各个点的误差之间的协方差矩阵，也是雅克比矩阵Ｊ,计算系数A 和 b * 5. 最后计算 变换矩阵 se3的 更新量 dse3 * dse3 = -(J转置*J)逆矩阵*J转置*w*err 直接求逆矩阵计算量较大* 也可以写成：* J转置*J * dse3 = -J转置*w*error* 紧凑形式：* A * dse3 = b* 使用 LDLT分解 A = LDU＝LDL转置＝LDLT，求解线性方程组 A * x = b* 记录 ｕ ＝ LD， D为对角矩阵 L为下三角矩阵 L转置为上三角矩阵* v ＝ L转置* 将A分解为上面两个矩阵 相乘 A = u * v* Ax = b就可以化为u(v*x) = u*y = b* 先求解 y* 得到 y = v*x* 再求解 x 即 dse3 ,是欧式变换矩阵se3的更新量* * 6. 然后对 变换矩阵 左乘 se3指数映射 进行 更新 * se3 指数映射到SE3 通过 李群乘法直接 对 变换矩阵 左乘更新
该类中有四个函数比较重要，分别为
* 1. SE3Tracker::trackFrame 主调函数* 2. SE3Tracker::calcResidualAndBuffers 被调用计算 * 初始误差 err 参考帧3d点变换到当前帧图像坐标系下的残差(灰度误差) 和 梯度(对应点像素梯度) * 3. SE3Tracker::calcWeightsAndResidual 被调用计算 * 误差可信度权重w 并对误差方差归一化得到加权平均后的误差* 4. SE3Tracker::calculateWarpUpdate 被调用计算 * 误差关系矩阵 协方差矩阵 雅克比矩阵J 以及A=J转置*J b=-J转置*w*error 求接线性方程组
SE3Tracker::trackFrame
* 图像金字塔迭代level-4到level-1* Step1: 对参考帧当前层构造点云(reference->makePointCloud) * 利用逆深度信息 和 相应的像素坐标 以及相机内参数得到 在参考帧坐标下的3d点 * (X，Y，Z) = D * (u,v,1)*K逆 =1/(1/D)* (u*fx_inv + cx_inv, v*fy_inv + cy_inv, 1)* Step2: 计算变换到当前帧的灰度匹配残差和 变换点的灰度梯度(calcResidualAndBuffers)* 计算参考帧3d点变换到当前帧图像坐标系下的残差(灰度误差) 和 梯度(对应点像素梯度) * 1. 参考帧3d点 R，t变换到 当前相机坐标系下 * Eigen::Matrix3f rotMat = referenceToFrame.rotationMatrix();// 旋转矩阵* Eigen::Vector3f transVec = referenceToFrame.translation();// 平移向量* Eigen::Vector3f Wxp = rotMat * (*refPoint) + transVec;// 欧式变换* 2. 再 投影到 当前相机 像素平面下* float u_new = (Wxp[0]/Wxp[2])*fx_l + cx_l;// float 浮点数类型* float v_new = (Wxp[1]/Wxp[2])*fy_l + cy_l;* 3. 根据浮点数坐标 四周的四个整数点坐标的梯度 使用位置差加权进行求和得到亚像素灰度值 和 亚像素 梯度值* x=u_new;* y=v_new;* int ix = (int)x;// 取整数 左上方点* int iy = (int)y;* float dx = x - ix;// 差值* float dy = y - iy;* float dxdy = dx*dy;* // map 像素梯度 指针* const Eigen::Vector4f* bp = mat +ix+iy*width;// 左上方点 像素位置 梯度的 指针* // 使用 左上方点 右上方点 右下方点 左下方点 的灰度梯度信息 * // 以及 相应位置差值作为权重值 线性加权获取 亚像素梯度信息* resInterp=dxdy * *(const Eigen::Vector3f*)(bp+1+width) // 右下方点* + (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width)// 左下方点* + (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1) // 右上方点* + (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp);// 左上方点* * 需要注意的是，在给梯度变量赋值的时候，这里乘了焦距* 这里求得 亚像素梯度之后 又乘上了 相机内参数，* 因为在求 误差偏导数时需要 需要用到 dx*fx dy*fy * *(buf_warped_dx+idx) = fx_l * resInterp[0];// 当前帧匹配点亚像素 梯度 gx = dx*fx * *(buf_warped_dy+idx) = fy_l * resInterp[1];// 匹配点亚像素 梯度 gy = dy*fy* *(buf_warped_residual+idx) = residual;// 对应 匹配点 像素误差* 这里的 dx = resInterp[0], dy = resInterp[0] 亚像素梯度* * (buf_d+idx) = 1.0f / (*refPoint)[2]; // 参考帧 Z轴 倒数 = 参考帧逆深度* *(buf_idepthVar+idx) = (*refColVar)[1];// 参考帧逆深度方差* 4. 记录变换 * a. 对参考帧灰度 进行一次仿射变换后 和 当前帧亚像素灰度做差得到 残差(灰度误差)* 现在求得的残差只是纯粹的光度误差* float c1 = affineEstimation_a * (*refColVar)[0] + affineEstimation_b;// 参考帧 灰度[0] 仿射变换后* float c2 = resInterp[2];// 当前帧 亚像素 第三维度是图像 灰度值* float residual = c1 - c2;// 匹配点对 的灰度误差值 * * 疑问1：代码中对参考帧的灰度做了一个仿射变换的处理，这里的原理是什么？* 在SVO代码中的确有考虑到两帧见位姿相差过大，* 因此通过在空间上的仿射变换之后再求灰度的操作。* 但是在这里的代码中没有看出具体原理。* b. 两帧下 3d点坐标的 Z轴深度值 的变化比例* float depthChange = (*refPoint)[2] / Wxp[2];* usageCount += depthChange < 1 ? depthChange : 1;//记录深度改变的比例 * c. 匹配点总数* buf_warped_size = idx;// 匹配点数量＝包括好的匹配点和不好的匹配点数量* Step3: 计算 方差归一化加权残差和 使用误差方差 对 误差归一化 后求和 (calcWeightsAndResidual)* 计算误差权重，归一化方差以及Huber-weight，最终把这个系数存在数组 buf_weight_p 中* 每个像素点都有一个光度匹配误差，而每个点匹配的可信度根据其偏导数可以求得，* 加权每一个误差 然后求所有点 光度匹配误差的均值* 误差导数drpdd = dr = -(dx*fx*(tx*z' - tz*x')/(z'^2*d) + dy*fy*(ty*z' - tz*y')/(z'^2*d))* = -(gx*(tx*z' - tz*x')/(z'^2*d) + gy*(ty*z' - tz*y')/(z'^2*d))* dx, dy 为参考帧3d点映射到当前帧图像平面后的梯度* fx, fy 相机内参数* tx,ty,tz 为参考帧到当前帧的平移变换(忽略旋转变换) t = translation()* x',y',z' 为参考帧3d点变换到当前帧坐标系下的3d点 x' = Wxp[0] , y' = Wxp[1] , z' = Wxp[2] * d = d = *(buf_d) 为参考帧3d点在参考帧下的逆深度* gx = *(buf_warped_dx)* gy = *(buf_warped_dy)* 方差平方 float s = settings.var_weight * *(buf_idepthVar+i);// 参考帧 逆深度方差 平方* 误差权重为加权方差平方的倒数 float w_p = 1.0f / ((cameraPixelNoise2) + s * drpdd * drpdd);// 误差权重 方差倒数* 初步误差 rp =*( buf_warped_residual) ; //初步误差* 加权的误差 float weighted_rp = fabs(rp*sqrtf(w_p));// |r|加权的误差* Huber-weight权重 * float wh = fabs(weighted_rp < (settings.huber_d/2) ? 1 : (settings.huber_d/2) / weighted_rp);* 记录 *(buf_weight_p+i) = wh * w_p; // 乘上 Huber-weight 权重 得到最终误差权重避免外点的影响* 求和 sumRes += wh * w_p * rp*rp; // 加权误差和* 取均值 return sumRes / buf_warped_size;// 返回加权误差均值 lastErr = sumRes / buf_warped_size;* Step4: 计算雅克比向量以及A和b(calculateWarpUpdate)* J转置*J * dse3 = -J转置*w*lastErr* 对于参考帧中的一个3D点pi位置处的残差求雅可比，这里需要使用链式求导法则 * Ji = 1/z'*dx*fx + 0* dy *fy* 0* dx *fx + 1/z' *dy *fy* -1 * -x'/z'^2 *dx*fx - y'/z'^2 *dy*fy * -x'*y'/z'^2 *dx*fx - (1+y'^2/z'^2) *dy*fy* (1+x'^2/z'^2)*dx*fx + x'*y'/z'^2*dy*fy* - y'/z' *dx*fx + x'/z' * dy*fy** A = J *J转置*(*(buf_weight_p+i))* b = -J转置*(*(buf_weight_p+i))*lastErr* * Step5: 计算得到收敛的delta，并且更新SE3(dse3 = A.ldlt().solve(b))* for(int i=0;i<6;i++) A(i,i) *= 1+LM_lambda;// (A+λI) 列文伯格马尔夸克更新算法 LM 调整量 * Vector6 inc = A.ldlt().solve(b);// LDLT矩阵分解求解 dse3 李代数更新量Sophus::SE3f new_referenceToFrame = Sophus::SE3f::exp((inc)) * referenceToFrame; * * 重复Step2-Step5直到收敛或者达到最大迭代次数* *计算下一层金字塔
#include "SE3Tracker.h"
#include <opencv2/highgui/highgui.hpp>
#include "DataStructures/Frame.h"
#include "Tracking/TrackingReference.h"
#include "util/globalFuncs.h"
#include "IOWrapper/ImageDisplay.h"
#include "Tracking/least_squares.h"#include <Eigen/Core>namespace lsd_slam
{// 指令集优化 X86 SSE指令集优化 ARM NENO指令集优化
// 通过callOptimized这个宏调用 calcResidualAndBuffers 这个函数进行优化操作
#if defined(ENABLE_NEON)#define callOptimized(function, arguments) function##NEON arguments
#else#if defined(ENABLE_SSE)#define callOptimized(function, arguments) (USESSE ? function##SSE arguments : function arguments)#else#define callOptimized(function, arguments) function arguments#endif
#endif// 使用图像存储 参数和 相机内参数初始化
SE3Tracker::SE3Tracker(int w, int h, Eigen::Matrix3f K)
{// 图像尺寸width = w;height = h;
// 相机内参数this->K = K;fx = K(0,0);fy = K(1,1);cx = K(0,2);cy = K(1,2);
// 系统全局配置文件 稠密深度跟踪器设置类// 定义了一些Tracking相关的设定，比如最大迭代次数，// 认为收敛的阈值(百分比形式，有些数据认为98%收敛，有些是99%)，// 还有huber距离所需参数等settings = DenseDepthTrackerSettings();//settings.maxItsPerLvl[0] = 2;
// 相机内参数 逆 倒数KInv = K.inverse();fxi = KInv(0,0);fyi = KInv(1,1);cxi = KInv(0,2);cyi = KInv(1,2);// 分配内存buf_warped_residual = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));// 梯度buf_warped_dx = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));buf_warped_dy = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));// 坐标buf_warped_x = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));buf_warped_y = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));buf_warped_z = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));// 逆深度buf_d = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));// 逆深度方差buf_idepthVar = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));// 计算权重buf_weight_p = (float*)Eigen::internal::aligned_malloc(w*h*sizeof(float));buf_warped_size = 0;// 可视化Tracking的迭代过程 需要的 图像debugImageWeights = cv::Mat(height,width,CV_8UC3);debugImageResiduals = cv::Mat(height,width,CV_8UC3);// 残差debugImageSecondFrame = cv::Mat(height,width,CV_8UC3);debugImageOldImageWarped = cv::Mat(height,width,CV_8UC3);debugImageOldImageSource = cv::Mat(height,width,CV_8UC3);lastResidual = 0;iterationNumber = 0;pointUsage = 0;lastGoodCount = lastBadCount = 0;// 计数器diverged = false;
}
// 类析构函数
SE3Tracker::~SE3Tracker()
{// cv::Mat 对象 自带的 释放存储空间的 方法debugImageResiduals.release();debugImageWeights.release();debugImageSecondFrame.release();debugImageOldImageSource.release();debugImageOldImageWarped.release();
// Eigen对象的 内存释放 方法Eigen::internal::aligned_free((void*)buf_warped_residual);Eigen::internal::aligned_free((void*)buf_warped_dx);// 梯度Eigen::internal::aligned_free((void*)buf_warped_dy);Eigen::internal::aligned_free((void*)buf_warped_x);// 坐标Eigen::internal::aligned_free((void*)buf_warped_y);Eigen::internal::aligned_free((void*)buf_warped_z);Eigen::internal::aligned_free((void*)buf_d);// 逆深度Eigen::internal::aligned_free((void*)buf_idepthVar);// 逆深度方差Eigen::internal::aligned_free((void*)buf_weight_p);// 权重
}// tracks a frame.
// first_frame has depth, second_frame DOES NOT have depth.
// 第一帧 参考帧 3d点 按照 欧式变换矩阵 以及当前帧的第4层相机内参数 变换到图像平面下
// 看映射后的坐标是否合理，以及看两个坐标系下点坐标的 Z轴深度信息的变化
float SE3Tracker::checkPermaRefOverlap(Frame* reference,SE3 referenceToFrameOrg)
{Sophus::SE3f referenceToFrame = referenceToFrameOrg.cast<float>();// 欧式变换矩阵// 上锁boost::unique_lock<boost::mutex> lock2 = boost::unique_lock<boost::mutex>(reference->permaRef_mutex);int w2 = reference->width(QUICK_KF_CHECK_LVL)-1;// 第4层int h2 = reference->height(QUICK_KF_CHECK_LVL)-1;Eigen::Matrix3f KLvl = reference->K(QUICK_KF_CHECK_LVL);float fx_l = KLvl(0,0);// 第四层 相机内参数 float fy_l = KLvl(1,1);float cx_l = KLvl(0,2);float cy_l = KLvl(1,2);Eigen::Matrix3f rotMat = referenceToFrame.rotationMatrix();// 旋转矩阵Eigen::Vector3f transVec = referenceToFrame.translation();// 平移向量const Eigen::Vector3f* refPoint = reference->permaRef_posData;// 参考帧3d点 存储的起始指针const Eigen::Vector3f* refPoint_max = reference->permaRef_posData + reference->permaRefNumPts;// 最大位置float usageCount = 0;for(;refPoint<refPoint_max; refPoint++){Eigen::Vector3f Wxp = rotMat * (*refPoint) + transVec;// 变换到当前图像坐标系下 的坐标float u_new = (Wxp[0]/Wxp[2])*fx_l + cx_l;// 对应图像上的像素坐标float v_new = (Wxp[1]/Wxp[2])*fy_l + cy_l;if((u_new > 0 && v_new > 0 && u_new < w2 && v_new < h2))//3d点 映射到 图像平面上 2d点坐标需要在合理范围内{float depthChange = (*refPoint)[2] / Wxp[2];// 两个坐标系下 逆深度的变换比例usageCount += depthChange < 1 ? depthChange : 1;// 按照最大值1 记录总和}}pointUsage = usageCount / (float)reference->permaRefNumPts;// 变换 均值return pointUsage;
}// tracks a frame.
// first_frame has depth, second_frame DOES NOT have depth.
SE3 SE3Tracker::trackFrameOnPermaref(Frame* reference,Frame* frame,SE3 referenceToFrameOrg)
{Sophus::SE3f referenceToFrame = referenceToFrameOrg.cast<float>();boost::shared_lock<boost::shared_mutex> lock = frame->getActiveLock();boost::unique_lock<boost::mutex> lock2 = boost::unique_lock<boost::mutex>(reference->permaRef_mutex);affineEstimation_a = 1; affineEstimation_b = 0;NormalEquationsLeastSquares ls;diverged = false;trackingWasGood = true;callOptimized(calcResidualAndBuffers, (reference->permaRef_posData, reference->permaRef_colorAndVarData, 0, reference->permaRefNumPts, frame, referenceToFrame, QUICK_KF_CHECK_LVL, false));if(buf_warped_size < MIN_GOODPERALL_PIXEL_ABSMIN * (width>>QUICK_KF_CHECK_LVL)*(height>>QUICK_KF_CHECK_LVL)){diverged = true;trackingWasGood = false;return SE3();}if(useAffineLightningEstimation){affineEstimation_a = affineEstimation_a_lastIt;affineEstimation_b = affineEstimation_b_lastIt;}float lastErr = callOptimized(calcWeightsAndResidual,(referenceToFrame));float LM_lambda = settings.lambdaInitialTestTrack;for(int iteration=0; iteration < settings.maxItsTestTrack; iteration++){callOptimized(calculateWarpUpdate,(ls));int incTry=0;while(true){// solve LS system with current lambdaVector6 b = -ls.b;Matrix6x6 A = ls.A;for(int i=0;i<6;i++) A(i,i) *= 1+LM_lambda;Vector6 inc = A.ldlt().solve(b);incTry++;// apply increment. pretty sure this way round is correct, but hard to test.Sophus::SE3f new_referenceToFrame = Sophus::SE3f::exp((inc)) * referenceToFrame;// re-evaluate residualcallOptimized(calcResidualAndBuffers, (reference->permaRef_posData, reference->permaRef_colorAndVarData, 0, reference->permaRefNumPts, frame, new_referenceToFrame, QUICK_KF_CHECK_LVL, false));if(buf_warped_size < MIN_GOODPERALL_PIXEL_ABSMIN * (width>>QUICK_KF_CHECK_LVL)*(height>>QUICK_KF_CHECK_LVL)){diverged = true;trackingWasGood = false;return SE3();}float error = callOptimized(calcWeightsAndResidual,(new_referenceToFrame));// accept inc?if(error < lastErr){// accept increferenceToFrame = new_referenceToFrame;if(useAffineLightningEstimation){affineEstimation_a = affineEstimation_a_lastIt;affineEstimation_b = affineEstimation_b_lastIt;}// converged?if(error / lastErr > settings.convergenceEpsTestTrack)iteration = settings.maxItsTestTrack;lastErr = error;if(LM_lambda <= 0.2)LM_lambda = 0;elseLM_lambda *= settings.lambdaSuccessFac;break;}else{if(!(inc.dot(inc) > settings.stepSizeMinTestTrack)){iteration = settings.maxItsTestTrack;break;}if(LM_lambda == 0)LM_lambda = 0.2;elseLM_lambda *= std::pow(settings.lambdaFailFac, incTry);}}}lastResidual = lastErr;trackingWasGood = !diverged&& lastGoodCount / (frame->width(QUICK_KF_CHECK_LVL)*frame->height(QUICK_KF_CHECK_LVL)) > MIN_GOODPERALL_PIXEL&& lastGoodCount / (lastGoodCount + lastBadCount) > MIN_GOODPERGOODBAD_PIXEL;return toSophus(referenceToFrame);
}// 最重要的跟踪匹配 函数
// SE3Tracker::trackFrame函数的主体是一个for循环，从图像金字塔的高层level-4开始遍历直到底层level-1。
// 在循环内实现加权高斯牛顿优化算法（Weighted Gauss-Newton Optimization），
// 其实就是增加了鲁棒函数的高斯牛顿。
// tracks a frame.
// first_frame has depth, second_frame DOES NOT have depth.
SE3 SE3Tracker::trackFrame(TrackingReference* reference,// 参考帧，第一帧 含有 逆深度信息Frame* frame,// 当前帧 第二帧 没有深度 需要匹配后 获取 const SE3& frameToReference_initialEstimate)// 初始变换矩阵 fram 到参考帧 的 变换矩阵
// 对于初始位姿，LSD-SLAM中使用了上一帧和参考帧间的位姿作为当前位姿估计的初值
{
// 内存上锁boost::shared_lock<boost::shared_mutex> lock = frame->getActiveLock();diverged = false;trackingWasGood = true;affineEstimation_a = 1; affineEstimation_b = 0;// 仿射变换参数
// 保存 跟踪 信息if(saveAllTrackingStages){saveAllTrackingStages = false;saveAllTrackingStagesInternal = true;}
// 初始化 跟踪迭代信息 if (plotTrackingIterationInfo){const float* frameImage = frame->image();// 当前帧 第０层 图像for (int row = 0; row < height; ++ row)// 每行for (int col = 0; col < width; ++ col)// 每列// 按照初始图像像素灰度值 设置 颜色setPixelInCvMat(&debugImageSecondFrame,getGrayCvPixel(frameImage[col+row*width]), col, row, 1);}// ============ track frame ============
// 首先将初始估计记录下来(记录了参考帧到当前帧的 欧式变换矩阵)// 参考帧 到 当前帧 fram 的 变换矩阵Sophus::SE3f referenceToFrame = frameToReference_initialEstimate.inverse().cast<float>();NormalEquationsLeastSquares ls;// 然后定义一个6自由度矩阵的误差判别计算对象lsint numCalcResidualCalls[PYRAMID_LEVELS];// cell数量 5层 金字塔int numCalcWarpUpdateCalls[PYRAMID_LEVELS];// float last_residual = 0;// 最终的残差// 函数的主体是一个for循环for(int lvl=SE3TRACKING_MAX_LEVEL-1;lvl >= SE3TRACKING_MIN_LEVEL;lvl--)// 在每一层金字塔上进行跟踪{// 从最高层的 金字塔图像(尺寸最小) 开始 向下计算// 从图像金字塔的高层level-4开始遍历直到底层level-1numCalcResidualCalls[lvl] = 0;numCalcWarpUpdateCalls[lvl] = 0;
// 【１】 有参考帧逆深度信息和对应像素点坐标 计算 在参考帧坐标系下的3d 点reference->makePointCloud(lvl);
//【２】计算参考帧3d点变换到当前帧图像坐标系下的残差(灰度误差) 和 梯度(对应点像素梯度) // 计算匹配 点对 像素误差 在这个函数中，把 buf_warped 相关的参数全部更新，并且更新了上次的相似变换参数等callOptimized(calcResidualAndBuffers, (reference->posData[lvl], reference->colorAndVarData[lvl], SE3TRACKING_MIN_LEVEL == lvl ? reference->pointPosInXYGrid[lvl] : 0, reference->numData[lvl], frame, referenceToFrame, lvl, (plotTracking && lvl == SE3TRACKING_MIN_LEVEL)));// buf_warped_size 记录了 匹配点数量 包括好的匹配点 和 不好的匹配点 数量 (只要投影后在图像范围内就可以)if(buf_warped_size < MIN_GOODPERALL_PIXEL_ABSMIN * (width>>lvl)*(height>>lvl)){// 如果匹配点数量过少，已经少于了这一层图像像素数量的1%)，那么我们认为差别太大，Tracking失败，返回一个空的SE3diverged = true;trackingWasGood = false;// Tracking失败return SE3();// 返回一个空的SE3}// 使用 仿射变换参数// 如果使用了，那么把通过calcResidualAndBuffers函数更新的affineEstimation_a_lastIt以及affineEstimation_b_lastIt，赋值给仿射变换系数if(useAffineLightningEstimation){affineEstimation_a = affineEstimation_a_lastIt;affineEstimation_b = affineEstimation_b_lastIt;}
// 【３】使用误差协方差(误差求导) 对方差归一化 后求和 调用calcWeightsAndResidual得到误差，并记录调用次数// 以及误差权重矩阵 避免外点的影响 float lastErr = callOptimized(calcWeightsAndResidual,(referenceToFrame));numCalcResidualCalls[lvl]++;float LM_lambda = settings.lambdaInitial[lvl];// λ LM优化的 调整量
// 【４】每一层进行 LM优化 (A+λI)x=b 在最大迭代次数范围内进行优化 for(int iteration=0; iteration < settings.maxItsPerLvl[lvl]; iteration++){// 计算 加权平均误差 以及误差权重callOptimized(calculateWarpUpdate,(ls));numCalcWarpUpdateCalls[lvl]++;iterationNumber = iteration;// 迭代次数int incTry=0;while(true){// solve LS system with current lambdaVector6 b = -ls.b;Matrix6x6 A = ls.A;// 列文伯格马尔夸克优化算法更新for(int i=0;i<6;i++) A(i,i) *= 1+LM_lambda;// (A+λI) LM 调整量 Vector6 inc = A.ldlt().solve(b);// LDLT矩阵分解求解 dse3 李代数更新量incTry++;// 对 变换矩阵 左乘 se3指数映射 进行 更新 // apply increment. pretty sure this way round is correct, but hard to test.Sophus::SE3f new_referenceToFrame = Sophus::SE3f::exp((inc)) * referenceToFrame;//Sophus::SE3f new_referenceToFrame = referenceToFrame * Sophus::SE3f::exp((inc));// re-evaluate residualcallOptimized(calcResidualAndBuffers, (reference->posData[lvl], reference->colorAndVarData[lvl], SE3TRACKING_MIN_LEVEL == lvl ? reference->pointPosInXYGrid[lvl] : 0, reference->numData[lvl], frame, new_referenceToFrame, lvl, (plotTracking && lvl == SE3TRACKING_MIN_LEVEL)));if(buf_warped_size < MIN_GOODPERALL_PIXEL_ABSMIN* (width>>lvl)*(height>>lvl)){// 匹配点数量记录 包括好的匹配点 和 不好的匹配点 数量// 匹配点数量少于阈值 优化失败diverged = true;trackingWasGood = false;// 优化失败return SE3();// 返回空}// 新变换下的误差float error = callOptimized(calcWeightsAndResidual,(new_referenceToFrame));numCalcResidualCalls[lvl]++;// accept inc?// 误差变小了if(error < lastErr)// 误差变小了{// accept inc// 更新 新求解的 变换矩阵 referenceToFrame = new_referenceToFrame;if(useAffineLightningEstimation){// 更新 仿射变换系数affineEstimation_a = affineEstimation_a_lastIt;affineEstimation_b = affineEstimation_b_lastIt;}// 打印调试信息if(enablePrintDebugInfo && printTrackingIterationInfo){// debug outputprintf("(%d-%d): ACCEPTED increment of %f with lambda %.1f, residual: %f -> %f\n",lvl,iteration, sqrt(inc.dot(inc)), LM_lambda, lastErr, error);printf(" p=%.4f %.4f %.4f %.4f %.4f %.4f\n",referenceToFrame.log()[0],referenceToFrame.log()[1],referenceToFrame.log()[2],referenceToFrame.log()[3],referenceToFrame.log()[4],referenceToFrame.log()[5]);}// converged?if(error / lastErr > settings.convergenceEps[lvl]){if(enablePrintDebugInfo && printTrackingIterationInfo){printf("(%d-%d): FINISHED pyramid level (last residual reduction too small).\n",lvl,iteration);}iteration = settings.maxItsPerLvl[lvl];}last_residual = lastErr = error;if(LM_lambda <= 0.2)LM_lambda = 0;elseLM_lambda *= settings.lambdaSuccessFac;break;}// 误差反而没有减少else{if(enablePrintDebugInfo && printTrackingIterationInfo){printf("(%d-%d): REJECTED increment of %f with lambda %.1f, (residual: %f -> %f)\n",lvl,iteration, sqrt(inc.dot(inc)), LM_lambda, lastErr, error);}if(!(inc.dot(inc) > settings.stepSizeMin[lvl])){if(enablePrintDebugInfo && printTrackingIterationInfo){printf("(%d-%d): FINISHED pyramid level (stepsize too small).\n",lvl,iteration);}iteration = settings.maxItsPerLvl[lvl];break;}// 调整 LM 参数if(LM_lambda == 0)LM_lambda = 0.2;elseLM_lambda *= std::pow(settings.lambdaFailFac, incTry);}}}}if(plotTracking)Util::displayImage("TrackingResidual", debugImageResiduals, false);if(enablePrintDebugInfo && printTrackingIterationInfo){printf("Tracking: ");for(int lvl=PYRAMID_LEVELS-1;lvl >= 0;lvl--){printf("lvl %d: %d (%d); ",lvl,numCalcResidualCalls[lvl],numCalcWarpUpdateCalls[lvl]);}printf("\n");}saveAllTrackingStagesInternal = false;lastResidual = last_residual;trackingWasGood = !diverged&& lastGoodCount / (frame->width(SE3TRACKING_MIN_LEVEL)*frame->height(SE3TRACKING_MIN_LEVEL)) > MIN_GOODPERALL_PIXEL&& lastGoodCount / (lastGoodCount + lastBadCount) > MIN_GOODPERGOODBAD_PIXEL;if(trackingWasGood)reference->keyframe->numFramesTrackedOnThis++;frame->initialTrackedResidual = lastResidual / pointUsage;frame->pose->thisToParent_raw = sim3FromSE3(toSophus(referenceToFrame.inverse()),1);frame->pose->trackingParent = reference->keyframe->pose;return toSophus(referenceToFrame.inverse());
}//// 每个像素点都有一个光度匹配误差，而每个点匹配的可信度根据其偏导数可以求得，
// 加权每一个误差 然后求所有点 光度匹配误差的均值
// 再者 计算误差的权重，在优化更新函数中作为权重// 使用 误差方差对方差归一化 后求和
// 计算权重 和 像素匹配误差
// 该函数的功能是计算归一化方差的光度误差系数，也就是计算公式(14)，
// 并且乘以了Huber-weight，最终把这个系数存在数组buf_weight_p中。
// 可能是考虑参考帧到当前帧的位姿变换比较小，
// 所以作者只考虑了位移t而忽略旋转R。
// 这样使得式子(14)中的偏导的形式简单了很多。
// 这里主要求 误差函数的导数 即得到 误差的协方差矩阵
#if defined(ENABLE_SSE)
float SE3Tracker::calcWeightsAndResidualSSE(const Sophus::SE3f& referenceToFrame)
{const __m128 txs = _mm_set1_ps((float)(referenceToFrame.translation()[0]));const __m128 tys = _mm_set1_ps((float)(referenceToFrame.translation()[1]));const __m128 tzs = _mm_set1_ps((float)(referenceToFrame.translation()[2]));const __m128 zeros = _mm_set1_ps(0.0f);const __m128 ones = _mm_set1_ps(1.0f);const __m128 depthVarFacs = _mm_set1_ps((float)settings.var_weight);// float depthVarFac = var_weight; // the depth var is over-confident. this is a constant multiplier to remedy that.... HACKconst __m128 sigma_i2s = _mm_set1_ps((float)cameraPixelNoise2);const __m128 huber_res_ponlys = _mm_set1_ps((float)(settings.huber_d/2));__m128 sumResP = zeros;float sumRes = 0;for(int i=0;i<buf_warped_size-3;i+=4){
// float px = *(buf_warped_x+i); // x'
// float py = *(buf_warped_y+i); // y'
// float pz = *(buf_warped_z+i); // z'
// float d = *(buf_d+i); // d
// float rp = *(buf_warped_residual+i); // r_p
// float gx = *(buf_warped_dx+i); // \delta_x I
// float gy = *(buf_warped_dy+i); // \delta_y I
// float s = depthVarFac * *(buf_idepthVar+i); // \sigma_d^2// calc dw/dd (first 2 components):__m128 pzs = _mm_load_ps(buf_warped_z+i); // z'__m128 pz2ds = _mm_rcp_ps(_mm_mul_ps(_mm_mul_ps(pzs, pzs), _mm_load_ps(buf_d+i))); // 1 / (z' * z' * d)__m128 g0s = _mm_sub_ps(_mm_mul_ps(pzs, txs), _mm_mul_ps(_mm_load_ps(buf_warped_x+i), tzs));g0s = _mm_mul_ps(g0s,pz2ds); //float g0 = (tx * pz - tz * px) / (pz*pz*d);//float g1 = (ty * pz - tz * py) / (pz*pz*d);__m128 g1s = _mm_sub_ps(_mm_mul_ps(pzs, tys), _mm_mul_ps(_mm_load_ps(buf_warped_y+i), tzs));g1s = _mm_mul_ps(g1s,pz2ds);// float drpdd = gx * g0 + gy * g1; // ommitting the minus__m128 drpdds = _mm_add_ps(_mm_mul_ps(g0s, _mm_load_ps(buf_warped_dx+i)),_mm_mul_ps(g1s, _mm_load_ps(buf_warped_dy+i)));//float w_p = 1.0f / (sigma_i2 + s * drpdd * drpdd);__m128 w_ps = _mm_rcp_ps(_mm_add_ps(sigma_i2s,_mm_mul_ps(drpdds,_mm_mul_ps(drpdds,_mm_mul_ps(depthVarFacs,_mm_load_ps(buf_idepthVar+i))))));//float weighted_rp = fabs(rp*sqrtf(w_p));__m128 weighted_rps = _mm_mul_ps(_mm_load_ps(buf_warped_residual+i),_mm_sqrt_ps(w_ps));weighted_rps = _mm_max_ps(weighted_rps, _mm_sub_ps(zeros,weighted_rps));//float wh = fabs(weighted_rp < huber_res_ponly ? 1 : huber_res_ponly / weighted_rp);__m128 whs = _mm_cmplt_ps(weighted_rps, huber_res_ponlys); // bitmask 0xFFFFFFFF for 1, 0x000000 for huber_res_ponly / weighted_rpwhs = _mm_or_ps(_mm_and_ps(whs, ones),_mm_andnot_ps(whs, _mm_mul_ps(huber_res_ponlys, _mm_rcp_ps(weighted_rps))));// sumRes.sumResP += wh * w_p * rp*rp;if(i+3 < buf_warped_size)sumResP = _mm_add_ps(sumResP,_mm_mul_ps(whs, _mm_mul_ps(weighted_rps, weighted_rps)));// *(buf_weight_p+i) = wh * w_p;_mm_store_ps(buf_weight_p+i, _mm_mul_ps(whs, w_ps) );}sumRes = SSEE(sumResP,0) + SSEE(sumResP,1) + SSEE(sumResP,2) + SSEE(sumResP,3);return sumRes / ((buf_warped_size >> 2)<<2);
}
#endif#if defined(ENABLE_NEON)
float SE3Tracker::calcWeightsAndResidualNEON(const Sophus::SE3f& referenceToFrame)
{float tx = referenceToFrame.translation()[0];float ty = referenceToFrame.translation()[1];float tz = referenceToFrame.translation()[2];float constants[] = {tx, ty, tz, settings.var_weight,cameraPixelNoise2, settings.huber_d/2, -1, -1 // last values are currently unused};// This could also become a constant if one register could be made free for it somehowfloat cutoff_res_ponly4[4] = {10000, 10000, 10000, 10000}; // removedfloat* cur_buf_warped_z = buf_warped_z;float* cur_buf_warped_x = buf_warped_x;float* cur_buf_warped_y = buf_warped_y;float* cur_buf_warped_dx = buf_warped_dx;float* cur_buf_warped_dy = buf_warped_dy;float* cur_buf_warped_residual = buf_warped_residual;float* cur_buf_d = buf_d;float* cur_buf_idepthVar = buf_idepthVar;float* cur_buf_weight_p = buf_weight_p;int loop_count = buf_warped_size / 4;int remaining = buf_warped_size - 4 * loop_count;float sum_vector[] = {0, 0, 0, 0};float sumRes=0;#ifdef DEBUGloop_count = 0;remaining = buf_warped_size;
#elseif (loop_count > 0){__asm__ __volatile__(// Extract constants"vldmia %[constants], {q8-q9} \n\t" // constants(q8-q9)"vdup.32 q13, d18[0] \n\t" // extract sigma_i2 x 4 to q13"vdup.32 q14, d18[1] \n\t" // extract huber_res_ponly x 4 to q14//"vdup.32 ???, d19[0] \n\t" // extract cutoff_res_ponly x 4 to ???"vdup.32 q9, d16[0] \n\t" // extract tx x 4 to q9, overwrite!"vdup.32 q10, d16[1] \n\t" // extract ty x 4 to q10"vdup.32 q11, d17[0] \n\t" // extract tz x 4 to q11"vdup.32 q8, d17[1] \n\t" // extract depthVarFac x 4 to q8, overwrite!"veor q15, q15, q15 \n\t" // set sumRes.sumResP(q15) to zero (by xor with itself)".loopcalcWeightsAndResidualNEON: \n\t""vldmia %[buf_idepthVar]!, {q7} \n\t" // s(q7)"vldmia %[buf_warped_z]!, {q2} \n\t" // pz(q2)"vldmia %[buf_d]!, {q3} \n\t" // d(q3)"vldmia %[buf_warped_x]!, {q0} \n\t" // px(q0)"vldmia %[buf_warped_y]!, {q1} \n\t" // py(q1)"vldmia %[buf_warped_residual]!, {q4} \n\t" // rp(q4)"vldmia %[buf_warped_dx]!, {q5} \n\t" // gx(q5)"vldmia %[buf_warped_dy]!, {q6} \n\t" // gy(q6)"vmul.f32 q7, q7, q8 \n\t" // s *= depthVarFac"vmul.f32 q12, q2, q2 \n\t" // pz*pz (q12, temp)"vmul.f32 q3, q12, q3 \n\t" // pz*pz*d (q3)"vrecpe.f32 q3, q3 \n\t" // 1/(pz*pz*d) (q3)"vmul.f32 q12, q9, q2 \n\t" // tx*pz (q12)"vmls.f32 q12, q11, q0 \n\t" // tx*pz - tz*px (q12) [multiply and subtract] {free: q0}"vmul.f32 q0, q10, q2 \n\t" // ty*pz (q0) {free: q2}"vmls.f32 q0, q11, q1 \n\t" // ty*pz - tz*py (q0) {free: q1}"vmul.f32 q12, q12, q3 \n\t" // g0 (q12)"vmul.f32 q0, q0, q3 \n\t" // g1 (q0)"vmul.f32 q12, q12, q5 \n\t" // gx * g0 (q12) {free: q5}"vldmia %[cutoff_res_ponly4], {q5} \n\t" // cutoff_res_ponly (q5), load for later"vmla.f32 q12, q6, q0 \n\t" // drpdd = gx * g0 + gy * g1 (q12) {free: q6, q0}"vmov.f32 q1, #1.0 \n\t" // 1.0 (q1), will be used later"vmul.f32 q12, q12, q12 \n\t" // drpdd*drpdd (q12)"vmul.f32 q12, q12, q7 \n\t" // s*drpdd*drpdd (q12)"vadd.f32 q12, q12, q13 \n\t" // sigma_i2 + s*drpdd*drpdd (q12)"vrecpe.f32 q12, q12 \n\t" // w_p = 1/(sigma_i2 + s*drpdd*drpdd) (q12) {free: q7}// float weighted_rp = fabs(rp*sqrtf(w_p));"vrsqrte.f32 q7, q12 \n\t" // 1 / sqrtf(w_p) (q7)"vrecpe.f32 q7, q7 \n\t" // sqrtf(w_p) (q7)"vmul.f32 q7, q7, q4 \n\t" // rp*sqrtf(w_p) (q7)"vabs.f32 q7, q7 \n\t" // weighted_rp (q7)// float wh = fabs(weighted_rp < huber_res_ponly ? 1 : huber_res_ponly / weighted_rp);"vrecpe.f32 q6, q7 \n\t" // 1 / weighted_rp (q6)"vmul.f32 q6, q6, q14 \n\t" // huber_res_ponly / weighted_rp (q6)"vclt.f32 q0, q7, q14 \n\t" // weighted_rp < huber_res_ponly ? all bits 1 : all bits 0 (q0)"vbsl q0, q1, q6 \n\t" // sets elements in q0 to 1(q1) where above condition is true, and to q6 where it is false {free: q6}"vabs.f32 q0, q0 \n\t" // wh (q0)// sumRes.sumResP += wh * w_p * rp*rp"vmul.f32 q4, q4, q4 \n\t" // rp*rp (q4)"vmul.f32 q4, q4, q12 \n\t" // w_p*rp*rp (q4)"vmla.f32 q15, q4, q0 \n\t" // sumRes.sumResP += wh*w_p*rp*rp (q15) {free: q4}// if(weighted_rp > cutoff_res_ponly)// wh = 0;// *(buf_weight_p+i) = wh * w_p;"vcle.f32 q4, q7, q5 \n\t" // mask in q4: ! (weighted_rp > cutoff_res_ponly)"vmul.f32 q0, q0, q12 \n\t" // wh * w_p (q0)"vand q0, q0, q4 \n\t" // set q0 to 0 where condition for q4 failed (i.e. weighted_rp > cutoff_res_ponly)"vstmia %[buf_weight_p]!, {q0} \n\t""subs %[loop_count], %[loop_count], #1 \n\t""bne .loopcalcWeightsAndResidualNEON \n\t""vstmia %[sum_vector], {q15} \n\t": /* outputs */ [buf_warped_z]"+&r"(cur_buf_warped_z),[buf_warped_x]"+&r"(cur_buf_warped_x),[buf_warped_y]"+&r"(cur_buf_warped_y),[buf_warped_dx]"+&r"(cur_buf_warped_dx),[buf_warped_dy]"+&r"(cur_buf_warped_dy),[buf_d]"+&r"(cur_buf_d),[buf_warped_residual]"+&r"(cur_buf_warped_residual),[buf_idepthVar]"+&r"(cur_buf_idepthVar),[buf_weight_p]"+&r"(cur_buf_weight_p),[loop_count]"+&r"(loop_count): /* inputs */ [constants]"r"(constants),[cutoff_res_ponly4]"r"(cutoff_res_ponly4),[sum_vector]"r"(sum_vector): /* clobber */ "memory", "cc","q0", "q1", "q2", "q3", "q4", "q5", "q6", "q7", "q8", "q9", "q10", "q11", "q12", "q13", "q14", "q15");sumRes += sum_vector[0] + sum_vector[1] + sum_vector[2] + sum_vector[3];}
#endiffor(int i=buf_warped_size-remaining; i<buf_warped_size; i++){float px = *(buf_warped_x+i); // x'float py = *(buf_warped_y+i); // y'float pz = *(buf_warped_z+i); // z'float d = *(buf_d+i); // dfloat rp = *(buf_warped_residual+i); // r_pfloat gx = *(buf_warped_dx+i); // \delta_x Ifloat gy = *(buf_warped_dy+i); // \delta_y Ifloat s = settings.var_weight * *(buf_idepthVar+i); // \sigma_d^2// calc dw/dd (first 2 components):float g0 = (tx * pz - tz * px) / (pz*pz*d);float g1 = (ty * pz - tz * py) / (pz*pz*d);// calc w_pfloat drpdd = gx * g0 + gy * g1; // ommitting the minusfloat w_p = 1.0f / (cameraPixelNoise2 + s * drpdd * drpdd);float weighted_rp = fabs(rp*sqrtf(w_p));float wh = fabs(weighted_rp < (settings.huber_d/2) ? 1 : (settings.huber_d/2) / weighted_rp);sumRes += wh * w_p * rp*rp;*(buf_weight_p+i) = wh * w_p;}return sumRes / buf_warped_size;
}
#endif
///
// 每个像素点都有一个光度匹配误差，而每个点匹配的可信度根据其偏导数可以求得，
// 加权每一个误差 然后求所有点 光度匹配误差的均值
// 再者 计算误差的权重，在优化更新函数中作为权重// 使用 误差方差对方差归一化 后求和
// 计算权重 和 像素匹配误差
// 该函数的功能是计算归一化方差的光度误差系数，也就是计算公式(14)，
// 并且乘以了Huber-weight，最终把这个系数存在数组buf_weight_p中。
// 可能是考虑参考帧到当前帧的位姿变换比较小，
// 所以作者只考虑了位移t而忽略旋转R。
// 这样使得式子(14)中的偏导的形式简单了很多。
// 这里主要求 误差函数的导数 即得到 误差的协方差矩阵
float SE3Tracker::calcWeightsAndResidual(const Sophus::SE3f& referenceToFrame)// 参考帧 到 当前帧 欧式变换矩阵
{// 参考帧 到 当前帧 平移向量float tx = referenceToFrame.translation()[0];float ty = referenceToFrame.translation()[1];float tz = referenceToFrame.translation()[2];float sumRes = 0;
// 每个像素点都有一个光度匹配误差，而每个点匹配的可信度根据其偏导数可以求得，
// 加权每一个误差 然后求所有点 光度匹配误差的均值 for(int i=0;i<buf_warped_size;i++)// 对于初步匹配得 每一个 匹配点{// 参考帧 映射到 当前帧 坐标系下的 3d 坐标float px = *(buf_warped_x+i); // x'float py = *(buf_warped_y+i); // y'float pz = *(buf_warped_z+i); // z'float d = *(buf_d+i); // d 参考帧 Z轴 倒数 = 参考帧 逆深度float rp = *(buf_warped_residual+i); // r_p 匹配点对 像素匹配误差float gx = *(buf_warped_dx+i); // \delta_x I gx = dx*fx当前帧 亚像素梯度值gx仿射变换后( 乘以 相机内参数)float gy = *(buf_warped_dy+i); // \delta_y I gy = gy*fyfloat s = settings.var_weight * *(buf_idepthVar+i); // \sigma_d^2 参考帧 逆深度 方差 平方
// 参考帧3D点通过Rt映射到当前帧坐标系下在通过相机内参数映射到当前帧图像平面上
// 的到所谓的光度误差 该误差对参考帧p点处的逆深度D求偏导数得到
// dE = -(dx*fx * (tx*z' - tz*x')/(z'^2*d) + dy*fy * (ty*z' - tz*y')/(z'^2*d))// calc dw/dd (first 2 components):float g0 = (tx * pz - tz * px) / (pz*pz*d);// float g1 = (ty * pz - tz * py) / (pz*pz*d);// 误差 偏导数的结果 calc w_p// 正如源码所注释，这里省略了负号float drpdd = gx * g0 + gy * g1; // ommitting the minusfloat w_p = 1.0f / ((cameraPixelNoise2) + s * drpdd * drpdd);// 误差权重 方差倒数float weighted_rp = fabs(rp*sqrtf(w_p));// |r|加权的误差// 为了减少外点（outliers）对算法的影响，论文中使用了迭代变权重最小二乘// 其实在实现的时候，这里给的权重就是Huber-weight。 float wh = fabs(weighted_rp < (settings.huber_d/2) ? 1 : (settings.huber_d/2) / weighted_rp);//sumRes += wh * w_p * rp*rp;//*(buf_weight_p+i) = wh * w_p;// // 乘上 Huber-weight 权重 的最终误差*(buf_weight_p+i) = wh * w_p;// // 乘上 Huber-weight 权重 的最终误差sumRes += *(buf_weight_p+i) * rp*rp;// 加权误差和}return sumRes / buf_warped_size;// 返回加权误差均值
}
/// 如果要可视化Tracking的迭代过程，那么第一步自然是把debug相关的参数都设置进去，
// 否则直接进行下一步，这个操作是通过调用calcResidualAndBuffers_debugStart()实现的
void SE3Tracker::calcResidualAndBuffers_debugStart()
{// 是否需要 可视化Tracking的迭代过程if(plotTrackingIterationInfo || saveAllTrackingStagesInternal){int other = saveAllTrackingStagesInternal ? 255 : 0;// 填充图像fillCvMat(&debugImageResiduals,cv::Vec3b(other,other,255));fillCvMat(&debugImageWeights,cv::Vec3b(other,other,255));fillCvMat(&debugImageOldImageSource,cv::Vec3b(other,other,255));fillCvMat(&debugImageOldImageWarped,cv::Vec3b(other,other,255));}
}// 完成匹配误差计算 保存误差信息图片
void SE3Tracker::calcResidualAndBuffers_debugFinish(int w)
{// 显示跟踪 优化迭代信息if(plotTrackingIterationInfo){Util::displayImage( "Weights", debugImageWeights );Util::displayImage( "second_frame", debugImageSecondFrame );Util::displayImage( "Intensities of second_frame at transformed positions", debugImageOldImageSource );Util::displayImage( "Intensities of second_frame at pointcloud in first_frame", debugImageOldImageWarped );Util::displayImage( "Residuals", debugImageResiduals );// wait for key and handle itbool looping = true;while(looping){int k = Util::waitKey(1);if(k == -1){if(autoRunWithinFrame)break;elsecontinue;}char key = k;if(key == ' ')looping = false;elsehandleKey(k);// 处理}}
// 保存跟踪迭代信息 图像if(saveAllTrackingStagesInternal){char charbuf[500];snprintf(charbuf,500,"save/%sresidual-%d-%d.png",packagePath.c_str(),w,iterationNumber);// 格式化字符串 图片名cv::imwrite(charbuf,debugImageResiduals);// 写图片 参差snprintf(charbuf,500,"save/%swarped-%d-%d.png",packagePath.c_str(),w,iterationNumber);cv::imwrite(charbuf,debugImageOldImageWarped);snprintf(charbuf,500,"save/%sweights-%d-%d.png",packagePath.c_str(),w,iterationNumber);cv::imwrite(charbuf,debugImageWeights);// 权重图片printf("saved three images for lvl %d, iteration %d\n",w,iterationNumber);// 打印信息}
}//
// 跟踪完成获取 变换矩阵 和 3d 点后 反投影到当前帧 计算 灰度匹配误差
// calcResidualAndBuffers X86平台 SSE指令集优化
#if defined(ENABLE_SSE)
float SE3Tracker::calcResidualAndBuffersSSE(const Eigen::Vector3f* refPoint,const Eigen::Vector2f* refColVar,int* idxBuf,int refNum,Frame* frame,const Sophus::SE3f& referenceToFrame,int level,bool plotResidual)
{return calcResidualAndBuffers(refPoint, refColVar, idxBuf, refNum, frame, referenceToFrame, level, plotResidual);
}
#endif
// calcResidualAndBuffers ARM平台 NENO指令集优化
#if defined(ENABLE_NEON)
float SE3Tracker::calcResidualAndBuffersNEON(const Eigen::Vector3f* refPoint,const Eigen::Vector2f* refColVar,int* idxBuf,int refNum,Frame* frame,const Sophus::SE3f& referenceToFrame,int level,bool plotResidual)
{return calcResidualAndBuffers(refPoint, refColVar, idxBuf, refNum, frame, referenceToFrame, level, plotResidual);
}
#endif// #ifdef的使用和#if defined()的用法一致
// #ifndef又和#if !defined()的用法一致。// 优化相关的函数，参数共有8个，在后面可以看到，其他函数通过 callOptimized 这个宏调用了这个函数进行优化操作
// 匹配帧通过 变换矩阵 和 相机内参数 投影到 当前图像平面上(值为float小数)
// 而原图像上的像素点坐标为 整数值，每一个像素位置对应有，梯度信息
// 那么 浮点数像素坐标对应的像素 是多少呢，根据浮点数坐标 四周的四个整数点坐标的梯度 加权和 后计算 灰度匹配误差
// 根据变换矩阵和原3D点和灰度值 计算匹配点对之间得 像素匹配误差 以及 匹配点对 好坏标志图
/*input:refPoint 参考帧 3d坐标 起始 地址指针refColVar 参考帧 灰度和 逆深度方差 起始地址idxBuf 匹配点好坏标志图 指针frame 当前帧 指针referenceToFrame 参考帧 变换到 当前帧下得 欧式变换矩阵 引用 level 金字塔层级plotResidual 匹配误差图标志*/
float SE3Tracker::calcResidualAndBuffers(const Eigen::Vector3f* refPoint,// 参考帧 3d坐标 起始 地址const Eigen::Vector2f* refColVar,// 参考帧 灰度和 逆深度方差 起始地址int* idxBuf,// 匹配点好坏标志图 指针int refNum,// 参考帧 3d点数量Frame* frame,// 当前帧const Sophus::SE3f& referenceToFrame,// 参考帧 变换到 当前帧下得 欧式变换矩阵 int level,// 金字塔层级bool plotResidual)//显示 匹配误差图标志
{
// 如果要可视化Tracking的迭代过程，那么第一步自然是把debug相关的参数都设置进去，
// 否则直接进行下一步，这个操作是通过调用calcResidualAndBuffers_debugStart()实现的calcResidualAndBuffers_debugStart();
// 然后判断是否可视化残差，如果需要可视化，那么初始化残差if(plotResidual)debugImageResiduals.setTo(0);
// 本地参数设置 int w = frame->width(level);// 图像尺寸int h = frame->height(level);Eigen::Matrix3f KLvl = frame->K(level);// 相机内参数float fx_l = KLvl(0,0);float fy_l = KLvl(1,1);float cx_l = KLvl(0,2);float cy_l = KLvl(1,2);Eigen::Matrix3f rotMat = referenceToFrame.rotationMatrix();// 旋转矩阵Eigen::Vector3f transVec = referenceToFrame.translation();// 平移向量const Eigen::Vector3f* refPoint_max = refPoint + refNum;// 参考帧的 3d点坐标 存储的最大位置const Eigen::Vector4f* frame_gradients = frame->gradients(level);// 当前帧 像素梯度 获取 关键点
// 然后定义后续所要使用的变量int idx=0;float sumResUnweighted = 0;bool* isGoodOutBuffer = idxBuf != 0 ? frame->refPixelWasGood() : 0;// 匹配点 对 匹配效果好坏int goodCount = 0;int badCount = 0;float sumSignedRes = 0;float sxx=0,syy=0,sx=0,sy=0,sw=0;float usageCount = 0;for(;refPoint<refPoint_max; refPoint++, refColVar++, idxBuf++)// 对于每一个 参考帧的 3d点坐标{// 参考帧3d点 R，t变换到 当前相机坐标系下Eigen::Vector3f Wxp = rotMat * (*refPoint) + transVec;// 在 投影到 当前相机 像素平面下float u_new = (Wxp[0]/Wxp[2])*fx_l + cx_l;// float 浮点数类型float v_new = (Wxp[1]/Wxp[2])*fy_l + cy_l;// step 1a: coordinates have to be in image:// (inverse test to exclude NANs)// 投影后点在 图像和合理范围内 判断当前点是否投影到图像中if(!(u_new > 1 && v_new > 1 && u_new < w-2 && v_new < h-2)){if(isGoodOutBuffer != 0)// 指针部位空isGoodOutBuffer[*idxBuf] = false;// 并标记continue;}
// 使用 当前点 右方点 右下方点 正下方点 的灰度梯度信息 以及 相应位置差值作为权重值 线性加权获取加权梯度信息
// 然后差值得到亚像素精度级别的 梯度(注意深度的第三个维度是图像数据)
// 浮点数像素坐标对应的像素 是多少呢，根据浮点数坐标 四周的四个整数点坐标的梯度 加权和Eigen::Vector3f resInterp = getInterpolatedElement43(frame_gradients, u_new, v_new, w);// 之后把参考图像数据做一次 仿射操作，// 疑问1：代码中对参考帧的灰度做了一个仿射变换的处理，这里的原理是什么？//在SVO代码中的确有考虑到两帧见位姿相差过大，因此通过在空间上的仿射变换之后再求灰度的操作。// 但是在这里的代码中没有看出具体原理。float c1 = affineEstimation_a * (*refColVar)[0] + affineEstimation_b;// 参考帧 灰度[0] ; 逆深度方差[1] float c2 = resInterp[2];// 当前帧 亚像素 第三维度是图像 灰度值float residual = c1 - c2;// 匹配点对 的灰度 误差值
// 通过差值自适应算得权重 也就是说，误差越大(匹配效果差)，权重越小 float weight = fabsf(residual) < 5.0f ? 1 : 5.0f / fabsf(residual);// 误差倒数为权重，误差小于5的分子为1 ， 大于5的分子为5 // 这些参数用来迭代计算 下一次 的 仿射变换系数 sxx += c1*c1*weight;// 参考帧 灰度值平方和 带误差倒数 权重syy += c2*c2*weight;// 匹配帧 亚像素 灰度值平方 和 带误差倒数 权重sx += c1*weight;// 参考帧 灰度值 和 带误差倒数 权重sy += c2*weight;// 匹配帧 亚像素 灰度值 和 带误差倒数 权重sw += weight;// 误差倒数 权重 和// 然后判断这个匹配点是好是坏，也是个自适应的阈值，这个阈值为一个最大的差异常数，加上 梯度值平和 乘以一个比例系数， // 这个和 匹配点对 的灰度误差值平方 比较，如果残差的平方小于它，那么认为这个点的估计比较好，然后再把这个判断赋值给isGoodOutBuffer[*idxBuf]// 匹配点对 的灰度 误差值 平和 小于 与 当前帧 点 的x和y方向梯度平和 相关数 时 为好的匹配点bool isGood = residual*residual / (MAX_DIFF_CONSTANT + MAX_DIFF_GRAD_MULT*(resInterp[0]*resInterp[0] + resInterp[1]*resInterp[1])) < 1;if(isGoodOutBuffer != 0)isGoodOutBuffer[*idxBuf] = isGood;// 记录 匹配点对好坏标志
// 之后记录计算得到的这一帧改变的值*(buf_warped_x+idx) = Wxp(0);// 参考帧 3d点 通过R,t变换矩阵 变换到 当前图像坐标系下的坐标值 X*(buf_warped_y+idx) = Wxp(1);// Y*(buf_warped_z+idx) = Wxp(2);// Z// 这里求得 亚像素梯度之后 又乘上了 相机内参数，因为在求 误差偏导数时需要 需要用到 dx*fx dy*fy // calcWeightsAndResidual 求误差导数来求 误差的协方差矩阵 后归一化误差*(buf_warped_dx+idx) = fx_l * resInterp[0];// 当前帧匹配点亚像素 梯度 gx = dx*fx*(buf_warped_dy+idx) = fy_l * resInterp[1];// 匹配点亚像素 梯度 gy = dy*fy *(buf_warped_residual+idx) = residual;// 对应 匹配点 像素误差*(buf_d+idx) = 1.0f / (*refPoint)[2];// 参考帧 Z轴 倒数 = 参考帧逆深度*(buf_idepthVar+idx) = (*refColVar)[1];// 参考帧逆深度方差idx++;// 点 ++// 之后再记录 匹配点像素误差的平方和，以 及匹配点像素误差值(带符号)if(isGood){sumResUnweighted += residual*residual;// 较好的 匹配点像素误差的平方和sumSignedRes += residual;// 较好的 匹配点像素误差和goodCount++;// 较好的匹配点对 计数}elsebadCount++;// 不好的匹配点对 计数
// 匹配点 变换前后 深度得变换 checkPermaRefOverlap()函数也有类似的计算 float depthChange = (*refPoint)[2] / Wxp[2]; // if depth becomes larger: pixel becomes "smaller", hence count it less.usageCount += depthChange < 1 ? depthChange : 1;// 记录深度改变的比例// 调试 如果设置了画图，就把他们可视化出来// DEBUG STUFFif(plotTrackingIterationInfo || plotResidual){// for debug plot only: find x,y again.// horribly inefficient, but who cares at this point...Eigen::Vector3f point = KLvl * (*refPoint);// 参考帧下 3d点对应的 像素点2d坐标int x = point[0] / point[2] + 0.5f;int y = point[1] / point[2] + 0.5f;if(plotTrackingIterationInfo){// 设置参考帧和 当前帧 关键点的 图像 使用 亚像素灰度值作为 点颜色setPixelInCvMat(&debugImageOldImageSource,getGrayCvPixel((float)resInterp[2]),u_new+0.5,v_new+0.5,(width/w));// 当前图像下的 关键点2d 坐标setPixelInCvMat(&debugImageOldImageWarped,getGrayCvPixel((float)resInterp[2]),x,y,(width/w));// 参考帧下得 关键点2d 坐标}if(isGood)// 匹配点对灰度像素匹配误差较小 显示 匹配误差图像setPixelInCvMat(&debugImageResiduals,getGrayCvPixel(residual+128),x,y,(width/w));// 误差越小 颜色越靠中间颜色 elsesetPixelInCvMat(&debugImageResiduals,cv::Vec3b(0,0,255),x,y,(width/w));// 误差较大的点 显示蓝色 rgb}}buf_warped_size = idx;// 匹配点数量＝ 包括好的匹配点 和 不好的匹配点 数量pointUsage = usageCount / (float)refNum;// 深度改变均值lastGoodCount = goodCount;// 好的匹配点计数lastBadCount = badCount;// 不好得匹配点计数lastMeanRes = sumSignedRes / goodCount;// 匹配点像素误差均值// 计算迭代之后得到的 下一次得仿射变换系数affineEstimation_a_lastIt = sqrtf((syy - sy*sy/sw) / (sxx - sx*sx/sw));affineEstimation_b_lastIt = (sy - affineEstimation_a_lastIt*sx)/sw;calcResidualAndBuffers_debugFinish(w);// 主要用来 保存调试信息 图像return sumResUnweighted / goodCount; // 匹配点像素误差的平方和 均值
}// 计算雅克比矩阵J 线性方程系数A,和偏置b，使用LDLT矩阵分解求解方程得到 李代数更新量 dse3
// 利用误差函数对误差向量求导数，求得各个点的误差之间的 协方差矩阵，也是雅克比矩阵Ｊ
// 是计算雅可比以及最小二乘方程的系数
// 对于参考帧中的一个3D点pi位置处的残差求雅可比，这里需要使用链式求导法则
// Ji = 1/z' *dx *fx + 0* dy *fy
// 0*dx *fx + 1/z' *dy *fy
// -1 * - x'/z'^2 *dx*fx - y'/z'^2 *dy*fy
// - x'*y'/z'^2 *dx*fx - (1+y'^2/z'^2) *dy*fy
// (1+x'^2/z'^2)*dx*fx + x'*y'/z'^2*dy*fy
// - y'/z' *dx*fx + x'/z' * dy*fy
// A = J *J转置*(*(buf_weight_p+i))
// b = -J转置*(*(buf_weight_p+i))*lastErr
// Vector6 inc = A.ldlt().solve(b);// LDLT矩阵分解求解 dse3 李代数更新量
#if defined(ENABLE_SSE)
Vector6 SE3Tracker::calculateWarpUpdateSSE(NormalEquationsLeastSquares &ls)
{ls.initialize(width*height);// printf("wupd SSE\n");for(int i=0;i<buf_warped_size-3;i+=4){Vector6 v1,v2,v3,v4;__m128 val1, val2, val3, val4;// redefine pz__m128 pz = _mm_load_ps(buf_warped_z+i);pz = _mm_rcp_ps(pz); // pz := 1/z__m128 gx = _mm_load_ps(buf_warped_dx+i);val1 = _mm_mul_ps(pz, gx); // gx / z => SET [0]//v[0] = z*gx;v1[0] = SSEE(val1,0);v2[0] = SSEE(val1,1);v3[0] = SSEE(val1,2);v4[0] = SSEE(val1,3);__m128 gy = _mm_load_ps(buf_warped_dy+i);val1 = _mm_mul_ps(pz, gy); // gy / z => SET [1]//v[1] = z*gy;v1[1] = SSEE(val1,0);v2[1] = SSEE(val1,1);v3[1] = SSEE(val1,2);v4[1] = SSEE(val1,3);__m128 px = _mm_load_ps(buf_warped_x+i);val1 = _mm_mul_ps(px, gy);val1 = _mm_mul_ps(val1, pz); // px * gy * z__m128 py = _mm_load_ps(buf_warped_y+i);val2 = _mm_mul_ps(py, gx);val2 = _mm_mul_ps(val2, pz); // py * gx * zval1 = _mm_sub_ps(val1, val2); // px * gy * z - py * gx * z => SET [5]//v[5] = -py * z * gx + px * z * gy;v1[5] = SSEE(val1,0);v2[5] = SSEE(val1,1);v3[5] = SSEE(val1,2);v4[5] = SSEE(val1,3);// redefine pzpz = _mm_mul_ps(pz,pz); // pz := 1/(z*z)// will use these for the following calculations a lot.val1 = _mm_mul_ps(px, gx);val1 = _mm_mul_ps(val1, pz); // px * z_sqr * gxval2 = _mm_mul_ps(py, gy);val2 = _mm_mul_ps(val2, pz); // py * z_sqr * gyval3 = _mm_add_ps(val1, val2);val3 = _mm_sub_ps(_mm_setr_ps(0,0,0,0),val3); //-px * z_sqr * gx -py * z_sqr * gy//v[2] = -px * z_sqr * gx -py * z_sqr * gy; => SET [2]v1[2] = SSEE(val3,0);v2[2] = SSEE(val3,1);v3[2] = SSEE(val3,2);v4[2] = SSEE(val3,3);val3 = _mm_mul_ps(val1, py); // px * z_sqr * gx * pyval4 = _mm_add_ps(gy, val3); // gy + px * z_sqr * gx * pyval3 = _mm_mul_ps(val2, py); // py * py * z_sqr * gyval4 = _mm_add_ps(val3, val4); // gy + px * z_sqr * gx * py + py * py * z_sqr * gyval4 = _mm_sub_ps(_mm_setr_ps(0,0,0,0),val4); //val4 = -val4.//v[3] = -px * py * z_sqr * gx +// -py * py * z_sqr * gy +// -gy; => SET [3]v1[3] = SSEE(val4,0);v2[3] = SSEE(val4,1);v3[3] = SSEE(val4,2);v4[3] = SSEE(val4,3);val3 = _mm_mul_ps(val1, px); // px * px * z_sqr * gxval4 = _mm_add_ps(gx, val3); // gx + px * px * z_sqr * gxval3 = _mm_mul_ps(val2, px); // px * py * z_sqr * gyval4 = _mm_add_ps(val4, val3); // gx + px * px * z_sqr * gx + px * py * z_sqr * gy//v[4] = px * px * z_sqr * gx +// px * py * z_sqr * gy +// gx; => SET [4]v1[4] = SSEE(val4,0);v2[4] = SSEE(val4,1);v3[4] = SSEE(val4,2);v4[4] = SSEE(val4,3);// step 6: integrate into A and b:ls.update(v1, *(buf_warped_residual+i+0), *(buf_weight_p+i+0));if(i+1>=buf_warped_size) break;ls.update(v2, *(buf_warped_residual+i+1), *(buf_weight_p+i+1));if(i+2>=buf_warped_size) break;ls.update(v3, *(buf_warped_residual+i+2), *(buf_weight_p+i+2));if(i+3>=buf_warped_size) break;ls.update(v4, *(buf_warped_residual+i+3), *(buf_weight_p+i+3));}Vector6 result;// solve lsls.finish();ls.solve(result);return result;
}
#endif#if defined(ENABLE_NEON)
Vector6 SE3Tracker::calculateWarpUpdateNEON(NormalEquationsLeastSquares &ls)
{
// weightEstimator.reset();
// weightEstimator.estimateDistributionNEON(buf_warped_residual, buf_warped_size);
// weightEstimator.calcWeightsNEON(buf_warped_residual, buf_warped_weights, buf_warped_size);ls.initialize(width*height);float* cur_buf_warped_z = buf_warped_z;float* cur_buf_warped_x = buf_warped_x;float* cur_buf_warped_y = buf_warped_y;float* cur_buf_warped_dx = buf_warped_dx;float* cur_buf_warped_dy = buf_warped_dy;Vector6 v1,v2,v3,v4;float* v1_ptr;float* v2_ptr;float* v3_ptr;float* v4_ptr;for(int i=0;i<buf_warped_size;i+=4){v1_ptr = &v1[0];v2_ptr = &v2[0];v3_ptr = &v3[0];v4_ptr = &v4[0];__asm__ __volatile__("vldmia %[buf_warped_z]!, {q10} \n\t" // pz(q10)"vrecpe.f32 q10, q10 \n\t" // z(q10)"vldmia %[buf_warped_dx]!, {q11} \n\t" // gx(q11)"vmul.f32 q0, q10, q11 \n\t" // q0 = z*gx // = v[0]"vldmia %[buf_warped_dy]!, {q12} \n\t" // gy(q12)"vmul.f32 q1, q10, q12 \n\t" // q1 = z*gy // = v[1]"vldmia %[buf_warped_x]!, {q13} \n\t" // px(q13)"vmul.f32 q5, q13, q12 \n\t" // q5 = px * gy"vmul.f32 q5, q5, q10 \n\t" // q5 = q5 * z = px * gy * z"vldmia %[buf_warped_y]!, {q14} \n\t" // py(q14)"vmul.f32 q3, q14, q11 \n\t" // q3 = py * gx"vmls.f32 q5, q3, q10 \n\t" // q5 = px * gy * z - py * gx * z // = v[5] (vmls: multiply and subtract from result)"vmul.f32 q10, q10, q10 \n\t" // q10 = 1/(pz*pz)"vmul.f32 q6, q13, q11 \n\t""vmul.f32 q6, q6, q10 \n\t" // q6 = val1 in SSE version = px * z_sqr * gx"vmul.f32 q7, q14, q12 \n\t""vmul.f32 q7, q7, q10 \n\t" // q7 = val2 in SSE version = py * z_sqr * gy"vadd.f32 q2, q6, q7 \n\t""vneg.f32 q2, q2 \n\t" // q2 = -px * z_sqr * gx -py * z_sqr * gy // = v[2]"vmul.f32 q8, q6, q14 \n\t" // val3(q8) = px * z_sqr * gx * py"vadd.f32 q9, q12, q8 \n\t" // val4(q9) = gy + px * z_sqr * gx * py"vmul.f32 q8, q7, q14 \n\t" // val3(q8) = py * py * z_sqr * gy"vadd.f32 q9, q8, q9 \n\t" // val4(q9) = gy + px * z_sqr * gx * py + py * py * z_sqr * gy"vneg.f32 q3, q9 \n\t" // q3 = v[3]"vst4.32 {d0[0], d2[0], d4[0], d6[0]}, [%[v1]]! \n\t" // store v[0] .. v[3] for 1st value and inc pointer"vst4.32 {d0[1], d2[1], d4[1], d6[1]}, [%[v2]]! \n\t" // store v[0] .. v[3] for 2nd value and inc pointer"vst4.32 {d1[0], d3[0], d5[0], d7[0]}, [%[v3]]! \n\t" // store v[0] .. v[3] for 3rd value and inc pointer"vst4.32 {d1[1], d3[1], d5[1], d7[1]}, [%[v4]]! \n\t" // store v[0] .. v[3] for 4th value and inc pointer"vmul.f32 q8, q6, q13 \n\t" // val3(q8) = px * px * z_sqr * gx"vadd.f32 q9, q11, q8 \n\t" // val4(q9) = gx + px * px * z_sqr * gx"vmul.f32 q8, q7, q13 \n\t" // val3(q8) = px * py * z_sqr * gy"vadd.f32 q4, q9, q8 \n\t" // q4 = v[4]"vst2.32 {d8[0], d10[0]}, [%[v1]] \n\t" // store v[4], v[5] for 1st value"vst2.32 {d8[1], d10[1]}, [%[v2]] \n\t" // store v[4], v[5] for 2nd value"vst2.32 {d9[0], d11[0]}, [%[v3]] \n\t" // store v[4], v[5] for 3rd value"vst2.32 {d9[1], d11[1]}, [%[v4]] \n\t" // store v[4], v[5] for 4th value: /* outputs */ [buf_warped_z]"+r"(cur_buf_warped_z),[buf_warped_x]"+r"(cur_buf_warped_x),[buf_warped_y]"+r"(cur_buf_warped_y),[buf_warped_dx]"+r"(cur_buf_warped_dx),[buf_warped_dy]"+r"(cur_buf_warped_dy),[v1]"+r"(v1_ptr),[v2]"+r"(v2_ptr),[v3]"+r"(v3_ptr),[v4]"+r"(v4_ptr): /* inputs */ : /* clobber */ "memory", "cc", // TODO: is cc necessary?"q0", "q1", "q2", "q3", "q4", "q5", "q6", "q7", "q8", "q9", "q10", "q11", "q12", "q13", "q14");// step 6: integrate into A and b:if(!(i+3>=buf_warped_size)){ls.update(v1, *(buf_warped_residual+i+0), *(buf_weight_p+i+0));ls.update(v2, *(buf_warped_residual+i+1), *(buf_weight_p+i+1));ls.update(v3, *(buf_warped_residual+i+2), *(buf_weight_p+i+2));ls.update(v4, *(buf_warped_residual+i+3), *(buf_weight_p+i+3));}else{ls.update(v1, *(buf_warped_residual+i+0), *(buf_weight_p+i+0));if(i+1>=buf_warped_size) break;ls.update(v2, *(buf_warped_residual+i+1), *(buf_weight_p+i+1));if(i+2>=buf_warped_size) break;ls.update(v3, *(buf_warped_residual+i+2), *(buf_weight_p+i+2));if(i+3>=buf_warped_size) break;ls.update(v4, *(buf_warped_residual+i+3), *(buf_weight_p+i+3));}}Vector6 result;// solve lsls.finish();ls.solve(result);return result;
}
#endif// 计算雅克比矩阵J 线性方程系数A,和偏置b，使用LDLT矩阵分解求解方程得到 李代数更新量 dse3
// 利用误差函数对误差向量求导数，求得各个点的误差之间的 协方差矩阵，也是雅克比矩阵Ｊ
// 是计算雅可比以及最小二乘方程的系数
// 对于参考帧中的一个3D点pi位置处的残差求雅可比，这里需要使用链式求导法则
// Ji = 1/z' *dx *fx + 0* dy *fy
// 0*dx *fx + 1/z' *dy *fy
// -1 * - x'/z'^2 *dx*fx - y'/z'^2 *dy*fy
// - x'*y'/z'^2 *dx*fx - (1+y'^2/z'^2) *dy*fy
// (1+x'^2/z'^2)*dx*fx + x'*y'/z'^2*dy*fy
// - y'/z' *dx*fx + x'/z' * dy*fy
// A = J *J转置*(*(buf_weight_p+i))
// b = -J转置*(*(buf_weight_p+i))*lastErr
// Vector6 inc = A.ldlt().solve(b);// LDLT矩阵分解求解 dse3 李代数更新量
Vector6 SE3Tracker::calculateWarpUpdate(NormalEquationsLeastSquares &ls)
{
// weightEstimator.reset();
// weightEstimator.estimateDistribution(buf_warped_residual, buf_warped_size);
// weightEstimator.calcWeights(buf_warped_residual, buf_warped_weights, buf_warped_size);
//ls.initialize(width*height);for(int i=0;i<buf_warped_size;i++){float px = *(buf_warped_x+i);//x'float py = *(buf_warped_y+i);//y'float pz = *(buf_warped_z+i);//z'float r = *(buf_warped_residual+i);//误差float gx = *(buf_warped_dx+i);// dx*fxfloat gy = *(buf_warped_dy+i);//dy*fx// step 3 + step 5 comp 6d error vectorfloat z = 1.0f / pz;// 1/z'float z_sqr = 1.0f / (pz*pz);//1/z'^2Vector6 v;v[0] = z*gx + 0;v[1] = 0 + z*gy;v[2] = (-px * z_sqr) * gx +(-py * z_sqr) * gy;v[3] = (-px * py * z_sqr) * gx +(-(1.0 + py * py * z_sqr)) * gy;v[4] = (1.0 + px * px * z_sqr) * gx +(px * py * z_sqr) * gy;v[5] = (-py * z) * gx +(px * z) * gy;// step 6: integrate into A and b:// J转置*J * dse3 = -J转置*w*error// v为求得的雅克比矩阵J r为误差 *(buf_weight_p+i)为误差权重 ls.update(v, r, *(buf_weight_p+i));// 这里计算的是 A 和 b的和 以及加权误差平方和 error }
// 欧式变换矩阵se3 更新后的量 Vector6 result;// solve ls// 对求得的 A 和 b的和以及加权误差平方和 error 求均值ls.finish();// LDLT求解线性方程组 A*x=b x = A.ldlt().solve(b);ls.solve(result);return result;}}lsb_slam Tracking线程 SE3Tracking 欧式变换矩阵跟踪参考帧 加权高斯牛顿优化算法WLM 最小二乘优化 归一化方差的光度误差函数 偏导数雅克比矩阵J 线性方程组LDLT求解相关推荐
- orbslam2代码详解之tracking线程——局部地图跟踪
目录 局部地图跟踪 TrackLocalMap() UpdateLocalMap() UpdateLocalKeyFrames() UpdateLocalPoints() SearchLocalPoi ...
- ORB_SLAM2中Tracking线程
Tracking线程是ORB_SLAM2的主线程.在System.cc中,使用构造函数进行了初始化,开启了三个线程. 可执行程序->System构造函数(初始化三个线程)->处理输入的 ...
- ORB_SLAM2代码阅读(2)——tracking线程
ORB_SLAM2代码阅读(2)--Tracking线程 1. 说明 2. 简介 2.1 Tracking 流程 2.2 Tracking 线程的二三四 2.2.1 Tracking 线程的二种模式 ...
- ORB-SLAM2代码阅读笔记（五）：Tracking线程3——Track函数中单目相机初始化
Table of Contents 1.特征点匹配相关理论简介 2.ORB-SLAM2中特征匹配代码分析 (1)Tracking线程中的状态机 (2)单目相机初始化函数MonocularInitial ...
- ORB_SLAM2中Tracking线程的三种追踪方式
1.参考关键帧追踪模式 bool Tracking::TrackReferenceKeyFrame() 对参考关键帧中的路标点进行跟踪.在Tracking线程中,每传入一帧,都会进行位姿优化. ...
- 一起研究ORB-SLAM（二）---Tracking线程
转载自 一起研究ORB-SLAM(二) 上一篇文章我讲述就ORB-SLAM的基本流程,还记得ORB-SLAM分为哪三个主要的线程吗?在脑子里头大声的所出来吧,Tracking.LOCAL MAPPIN ...
- PULT：Progressive Unsupervised Learning for Visual Object Tracking（用于视觉目标跟踪的渐进式无监督学习）
Progressive Unsupervised Learning for Visual Object Tracking(用于视觉目标跟踪的渐进式无监督学习 ) 因为是无监督学习,所以需要对样本数据充 ...
- App性能优化（布局优化，线程优化，app瘦身优化，页面切换优化，App启动优化，内存优化）
Android APP性能优化(最新总结) 在目前Android开发中,UI布局可以说是每个App使用频率很高的,随着UI越来越多,布局的重复性.复杂度也随之增长,这样使得UI布局的优化,显得至关重要 ...
- 论文学习-卫星视频与目标追踪-1-融合KCF跟踪器和三帧差算法
论文学习-卫星视频与目标追踪-1 大家好,近来一直在研究基于视频卫星的目标追踪领域.为了更好地梳理自己的论文学习过程,故采用博客的方式记录下来.接下来我会将此领域一些我觉得典型的有意义的论文,以我自己 ...
- 45.JVM调优策略、常见问题：内存泄漏(年老代堆空间被占满、持久代被占满、堆栈溢出、线程堆栈满、系统内存被占满)优化方法：优化目标、优化GC步骤、优化总结；案例分析（公司系统参数、网上给的配置参数）
45.JVM调优策略 45.1.常见问题 45.1.1.内存泄漏 45.1.1.1.年老代堆空间被占满 45.1.1.2.持久代被占满 45.1.1.3.堆栈溢出 45.1.1.4.线程堆栈满 45. ...
最新文章
- 2020互联网公司中秋礼盒大比拼！（文末送福利）
- Math4DS 直播 NO.10 | “机器学习之父”、加州大学伯克利分校迈克尔·乔丹
- 【开源框架】Android之史上最全最简单最有用的第三方开源库收集整理，有助于快速开发，欢迎各位网友补充完善...
- spring boot(一)：Hello World
- Java虚拟机类加载机制--类加载器详解
- android高德地图热力图,2D 热力图-热力 HeatmapLayer-示例中心-Loca API 示例 | 高德地图API...
- 五胡十六国、东晋南北朝这280年历史，你知道多少？5000字带你看个清楚明白
- [node] 对某网站的简单爬虫
- latex 编译生成的 PDF 中字体有锯齿
- VS2019提示“未能完成操作，不支持此接口”
- npm install 报错 gyp info it worked if it ends with ok
- 安卓京东自动炸年兽v4.1.1
- 通过一款早期代码抽取壳入门学习 so 层分析
- 等保（公安部82号令）
- Cache Tiering
- 计算机操作系统——LINUX的C语言编程与shell编程
- 小程序源码：2022强大的修复版趣味心理测试小程序源码,趣味测试引流裂变神器-多玩法安装简单
- 计算机虚拟化技术的未来前景,计算机虚拟化技术及应用前景分析
- php实现二级下拉菜单,jquery,_用jquery实现二级下拉菜单，jquery - phpStudy
- 汽车-轿车五档变速器毕业设计