数据结构
lsd_slam代码 算法分析Frame 帧类 详情
* 每张图像创建 5层的图像金字塔 每一层的尺度 变为上一层的1/2
* 图像的 内参数 也上上一层的 1/2
* 内参数求逆得到 内参数逆矩阵
* 一帧包含 Ki = [Ii,Gi,Mi,Di,Vi]
* [像素 梯度 最大梯度值 逆深度 逆深度方差]
* 最大梯度 阈值滤波得到 关键点 需要跟踪 并需要计算三维点映射到地图中
一、图像金字塔构建方法为 :
* 上一层 的 四个像素的值的平均值合并成一个像素为下一层的像素
*
* int wh = width*height;// 当前层 像素总数
* const float* s;
* for(int y=0; y<wh; y += width*2)// 隔行
* {
* for(int x=0; x<width; x+= 2)// 隔列下采样
* {
* s = source + x + y;// 上一层 像素对应位置
* *dest = (s[0] +
* s[1] +
* s[width] +
* s[1+width]) * 0.25f;// 四个像素的值的平均值合并成一个
* dest++;
* }
* }
*
二、梯度金字塔构建方法(四个值 dx , dy, i, null)
* 使用同一层的 图像 左右像素求得x方向梯度 上下求得 方向梯度
* *(img_pt-width)
* val_m1 *(img_pt) val_p1
* *(img_pt+width)
* 1. (val_p1 - val_m1)/2 = x 方向梯度
* 2. 0.5f*(*(img_pt+width) - *(img_pt-width)) = y方向梯度
* 3. val_00 = *(img_pt) 当前 点像素值
* 4. 第四维度 没有存储数据 gradxyii_pt Eigen::Vector4f
*
*
三、临近最大合成梯度 值 地图构建 一个合成梯度值
* 创建 梯度图内 临近四点中梯度最大值 的 最大值梯度 图 , 并记录梯度值较大的可以映射 成 地图点的数量
* 在梯度图中 求去合成梯度 g=sqrt(gx^2+gy^2) ,求的 上中下 三个梯度值中的最大值,形成临时梯度最大值图
* 在临时梯度最大值图 中求 的 左中右 三个梯度值中的最大值,形成最后的 最大梯度值地图
* 并记录 最大梯度大小超过阈值的点 可以映射成地图点
*
四、构建 第0层 逆深度均值图 和方差图
* 1. 使用 真实 深度值 取反得到逆深度值,方差初始为一个设定值
* 2. 没有真实值是,也可以使用高斯分布均值初始化 逆深度均值图 和方差图
*
五、高层逆深度均值金字塔图 和逆深度方差金字塔图的构建
*
* 根据逆深度 构建 逆深度均值图 方差图(高斯分布) 金字塔
* current -----> 右边一个
* 下边 下右边 上一层四个位置
* 上一层 逆方差和 / 上一层 逆深度均值 (四个位置处) 和 得到深度信息 再 取逆得到 逆深度均值
* 上一层 逆深度 方差和 取逆得到 本层 逆深度方差
Frame.cpp
#include "DataStructures/Frame.h" #include "DataStructures/FrameMemory.h" #include "DepthEstimation/DepthMapPixelHypothesis.h" #include "Tracking/TrackingReference.h" namespace lsd_slam { int privateFrameAllocCount = 0; // 构造函数有两个,主要是最后一个参数给的有所不同,实际上它代表的是两种不同格式的图片数据 // 构造函数 0~255 的图像 Frame::Frame(int id, int width, int height, const Eigen::Matrix3f& K, double timestamp, const unsigned char* image) { // 类参数变量初始化 主要初始化 图像金字塔的 内参数 内参数逆 initialize(id, width, height, K, timestamp); // 获取图像内存空间 data.image[0] = FrameMemory::getInstance().getFloatBuffer(data.width[0]*data.height[0]);//float类型存储 指针 float* maxPt = data.image[0] + data.width[0]*data.height[0];// 存储区域 float 的个数 对应像素个数 // 使用指针复制图像的内一个像素 for(float* pt = data.image[0]; pt < maxPt; pt++)// 拷贝每一个像素 { *pt = *image;// 拷贝每一个像素 image++;// 指针指向下一个 } data.imageValid[0] = true;// 第0层图像金字塔 已经有图像数据了 privateFrameAllocCount++;// 私有帧数量++ if(enablePrintDebugInfo && printMemoryDebugInfo)// 调试信息 printf("ALLOCATED frame %d, now there are %d\n", this->id(), privateFrameAllocCount); } // 构造函数 0~1 的图像 Frame::Frame(int id, int width, int height, const Eigen::Matrix3f& K, double timestamp, const float* image) { // 类参数变量初始化 主要初始化 图像金字塔的 内参数 内参数逆 initialize(id, width, height, K, timestamp); // 获取图像内存空间 data.image[0] = FrameMemory::getInstance().getFloatBuffer(data.width[0]*data.height[0]); // 使用memcpy 复制图像内容 两个指针指向的类型一直 memcpy(data.image[0], image, data.width[0]*data.height[0] * sizeof(float));// 指针类型相同 使用 memcpy拷贝内容 data.imageValid[0] = true;// 第0层图像金字塔 已经有图像数据了 privateFrameAllocCount++;// 私有帧数量++ if(enablePrintDebugInfo && printMemoryDebugInfo)// 调试信息 printf("ALLOCATED frame %d, now there are %d\n", this->id(), privateFrameAllocCount); } // 析构函数 Frame::~Frame() { // 调试信息 if(enablePrintDebugInfo && printMemoryDebugInfo) printf("DELETING frame %d\n", this->id());// 打印信息 删除 帧 // 回收内存 FrameMemory::getInstance().deactivateFrame(this);// 这里只是回收内存 if(!pose->isRegisteredToGraph) delete pose;// 还没有 注册放入到地图 就删除位姿 else pose->frame = 0; // 删除图像金字塔的每一层 图像 for (int level = 0; level < PYRAMID_LEVELS; ++ level)// 每一层图像金字塔 { FrameMemory::getInstance().returnBuffer(data.image[level]);// 图像 FrameMemory::getInstance().returnBuffer(reinterpret_cast<float*>(data.gradients[level]));// 梯度 FrameMemory::getInstance().returnBuffer(data.maxGradients[level]);// FrameMemory::getInstance().returnBuffer(data.idepth[level]);// 逆深度 均值 FrameMemory::getInstance().returnBuffer(data.idepthVar[level]);// 逆深度方差 } // 删除 深度 逆深度均值 逆深度方差 FrameMemory::getInstance().returnBuffer((float*)data.validity_reAct); FrameMemory::getInstance().returnBuffer(data.idepth_reAct); FrameMemory::getInstance().returnBuffer(data.idepthVar_reAct); // 最后再释放permaRef_colorAndVarData和permaRef_posData,这两个参数是位置,颜色和方差的引用,用于重定位, // 注意:这个参数只是在initialize中初始化为空指针 if(permaRef_colorAndVarData != 0) delete permaRef_colorAndVarData;// 删除 颜色和方差的引用 if(permaRef_posData != 0) delete permaRef_posData;// 位置 的 引用 privateFrameAllocCount--;/// 私有帧数量-- if(enablePrintDebugInfo && printMemoryDebugInfo)// 调试信息 printf("DELETED frame %d, now there are %d\n", this->id(), privateFrameAllocCount); } void Frame::takeReActivationData(DepthMapPixelHypothesis* depthMap) { // 上锁 boost::shared_lock<boost::shared_mutex> lock = getActiveLock(); // 申请内存 if(data.validity_reAct == 0) data.validity_reAct = (unsigned char*) FrameMemory::getInstance().getBuffer(data.width[0]*data.height[0]); if(data.idepth_reAct == 0) data.idepth_reAct = FrameMemory::getInstance().getFloatBuffer((data.width[0]*data.height[0])); if(data.idepthVar_reAct == 0) data.idepthVar_reAct = FrameMemory::getInstance().getFloatBuffer((data.width[0]*data.height[0])); // 赋值 float* id_pt = data.idepth_reAct;// 起始位置 float* id_pt_max = data.idepth_reAct + (data.width[0]*data.height[0]);// 最大位置 float* idv_pt = data.idepthVar_reAct; unsigned char* val_pt = data.validity_reAct; for (; id_pt < id_pt_max; ++ id_pt, ++ idv_pt, ++ val_pt, ++depthMap) { if(depthMap->isValid)// 深度图 有效 { *id_pt = depthMap->idepth;// 深度值 *idv_pt = depthMap->idepth_var;// 深度值方差 *val_pt = depthMap->validity_counter; } else if(depthMap->blacklisted < MIN_BLACKLIST) { *idv_pt = -2; } else { *idv_pt = -1; } } data.reActivationDataValid = true; } // 参考帧 void Frame::setPermaRef(TrackingReference* reference) { assert(reference->frameID == id()); reference->makePointCloud(QUICK_KF_CHECK_LVL); permaRef_mutex.lock();// 上锁 if(permaRef_colorAndVarData != 0) delete permaRef_colorAndVarData; if(permaRef_posData != 0) delete permaRef_posData; permaRefNumPts = reference->numData[QUICK_KF_CHECK_LVL]; permaRef_colorAndVarData = new Eigen::Vector2f[permaRefNumPts]; permaRef_posData = new Eigen::Vector3f[permaRefNumPts]; memcpy(permaRef_colorAndVarData, reference->colorAndVarData[QUICK_KF_CHECK_LVL], sizeof(Eigen::Vector2f) * permaRefNumPts); memcpy(permaRef_posData, reference->posData[QUICK_KF_CHECK_LVL], sizeof(Eigen::Vector3f) * permaRefNumPts); permaRef_mutex.unlock(); } // 求解 深度值标准差逆( 深度值方差逆 开根号 ) 均值(求和 取平均) void Frame::calculateMeanInformation() { return; if(numMappablePixels < 0) maxGradients(0); const float* idv = idepthVar(0);// 深度方差 起始位置 const float* idv_max = idv + width(0)*height(0);// 最大位置 float sum = 0; int goodpx = 0; for(const float* pt=idv; pt < idv_max; pt++) { if(*pt > 0) { sum += sqrtf(1.0f / *pt);// 深度值方差逆 开根号 得到 深度值标准差逆 和 goodpx++; } } meanInformation = sum / goodpx;// 深度值标准差逆 均值 } //////////////////////////////////////////////////////////////////////////////////////////// // 使用 逆深度图深度值假设值(高斯分布平滑后的数据) 设置 第0层的 逆深度均值和方差 对深度估计值进行设定 void Frame::setDepth(const DepthMapPixelHypothesis* newDepth) { // 首先调用了锁,把数据都锁了起来 boost::shared_lock<boost::shared_mutex> lock = getActiveLock(); boost::unique_lock<boost::mutex> lock2(buildMutex); // 申请第0层的 深度均值和方差 的存储空间 if(data.idepth[0] == 0) data.idepth[0] = FrameMemory::getInstance().getFloatBuffer(data.width[0]*data.height[0]); if(data.idepthVar[0] == 0) data.idepthVar[0] = FrameMemory::getInstance().getFloatBuffer(data.width[0]*data.height[0]); float* pyrIDepth = data.idepth[0];// 第一个内存地址的指针 float* pyrIDepthVar = data.idepthVar[0]; float* pyrIDepthMax = pyrIDepth + (data.width[0]*data.height[0]);// 最大的内存地址 float sumIdepth=0;// 逆深度均值和 int numIdepth=0;// 有 有效逆深度值 的 像素点数量 for (; pyrIDepth < pyrIDepthMax; ++ pyrIDepth, ++ pyrIDepthVar, ++ newDepth) //, ++ pyrRefID) { if (newDepth->isValid && newDepth->idepth_smoothed >= -0.05)// 预设值的值符合 一些前提条件 { *pyrIDepth = newDepth->idepth_smoothed;// 逆深度均值 *pyrIDepthVar = newDepth->idepth_var_smoothed;// 逆深度方差 numIdepth++;// 有 有效逆深度值 的 像素点数量 sumIdepth += newDepth->idepth_smoothed;// 逆深度均值和 } // 预设值不符合规定,设置异常值 -1 else { *pyrIDepth = -1; *pyrIDepthVar = -1; } } meanIdepth = sumIdepth / numIdepth;// 逆深度均值 numPoints = numIdepth;// 有 有效逆深度值 的 像素点数量 data.idepthValid[0] = true;// 第0层逆深度值 已经得到 data.idepthVarValid[0] = true;// 第0层逆深度均值 已经得到 release(IDEPTH | IDEPTH_VAR, true, true);// 最后调用release,释放第0层以上层的深度估计值 data.hasIDepthBeenSet = true; depthHasBeenUpdatedFlag = true;// 更新标志 } // 从 深度真实值 设置 第0层的逆深度值和方差 void Frame::setDepthFromGroundTruth(const float* depth, float cov_scale) { // 首先调用了锁,把数据都锁了起来 boost::shared_lock<boost::shared_mutex> lock = getActiveLock(); const float* pyrMaxGradient = maxGradients(0); boost::unique_lock<boost::mutex> lock2(buildMutex); // 申请第0层的 深度均值和方差 的存储空间 if(data.idepth[0] == 0) data.idepth[0] = FrameMemory::getInstance().getFloatBuffer(data.width[0]*data.height[0]); if(data.idepthVar[0] == 0) data.idepthVar[0] = FrameMemory::getInstance().getFloatBuffer(data.width[0]*data.height[0]); float* pyrIDepth = data.idepth[0];// 第一个内存地址的指针 float* pyrIDepthVar = data.idepthVar[0]; int width0 = data.width[0];// 深度值图 宽度 int height0 = data.height[0];// 高度 for(int y=0;y<height0;y++)// 每一行 { for(int x=0;x<width0;x++)// 每一列 { // 有效位置 if (x > 0 && x < width0-1 && y > 0 && y < height0-1 && // pyramidMaxGradient is not valid for the border pyrMaxGradient[x+y*width0] >= MIN_ABS_GRAD_CREATE && !isnanf(*depth) && *depth > 0) { *pyrIDepth = 1.0f / *depth;// 逆深度值 *pyrIDepthVar = VAR_GT_INIT_INITIAL * cov_scale;// 逆深度方差 } // 无效位置 else { *pyrIDepth = -1; *pyrIDepthVar = -1; } ++ depth; ++ pyrIDepth; ++ pyrIDepthVar; } } // 设置标志 data.idepthValid[0] = true;// 第0层 逆深度值 已经得到 data.idepthVarValid[0] = true; // data.refIDValid[0] = true; // Invalidate higher levels, they need to be updated with the new data release(IDEPTH | IDEPTH_VAR, true, true);// 最后调用release,释放第0层以上层的深度估计值 data.hasIDepthBeenSet = true; } //////////////////////////////////////////////// // 这个函数是用来设置变换的,准备 两帧之前的 旋转 平移 变换矩阵 为双目三角测量得到深度做准备 // 第一个参数传入是哪一帧, // 第二个参数是这一帧的相似变换矩阵 ----> sR, t // 第三个参数是相机参数 K , // 第四个参数是金字塔等级 level void Frame::prepareForStereoWith(Frame* other, Sim3 thisToOther, const Eigen::Matrix3f& K, const int level) { Sim3 otherToThis = thisToOther.inverse(); // 1. other 变换到 当前帧 K*T = K*(R +t) = K*R + K*t //otherToThis = data.worldToCam * other->data.camToWorld; K_otherToThis_R = K * otherToThis.rotationMatrix().cast<float>() * otherToThis.scale();// K*R otherToThis_t = otherToThis.translation().cast<float>(); K_otherToThis_t = K * otherToThis_t;// K*t // 2. 当前帧 变换到 other K*T = K*(R +t) = K*R + K*t thisToOther_t = thisToOther.translation().cast<float>(); K_thisToOther_t = K * thisToOther_t; thisToOther_R = thisToOther.rotationMatrix().cast<float>() * thisToOther.scale(); // 3. other 变换到 当前帧 旋转矩阵的每一行 R逆 = R转置 otherToThis_R_row0 = thisToOther_R.col(0);// 第0列 转置 第0行 otherToThis_R_row1 = thisToOther_R.col(1);// 第1列 转置 第1行 otherToThis_R_row2 = thisToOther_R.col(2);// 第2列 转置 第2行 // 4. 两帧之间的 距离 平方 t * t' distSquared = otherToThis.translation().dot(otherToThis.translation()); //参考帧 referenceID = other->id(); referenceLevel = level; } // 是某种请求函数 // 判断需要怎样的数据,如果这个数据没有,就调用相应的构建函数build* void Frame::require(int dataFlags, int level) { if ((dataFlags & IMAGE) && ! data.imageValid[level]) { buildImage(level);// 第几层的图像没有 则创建 递归创建 从最大的图开始 依次 下采样 获取 金字塔图像 } if ((dataFlags & GRADIENTS) && ! data.gradientsValid[level]) { buildGradients(level);// 第几层的 梯度没有 则创建 } if ((dataFlags & MAX_GRADIENTS) && ! data.maxGradientsValid[level]) { buildMaxGradients(level);// 第几层的 最大梯度 没有 则创建 } if (((dataFlags & IDEPTH) && ! data.idepthValid[level]) || ((dataFlags & IDEPTH_VAR) && ! data.idepthVarValid[level])) { buildIDepthAndIDepthVar(level);// 第几层的 逆深度 没有 则创建 } } // 是某种释放函数 void Frame::release(int dataFlags, bool pyramidsOnly, bool invalidateOnly) { for (int level = (pyramidsOnly ? 1 : 0); level < PYRAMID_LEVELS; ++ level) { if ((dataFlags & IMAGE) && data.imageValid[level]) { data.imageValid[level] = false; if(!invalidateOnly) releaseImage(level);// 释放 图像 } if ((dataFlags & GRADIENTS) && data.gradientsValid[level]) { data.gradientsValid[level] = false; if(!invalidateOnly) releaseGradients(level);// 释放 梯度图 } if ((dataFlags & MAX_GRADIENTS) && data.maxGradientsValid[level]) { data.maxGradientsValid[level] = false; if(!invalidateOnly) releaseMaxGradients(level);// 释放最大梯度图 } if ((dataFlags & IDEPTH) && data.idepthValid[level]) { data.idepthValid[level] = false; if(!invalidateOnly) releaseIDepth(level);// 释放逆深度图 } if ((dataFlags & IDEPTH_VAR) && data.idepthVarValid[level]) { data.idepthVarValid[level] = false; if(!invalidateOnly) releaseIDepthVar(level);// 释放逆深度图 值 } } } // 是最小化储存函数 // 就是释放一些内存 bool Frame::minimizeInMemory() { if(activeMutex.timed_lock(boost::posix_time::milliseconds(10))) { buildMutex.lock(); // 打印信息 if(enablePrintDebugInfo && printMemoryDebugInfo) printf("minimizing frame %d\n",id()); release(IMAGE | IDEPTH | IDEPTH_VAR, true, false); release(GRADIENTS | MAX_GRADIENTS, false, false); clear_refPixelWasGood(); buildMutex.unlock(); activeMutex.unlock(); return true; } return false; } // 类参数变量初始化 主要初始化 图像金字塔的 内参数 内参数逆 void Frame::initialize(int id, int width, int height, const Eigen::Matrix3f& K, double timestamp) { data.id = id;// id pose = new FramePoseStruct(this);// 帧位姿 // 相机内参 data.K[0] = K; data.fx[0] = K(0,0); data.fy[0] = K(1,1); data.cx[0] = K(0,2); data.cy[0] = K(1,2); // 相机内参数逆 data.KInv[0] = K.inverse();// 逆矩阵 data.fxInv[0] = data.KInv[0](0,0); data.fyInv[0] = data.KInv[0](1,1); data.cxInv[0] = data.KInv[0](0,2); data.cyInv[0] = data.KInv[0](1,2); data.timestamp = timestamp;// 时间戳 data.hasIDepthBeenSet = false; depthHasBeenUpdatedFlag = false; referenceID = -1; referenceLevel = -1; numMappablePixels = -1; /// 初始化金字塔 for (int level = 0; level < PYRAMID_LEVELS; ++ level) { data.width[level] = width >> level;// 右移位 相等于 除以2 data.height[level] = height >> level; data.imageValid[level] = false; data.gradientsValid[level] = false; data.maxGradientsValid[level] = false; data.idepthValid[level] = false; data.idepthVarValid[level] = false; data.image[level] = 0; data.gradients[level] = 0; data.maxGradients[level] = 0; data.idepth[level] = 0; data.idepthVar[level] = 0; data.reActivationDataValid = false; // data.refIDValid[level] = false; // 初始化相机金字塔 if (level > 0) { data.fx[level] = data.fx[level-1] * 0.5;// 相应的 内参数 是 上一层的一般 大小变小了 变为原来的1/2 data.fy[level] = data.fy[level-1] * 0.5; data.cx[level] = (data.cx[0] + 0.5) / ((int)1<<level) - 0.5;// 也是 变为 1/2 data.cy[level] = (data.cy[0] + 0.5) / ((int)1<<level) - 0.5; data.K[level] << data.fx[level], 0.0, data.cx[level], 0.0, data.fy[level], data.cy[level], 0.0, 0.0, 1.0; // synthetic data.KInv[level] = (data.K[level]).inverse(); data.fxInv[level] = data.KInv[level](0,0); data.fyInv[level] = data.KInv[level](1,1); data.cxInv[level] = data.KInv[level](0,2); data.cyInv[level] = data.KInv[level](1,2); } } data.validity_reAct = 0; data.idepthVar_reAct = 0; data.idepth_reAct = 0; data.refPixelWasGood = 0; permaRefNumPts = 0; permaRef_colorAndVarData = 0; permaRef_posData = 0; meanIdepth = 1; numPoints = 0; numFramesTrackedOnThis = numMappedOnThis = numMappedOnThisTotal = 0; idxInKeyframes = -1; edgeErrorSum = edgesNum = 1; lastConstraintTrackedCamToWorld = Sim3(); isActive = false; } void Frame::setDepth_Allocate() { return; } // 创建对应层图像金字塔的 图像 // 递归构建底层金字塔,因为上层金字塔是以底层为基础的 // 分配内存 从上一层 隔行隔列下采样 方案是下层金字塔的四个像素的值的平均值合并成一个 // 建立每一层 图像金字塔 void Frame::buildImage(int level) { if (level == 0) { printf("Frame::buildImage(0): Loading image from disk is not implemented yet! No-op.\n"); return; } require(IMAGE, level - 1);// 首先是递归构建底层金字塔,因为上层金字塔是以底层为基础的 // 递归到底层后,调用buildMutex互斥锁, boost::unique_lock<boost::mutex> lock2(buildMutex);// 创建线程锁 上锁 if(data.imageValid[level]) return; // 打印调试信息 if(enablePrintDebugInfo && printFrameBuildDebugInfo) printf("CREATE Image lvl %d for frame %d\n", level, id()); // 当前层的 图像宽 高 int width = data.width[level - 1];// 0,1,2,3,4 int height = data.height[level - 1]; const float* source = data.image[level - 1];// 上一层的图像 // 之后才是判断这个等级的金字塔是否已经构建,然后向内存管理的对象申请内存,之后构建整个金字塔 if (data.image[level] == 0)// 指针为空 当前层 未分配存储空间 // 分配 存储空间 data.image[level] = FrameMemory::getInstance().getFloatBuffer(data.width[level] * data.height[level]); float* dest = data.image[level];// 目标地址 #if defined(ENABLE_SSE) // I assume all all subsampled width's are a multiple of 8. // if this is not the case, this still works except for the last * pixel, which will produce a segfault. // in that case, reduce this loop and calculate the last 0-3 dest pixels by hand.... if (width % 8 == 0) { __m128 p025 = _mm_setr_ps(0.25f,0.25f,0.25f,0.25f); const float* maxY = source+width*height; for(const float* y = source; y < maxY; y+=width*2) { const float* maxX = y+width; for(const float* x=y; x < maxX; x += 8) { // i am calculating four dest pixels at a time. __m128 top_left = _mm_load_ps((float*)x); __m128 bot_left = _mm_load_ps((float*)x+width); __m128 left = _mm_add_ps(top_left,bot_left); __m128 top_right = _mm_load_ps((float*)x+4); __m128 bot_right = _mm_load_ps((float*)x+width+4); __m128 right = _mm_add_ps(top_right,bot_right); __m128 sumA = _mm_shuffle_ps(left,right, _MM_SHUFFLE(2,0,2,0)); __m128 sumB = _mm_shuffle_ps(left,right, _MM_SHUFFLE(3,1,3,1)); __m128 sum = _mm_add_ps(sumA,sumB); sum = _mm_mul_ps(sum,p025); _mm_store_ps(dest, sum); dest += 4; } } data.imageValid[level] = true; return; } #elif defined(ENABLE_NEON) // I assume all all subsampled width's are a multiple of 8. // if this is not the case, this still works except for the last * pixel, which will produce a segfault. // in that case, reduce this loop and calculate the last 0-3 dest pixels by hand.... if (width % 8 == 0) { static const float p025[] = {0.25, 0.25, 0.25, 0.25}; int width_iteration_count = width / 8; int height_iteration_count = height / 2; const float* cur_px = source; const float* next_row_px = source + width; __asm__ __volatile__ ( "vldmia %[p025], {q10} \n\t" // p025(q10) ".height_loop: \n\t" "mov r5, %[width_iteration_count] \n\t" // store width_iteration_count ".width_loop: \n\t" "vldmia %[cur_px]!, {q0-q1} \n\t" // top_left(q0), top_right(q1) "vldmia %[next_row_px]!, {q2-q3} \n\t" // bottom_left(q2), bottom_right(q3) "vadd.f32 q0, q0, q2 \n\t" // left(q0) "vadd.f32 q1, q1, q3 \n\t" // right(q1) "vpadd.f32 d0, d0, d1 \n\t" // pairwise add into sum(q0) "vpadd.f32 d1, d2, d3 \n\t" "vmul.f32 q0, q0, q10 \n\t" // multiply with 0.25 to get average "vstmia %[dest]!, {q0} \n\t" "subs %[width_iteration_count], %[width_iteration_count], #1 \n\t" "bne .width_loop \n\t" "mov %[width_iteration_count], r5 \n\t" // restore width_iteration_count // Advance one more line "add %[cur_px], %[cur_px], %[rowSize] \n\t" "add %[next_row_px], %[next_row_px], %[rowSize] \n\t" "subs %[height_iteration_count], %[height_iteration_count], #1 \n\t" "bne .height_loop \n\t" : /* outputs */ [cur_px]"+&r"(cur_px), [next_row_px]"+&r"(next_row_px), [width_iteration_count]"+&r"(width_iteration_count), [height_iteration_count]"+&r"(height_iteration_count), [dest]"+&r"(dest) : /* inputs */ [p025]"r"(p025), [rowSize]"r"(width * sizeof(float)) : /* clobber */ "memory", "cc", "r5", "q0", "q1", "q2", "q3", "q10" ); data.imageValid[level] = true; return; } #endif int wh = width*height;// 当前层 像素总数 const float* s; for(int y=0; y<wh; y += width*2) { for(int x=0; x<width; x+= 2)// 从上一层 隔行隔列下采样 { s = source + x + y;// 上一层 像素对应位置 *dest = (s[0] + s[1] + s[width] + s[1+width]) * 0.25f;// 四个像素的值的平均值合并成一个 dest++; } } data.imageValid[level] = true;// 该层 图像已经构建 } // 释放图像 void Frame::releaseImage(int level) { if (level == 0) { printf("Frame::releaseImage(0): Storing image on disk is not supported yet! No-op.\n"); return; } FrameMemory::getInstance().returnBuffer(data.image[level]); data.image[level] = 0;// 指针为0 } // 根据 图像金字塔 构建梯度金字塔 xy方向梯度 是 中心差分 void Frame::buildGradients(int level) { // 梯度图像需要 同一层的 图像 require(IMAGE, level); boost::unique_lock<boost::mutex> lock2(buildMutex); if(data.gradientsValid[level])// 空指针 为分配内存 返回 return; // 打印调试信息 if(enablePrintDebugInfo && printFrameBuildDebugInfo) printf("CREATE Gradients lvl %d for frame %d\n", level, id()); // 当前层图像的宽度和高度 int width = data.width[level]; int height = data.height[level]; // 存储指针为空,则 申请内存空间 if(data.gradients[level] == 0) // x 方向梯度 y方向梯度 当前点像素值 第四维度没有存储数据 gradxyii_pt data.gradients[level] = (Eigen::Vector4f*)FrameMemory::getInstance().getBuffer(sizeof(Eigen::Vector4f) * width * height); const float* img_pt = data.image[level] + width;// 第二行开始 的 图像值 应为要计算y方向梯度 const float* img_pt_max = data.image[level] + width*(height-1);// 图像 最大的指针地址 Eigen::Vector4f* gradxyii_pt = data.gradients[level] + width;// 梯度值对应的 空间指针 // in each iteration i need -1,0,p1,mw,pw float val_m1 = *(img_pt-1);// 是左右两个像素点的梯度 float val_00 = *img_pt;// 当前中心点 float val_p1;// 是中心差分 // *(img_pt-width) // val_m1 *(img_pt) val_p1 // *(img_pt+width) // (val_p1 - val_m1)/2 = x 方向梯度 // 0.5f*(*(img_pt+width) - *(img_pt-width)) = y方向梯度 // val_00 = *(img_pt) 当前 点像素值 // 第四维度 没有存储数据 gradxyii_pt Eigen::Vector4f for(; img_pt < img_pt_max; img_pt++, gradxyii_pt++) { val_p1 = *(img_pt+1); *((float*)gradxyii_pt) = 0.5f*(val_p1 - val_m1);// 是左右两个像素点的梯度 x方向梯度 *(((float*)gradxyii_pt)+1) = 0.5f*(*(img_pt+width) - *(img_pt-width));// +/- 一行的长度就得到 上下的坐标 y方向梯度 *(((float*)gradxyii_pt)+2) = val_00;// 像素值 val_m1 = val_00;// 迭代 val_00 = val_p1; } data.gradientsValid[level] = true; } // 释放 梯度 金字塔 void Frame::releaseGradients(int level) { FrameMemory::getInstance().returnBuffer(reinterpret_cast<float*>(data.gradients[level])); data.gradients[level] = 0; } // 创建 梯度图内 临近四点中梯度最大值 的 最大值梯度 图 , 并记录梯度值较大的可以映射 成 地图点的数量 // 在梯度图中 求的 上中下 三个梯度值中的最大值,形成临时梯度最大值图 // 在临时梯度最大值图 中求 的 左中右 三个梯度值中的最大值,形成最后的 最大梯度值地图 // 并记录 最大梯度大小超过阈值的点 可以映射成地图点 void Frame::buildMaxGradients(int level) { // 需要同一层级的 梯度图 require(GRADIENTS, level); boost::unique_lock<boost::mutex> lock2(buildMutex); // 已经 得到过了,就不要计算了 if(data.maxGradientsValid[level]) return; // 打印调试信息 if(enablePrintDebugInfo && printFrameBuildDebugInfo) printf("CREATE AbsGrad lvl %d for frame %d\n", level, id()); // 当前层级的 宽度和高度 int width = data.width[level]; int height = data.height[level]; // 未申请内存则申请内存 if (data.maxGradients[level] == 0) data.maxGradients[level] = FrameMemory::getInstance().getFloatBuffer(width * height); // 临时内存地址 float* maxGradTemp = FrameMemory::getInstance().getFloatBuffer(width * height); // 1. 秋去合成梯度大小 sqrt(dx^2 + dy^2) write abs gradients in real data. Eigen::Vector4f* gradxyii_pt = data.gradients[level] + width;// 梯度 第二行开始的 地址 , x梯度 y梯度 像素值 空 float* maxgrad_pt = data.maxGradients[level] + width;// 对应最大梯度值 存储起始地址 float* maxgrad_pt_max = data.maxGradients[level] + width*(height-1);// 对应最大梯度值 最大存储地址 for(; maxgrad_pt < maxgrad_pt_max; maxgrad_pt++, gradxyii_pt++) { float dx = *((float*)gradxyii_pt);// x 方向梯度 float dy = *(1+(float*)gradxyii_pt);// y方向梯度 *maxgrad_pt = sqrtf(dx*dx + dy*dy);// 和成梯度 } // 2. 求每个梯度值上中下三个位置中的最大值形成的 临时最大梯度图 // smear up/down direction into temp buffer maxgrad_pt = data.maxGradients[level] + width+1;//第二行 第二个 开始 位置 maxgrad_pt_max = data.maxGradients[level] + width*(height-1)-1;// 最大位置 float* maxgrad_t_pt = maxGradTemp + width+1;// 对应位置的 临时变量地址 for(;maxgrad_pt<maxgrad_pt_max; maxgrad_pt++, maxgrad_t_pt++) { float g1 = maxgrad_pt[-width];// 上方位置的 合成梯度 float g2 = maxgrad_pt[0];// 当前点 合成梯度 if(g1 < g2) g1 = g2;// 保留g1 和 g2 中的最大值 float g3 = maxgrad_pt[width];// 下方位置的 合成梯度 if(g1 < g3) *maxgrad_t_pt = g3; // 临时最大值 else *maxgrad_t_pt = g1;// 临时最大值 } float numMappablePixels = 0; // 3. smear left/right direction into real data maxgrad_pt = data.maxGradients[level] + width+1;//第二行 第二个 开始 位置 maxgrad_pt_max = data.maxGradients[level] + width*(height-1)-1;// 最大位置 maxgrad_t_pt = maxGradTemp + width+1;// 对应位置的 临时变量地址 for(;maxgrad_pt<maxgrad_pt_max; maxgrad_pt++, maxgrad_t_pt++) { float g1 = maxgrad_t_pt[-1];// 左边 float g2 = maxgrad_t_pt[0];// 中间 if(g1 < g2) g1 = g2;// 保留g1 和 g2 中的最大值 float g3 = maxgrad_t_pt[1];// 右边 if(g1 < g3) { *maxgrad_pt = g3;// 最大值 if(g3 >= MIN_ABS_GRAD_CREATE) numMappablePixels++;// 梯度大小超过阈值的 计数 } else { *maxgrad_pt = g1; if(g1 >= MIN_ABS_GRAD_CREATE) numMappablePixels++;// 梯度大小超过阈值的 计数 } } if(level==0) this->numMappablePixels = numMappablePixels;// 梯度值较大的点可以 求的 地图点 FrameMemory::getInstance().returnBuffer(maxGradTemp);// 删除临时内存 data.maxGradientsValid[level] = true;// 得到周围四点 最大梯度值 地图 } // 深度 最大梯度值地图 void Frame::releaseMaxGradients(int level) { FrameMemory::getInstance().returnBuffer(data.maxGradients[level]); data.maxGradients[level] = 0; } // 根据最初逆深度均值图 构建 逆深度均值图 方差图(高斯分布) 金字塔 // current -----> 右边一个 // 下边 下右边 上一层四个位置处的和 // 上一层 逆方差和 / 上一层 逆深度均值 (四个位置处) 和 得到深度信息 再 取逆得到 逆深度均值 // 上一层 逆深度 方差和 取逆得到 本层 逆深度方差 void Frame::buildIDepthAndIDepthVar(int level) { if (! data.hasIDepthBeenSet) { printfAssert("Frame::buildIDepthAndIDepthVar(): idepth has not been set yet!\n"); return; } if (level == 0) { printf("Frame::buildIDepthAndIDepthVar(0): Loading depth from disk is not implemented yet! No-op.\n"); return; } // 递归构建 从最开始的 逆深度图开始构建 require(IDEPTH, level - 1);// 需要上一层的逆深度图 boost::unique_lock<boost::mutex> lock2(buildMutex); if(data.idepthValid[level] && data.idepthVarValid[level])// 该层 逆深度均值 和方差 已经创建 就返回 return; // 打印调试信息 if(enablePrintDebugInfo && printFrameBuildDebugInfo) printf("CREATE IDepth lvl %d for frame %d\n", level, id()); // 当前层的 宽度和高度 int width = data.width[level]; int height = data.height[level]; // 逆深度均值和 逆深度方差图 内存是否分配 // 若未分配内存,则分配内存 if (data.idepth[level] == 0) data.idepth[level] = FrameMemory::getInstance().getFloatBuffer(width * height); if (data.idepthVar[level] == 0) data.idepthVar[level] = FrameMemory::getInstance().getFloatBuffer(width * height); // 上一层 图像宽度 int sw = data.width[level - 1]; const float* idepthSource = data.idepth[level - 1];// 上一层 逆深度均值 指针 const float* idepthVarSource = data.idepthVar[level - 1];// 上一层 逆深度方差 指针 float* idepthDest = data.idepth[level];// 当前层 逆深度均值 指针 float* idepthVarDest = data.idepthVar[level];// 当前层 逆深度方差 指针 // // current -----> 右边一个 // 下边 下右边 上一层四个位置处的和 // for(int y=0;y<height;y++)// 每一行 { for(int x=0;x<width;x++)// 每一列 { int idx = 2*(x+y*sw);// 在上一层中的偏移量×2 因为上一层的尺寸是当前层的 2倍 int idxDest = (x+y*width);// 当前层的 偏移量 float idepthSumsSum = 0;// 逆深度均值 之和 float ivarSumsSum = 0;// 逆深度方差 的逆之 和 int num=0; // build sums float ivar;// 逆方差 float var = idepthVarSource[idx];// 上一层 逆深度方差 if(var > 0) { ivar = 1.0f / var;// 逆方差 ivarSumsSum += ivar;// 逆方差 之和 idepthSumsSum += ivar * idepthSource[idx];// 加权 逆深度之和 以逆方差为权重 num++; } var = idepthVarSource[idx+1];// 右边一个 if(var > 0) { ivar = 1.0f / var; ivarSumsSum += ivar; idepthSumsSum += ivar * idepthSource[idx+1]; num++; } var = idepthVarSource[idx+sw];// 下边的一个 if(var > 0) { ivar = 1.0f / var; ivarSumsSum += ivar; idepthSumsSum += ivar * idepthSource[idx+sw]; num++; } var = idepthVarSource[idx+sw+1];// 下右边的一个 if(var > 0) { ivar = 1.0f / var; ivarSumsSum += ivar; idepthSumsSum += ivar * idepthSource[idx+sw+1]; num++; } if(num > 0) { float depth = ivarSumsSum / idepthSumsSum;// 上一层 逆方差和 / 上一层 逆深度和 得到深度信息 idepthDest[idxDest] = 1.0f / depth;// 深度取逆 得到 逆深度 idepthVarDest[idxDest] = num / ivarSumsSum;// 上一层 逆深度 方差和 取逆得到 本层 逆深度方法 } else { idepthDest[idxDest] = -1; idepthVarDest[idxDest] = -1; } } } data.idepthValid[level] = true; data.idepthVarValid[level] = true; } // 释放 逆深度均值图 层 void Frame::releaseIDepth(int level) { if (level == 0) { printf("Frame::releaseIDepth(0): Storing depth on disk is not supported yet! No-op.\n"); return; } FrameMemory::getInstance().returnBuffer(data.idepth[level]); data.idepth[level] = 0; } // 释放逆深度 方差 图 层 void Frame::releaseIDepthVar(int level) { if (level == 0) { printf("Frame::releaseIDepthVar(0): Storing depth variance on disk is not supported yet! No-op.\n"); return; } FrameMemory::getInstance().returnBuffer(data.idepthVar[level]); data.idepthVar[level] = 0; } void Frame::printfAssert(const char* message) const { assert(!message); printf("%s\n", message); } }