數據結構
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);
}
}