Randomized Ferns 在 ElasticFusion 中地位和作用
ElasticFusion 通過 Randomized Ferns 重定位和迴環檢測,Randomized ferns 是實時的重定位和迴環檢測算法。
Randomized Ferns 對關鍵幀圖像編碼,採用特殊的編碼存儲方式,加快圖像比較的效率。
算法流程圖
編碼流程
如上圖,每幅圖像的編碼由
每個 block 由
每個 Fern 通過比較
如上圖所示,每個 block 有一個像素位置處,4 通道編碼的 Ferns 組成,白色的叉號表示的是選擇的像素點的位置,4 個圖像表示 RGB-D 的 4 個通道,每個 block 包含 4 個 Ferns,對應
其中,每個 Fern 像素點的位置
編碼存儲結構
算法對於關鍵幀的編碼按照列表的形式存儲,如上圖右側所示,表中的每列表示 block 編碼的一種可能情況,如果 block 採用 n 個 Ferns 確定對 block 的編碼,那麼表有
對於待編碼的關鍵幀圖像,計算每個 block 的編碼,根據 block 的編碼,將圖像的編號存儲到列表中。
圖像搜索
對於一幅新獲取的圖像
對於圖像
通過相似度判別當前幀是否加入做關鍵幀(對於所有存儲的關鍵幀相似度小於一定閾值),或者存在迴環(和某一幀圖像相似度大於一定閾值),還可以通過和相似度大的圖像配準,進行重定位。
代碼解析
// 隨機初始化 Ferns 的位置,像素通道和閾值大小
void Ferns::generateFerns()
{
for(int i = 0; i < num; i++)
{
Fern f;
//隨機初始化 Fern 的位置
f.pos(0) = widthDist(random);
f.pos(1) = heightDist(random);
// 隨機初始化每個通道的閾值
f.rgbd(0) = rgbDist(random);
f.rgbd(1) = rgbDist(random);
f.rgbd(2) = rgbDist(random);
f.rgbd(3) = dDist(random);
conservatory.push_back(f);
}
}
//新輸入的幀和以前幀進行比較,如果相似度小於一定閾值,則將當前幀插入作爲迴環檢測的關鍵幀
bool Ferns::addFrame(GPUTexture * imageTexture, GPUTexture * vertexTexture, GPUTexture * normalTexture, const Eigen::Matrix4f & pose, int srcTime, const float threshold)
{
Img<Eigen::Matrix<unsigned char, 3, 1>> img(height, width);
Img<Eigen::Vector4f> verts(height, width);
Img<Eigen::Vector4f> norms(height, width);
resize.image(imageTexture, img);
resize.vertex(vertexTexture, verts);
resize.vertex(normalTexture, norms);
Frame * frame = new Frame(num,
frames.size(),
pose,
srcTime,
width * height,
(unsigned char *)img.data,
(Eigen::Vector4f *)verts.data,
(Eigen::Vector4f *)norms.data);
int * coOccurrences = new int[frames.size()];
memset(coOccurrences, 0, sizeof(int) * frames.size());
for(int i = 0; i < num; i++)
{
unsigned char code = badCode;
if(verts.at<Eigen::Vector4f>(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2) > 0)
{
const Eigen::Matrix<unsigned char, 3, 1> & pix = img.at<Eigen::Matrix<unsigned char, 3, 1>>(conservatory.at(i).pos(1), conservatory.at(i).pos(0));
//隨機選取像素點處的編碼
code = (pix(0) > conservatory.at(i).rgbd(0)) << 3 |
(pix(1) > conservatory.at(i).rgbd(1)) << 2 |
(pix(2) > conservatory.at(i).rgbd(2)) << 1 |
(int(verts.at<Eigen::Vector4f>(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2) * 1000.0f) > conservatory.at(i).rgbd(3));
frame->goodCodes++;
// 計算和以前存儲幀之間的相似度
for(size_t j = 0; j < conservatory.at(i).ids[code].size(); j++)
{
coOccurrences[conservatory.at(i).ids[code].at(j)]++;
}
}
frame->codes[i] = code;
}
float minimum = std::numeric_limits<float>::max();
if(frame->goodCodes > 0)
{
for(size_t i = 0; i < frames.size(); i++)
{
float maxCo = std::min(frame->goodCodes, frames.at(i)->goodCodes);
float dissim = (float)(maxCo - coOccurrences[i]) / (float)maxCo;
if(dissim < minimum)
{
minimum = dissim;
}
}
}
delete [] coOccurrences;
if((minimum > threshold || frames.size() == 0) && frame->goodCodes > 0)
{
for(int i = 0; i < num; i++)
{
if(frame->codes[i] != badCode)
{
//conservatory 存儲關係:第一層 fern 的編號,第二層 code 的編號,第三層圖像幀的編號
conservatory.at(i).ids[frame->codes[i]].push_back(frame->id);
}
}
frames.push_back(frame);
return true;
}
else
{
delete frame;
return false;
}
}
// 對於輸入的圖像進行全局的迴環檢測,通過判斷和以前存儲的幀編碼相似度,判斷當前幀是否作爲關鍵幀
Eigen::Matrix4f Ferns::findFrame(std::vector<SurfaceConstraint> & constraints,
const Eigen::Matrix4f & currPose,
GPUTexture * vertexTexture,
GPUTexture * normalTexture,
GPUTexture * imageTexture,
const int time,
const bool lost)
{
lastClosest = -1;
Img<Eigen::Matrix<unsigned char, 3, 1>> imgSmall(height, width);
Img<Eigen::Vector4f> vertSmall(height, width);
Img<Eigen::Vector4f> normSmall(height, width);
//對輸入圖像降採樣 8X8 倍
resize.image(imageTexture, imgSmall);
resize.vertex(vertexTexture, vertSmall);
resize.vertex(normalTexture, normSmall);
Frame * frame = new Frame(num, 0, Eigen::Matrix4f::Identity(), 0, width * height);
int * coOccurrences = new int[frames.size()];
memset(coOccurrences, 0, sizeof(int) * frames.size());
for(int i = 0; i < num; i++)
{
unsigned char code = badCode;
if(vertSmall.at<Eigen::Vector4f>(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2) > 0)
{
const Eigen::Matrix<unsigned char, 3, 1> & pix = imgSmall.at<Eigen::Matrix<unsigned char, 3, 1>>(conservatory.at(i).pos(1), conservatory.at(i).pos(0));
//指定像素點處做編碼
code = (pix(0) > conservatory.at(i).rgbd(0)) << 3 |
(pix(1) > conservatory.at(i).rgbd(1)) << 2 |
(pix(2) > conservatory.at(i).rgbd(2)) << 1 |
(int(vertSmall.at<Eigen::Vector4f>(conservatory.at(i).pos(1), conservatory.at(i).pos(0))(2) * 1000.0f) > conservatory.at(i).rgbd(3));
frame->goodCodes++;
// conservatory.at(i) 表示第 i 個 fern 總共隨機採樣了 num 個
// conservatory.at(i).ids[code] 表示第 i 個 fern 編碼值爲 code 的 vector
// conservatory.at(i).ids[code].at(j) 值表示第 at(j) 幀圖像
for(size_t j = 0; j < conservatory.at(i).ids[code].size(); j++)
{
// coOccurrences[conservatory.at(i).ids[code].at(j)] 表示和第 conservatory.at(i).ids[code].at(j) 幀圖像的相似度
coOccurrences[conservatory.at(i).ids[code].at(j)]++;
}
}
frame->codes[i] = code;
}
float minimum = std::numeric_limits<float>::max();
int minId = -1;
//在所有幀中找相似度最小的幀
for(size_t i = 0; i < frames.size(); i++)
{
float maxCo = std::min(frame->goodCodes, frames.at(i)->goodCodes);
float dissim = (float)(maxCo - coOccurrences[i]) / (float)maxCo;
if(dissim < minimum && time - frames.at(i)->srcTime > 300)
{
minimum = dissim;
minId = i;
}
}
//根據圖像的編碼計算兩幀之間的相似度
float Ferns::blockHDAware(const Frame * f1, const Frame * f2)
{
int count = 0;
float val = 0;
for(int i = 0; i < num; i++)
{
if(f1->codes[i] != badCode && f2->codes[i] != badCode)
{
count++;
if(f1->codes[i] == f2->codes[i])
{
val += 1.0f;
}
}
}
return val / (float)count;
}