Program Listing for File Frame.cpp¶
↰ Return to documentation for file (lib/DataStructures/Frame.cpp)
#include "DataStructures/Frame.h"
#include "DataStructures/FrameMemory.h"
#include "DataStructures/KeyFrame.h"
#include "DepthEstimation/DepthMapPixelHypothesis.h"
#include "Tracking/TrackingReference.h"
#include "DataStructures/FramePoseStruct.h"
#include "DepthEstimation/DepthMap.h"
#include <g3log/g3log.hpp>
namespace lsd_slam
{
int privateFrameAllocCount = 0;
Frame::Frame(int frameId, const Camera &cam, const ImageSize &sz,
double timestamp, const unsigned char* image )
: _trackingParent( nullptr ),
_id( frameId ),
_timestamp( timestamp ),
data( cam, sz, image ),
pose( new FramePoseStruct(*this) ),
referenceID ( -1 ),
referenceLevel( -1 ),
numMappablePixels( -1 ),
meanIdepth( 1 ),
numPoints( 0 ),
idxInKeyframes( -1 ),
edgeErrorSum( 1 ),
edgesNum( 1 ),
lastConstraintTrackedCamToWorld( Sim3() ),
isActive( false )
{
privateFrameAllocCount++;
LOG_IF(INFO, Conf().print.memoryDebugInfo )
<< "ALLOCATED frame " << id()
<< ", now there are " << privateFrameAllocCount;
}
Frame::Frame(int frameId, const Camera &cam, const ImageSize &sz,
double timestamp, const float* image )
: _trackingParent( nullptr ),
_id( frameId ),
_timestamp( timestamp ),
data( cam, sz, image ),
pose( new FramePoseStruct(*this) ),
referenceID ( -1 ),
referenceLevel( -1 ),
numMappablePixels( -1 ),
meanIdepth( 1 ),
numPoints( 0 ),
idxInKeyframes( -1 ),
edgeErrorSum( 1 ),
edgesNum( 1 ),
lastConstraintTrackedCamToWorld( Sim3() ),
isActive( false )
{
privateFrameAllocCount++;
LOG_IF(INFO, Conf().print.memoryDebugInfo )
<< "ALLOCATED frame " << id()
<< ", now there are " << privateFrameAllocCount;
}
Frame::~Frame()
{
LOGF_IF(INFO, Conf().print.frameBuildDebugInfo,"DELETING frame %d", id());
FrameMemory::getInstance().deactivateFrame(this);
// if(!pose->isRegisteredToGraph)
// delete pose;
// // else
// // pose->frame = 0;
// if(permaRef_colorAndVarData != 0)
// delete permaRef_colorAndVarData;
// if(permaRef_posData != 0)
// delete permaRef_posData;
privateFrameAllocCount--;
LOGF_IF(DEBUG, Conf().print.frameBuildDebugInfo, "DELETED frame %d, now there are %d\n", id(), privateFrameAllocCount);
}
bool Frame::isTrackingParent( const std::shared_ptr<Frame> &other ) const
{ return isTrackingParent( other->id() ); }
bool Frame::isTrackingParent( const std::shared_ptr<KeyFrame> &other ) const
{ return isTrackingParent( other->id() ); }
bool Frame::isTrackingParent( int id ) const
{ return hasTrackingParent() && ( id == trackingParent()->id() ); }
void Frame::calculateMeanInformation()
{
return;
if(numMappablePixels < 0)
maxGradients(0);
const float* idv = idepthVar(0);
const float* idv_max = idv + area(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;
}
void Frame::setDepth(const DepthMap::SharedPtr &depthMap ) //PixelHypothesis* newDepth)
{
boost::shared_lock<boost::shared_mutex> lock = getActiveLock();
boost::unique_lock<boost::mutex> lock2(buildMutex);
if(data.idepth[0] == 0)
data.idepth[0] = FrameMemory::getInstance().getFloatBuffer(area(0));
if(data.idepthVar[0] == 0)
data.idepthVar[0] = FrameMemory::getInstance().getFloatBuffer(area(0));
float* pyrIDepth = data.idepth[0];
float* pyrIDepthVar = data.idepthVar[0];
float* pyrIDepthMax = pyrIDepth + (area(0));
DepthMapPixelHypothesis *newDepth = depthMap->hypothesisAt(0,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;
}
else
{
*pyrIDepth = -1;
*pyrIDepthVar = -1;
}
}
meanIdepth = sumIdepth / numIdepth;
numPoints = numIdepth;
LOG(INFO) << "Imported " << numPoints << " points from DepthMap";
data.idepthValid[0] = true;
data.idepthVarValid[0] = true;
release(IDEPTH | IDEPTH_VAR, true, true);
data.hasIDepthBeenSet = true;
//depthHasBeenUpdatedFlag = true;
}
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);
if(data.idepth[0] == 0)
data.idepth[0] = FrameMemory::getInstance().getFloatBuffer(area(0));
if(data.idepthVar[0] == 0)
data.idepthVar[0] = FrameMemory::getInstance().getFloatBuffer(area(0));
float* pyrIDepth = data.idepth[0];
float* pyrIDepthVar = data.idepthVar[0];
const int width0 = width(0);
const int height0 = 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;
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);
data.hasIDepthBeenSet = true;
}
void Frame::prepareForStereoWith(const Frame::SharedPtr &other, Sim3 thisToOther, const int level)
{
Sim3 otherToThis = thisToOther.inverse();
//otherToThis = data.worldToCam * other->data.camToWorld;
K_otherToThis_R = other->camera(level).K * otherToThis.rotationMatrix().cast<float>() * otherToThis.scale();
otherToThis_t = otherToThis.translation().cast<float>();
K_otherToThis_t = other->camera(level).K * otherToThis_t;
thisToOther_t = thisToOther.translation().cast<float>();
K_thisToOther_t = camera(level).K * thisToOther_t;
thisToOther_R = thisToOther.rotationMatrix().cast<float>() * thisToOther.scale();
otherToThis_R_row0 = thisToOther_R.col(0);
otherToThis_R_row1 = thisToOther_R.col(1);
otherToThis_R_row2 = thisToOther_R.col(2);
distSquared = otherToThis.translation().dot(otherToThis.translation());
referenceID = other->id();
referenceLevel = level;
}
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();
LOGF_IF(DEBUG, Conf().print.frameBuildDebugInfo, "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::setDepth_Allocate()
{
return;
}
void Frame::buildImage(int level)
{
if (level == 0)
{
LOG(WARNING) << "Frame::buildImage(0): Loading image from disk is not implemented yet! No-op.";
return;
}
require(IMAGE, level - 1);
boost::unique_lock<boost::mutex> lock2(buildMutex);
if(data.imageValid[level]) return;
const int width = imgSize(level-1).width;
const int height = imgSize(level-1).height;
const float* source = data.image[level - 1];
LOGF_IF(DEBUG, Conf().print.frameBuildDebugInfo,"CREATE Image lvl %d for frame %d, %f x %f", level, id(), width/2.0, height/2.0);
if (data.image[level] == 0)
data.image[level] = FrameMemory::getInstance().getFloatBuffer(area(level));
float* dest = data.image[level];
#if defined(FOOBAR)
#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
#endif
// Width and height are valid for level _above_ this one
CHECK( width % 2 == 0 );
CHECK( height % 2 == 0 );
int wh = width*height;
const float *s;
for(int y=0 ; y<height ; y+=2)
{
for(int x=0 ; x<width ; x+=2)
{
s = source + x + y*width;
*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)
{
LOG(WARNING) << "Frame::releaseImage(0): Storing image on disk is not supported yet! No-op.";
return;
}
FrameMemory::getInstance().returnBuffer(data.image[level]);
data.image[level] = 0;
}
void Frame::buildGradients(int level)
{
require(IMAGE, level);
boost::unique_lock<boost::mutex> lock2(buildMutex);
if(data.gradientsValid[level])
return;
LOGF_IF(DEBUG, Conf().print.frameBuildDebugInfo,"CREATE Gradients lvl %d for frame %d", level, id());
const int width = imgSize(level).width;
const int height = imgSize(level).height;
if(data.gradients[level] == 0)
data.gradients[level] = (Eigen::Vector4f*)FrameMemory::getInstance().getBuffer(sizeof(Eigen::Vector4f) * width * height);
const float* img_pt = data.image[level] + width;
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;
for(; img_pt < img_pt_max; img_pt++, gradxyii_pt++)
{
val_p1 = *(img_pt+1);
*((float*)gradxyii_pt) = 0.5f*(val_p1 - val_m1);
*(((float*)gradxyii_pt)+1) = 0.5f*(*(img_pt+width) - *(img_pt-width));
*(((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;
LOGF_IF(DEBUG, Conf().print.frameBuildDebugInfo,"CREATE AbsGrad lvl %d for frame %d", level, id());
const int width = imgSize(level).width;
const int height = imgSize(level).height;
if (data.maxGradients[level] == 0)
data.maxGradients[level] = FrameMemory::getInstance().getFloatBuffer(width * height);
float* maxGradTemp = FrameMemory::getInstance().getFloatBuffer(width * height);
// 1. write abs gradients in real data.
Eigen::Vector4f* gradxyii_pt = data.gradients[level] + width;
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);
float dy = *(1+(float*)gradxyii_pt);
*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;
float g3 = maxgrad_pt[width];
if(g1 < g3)
*maxgrad_t_pt = g3;
else
*maxgrad_t_pt = g1;
}
float numMappablePixels = 0;
// 2. 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;
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;
}
void Frame::buildIDepthAndIDepthVar(int level)
{
if (! hasIDepthBeenSet())
{
LOG(WARNING) << "Frame::buildIDepthAndIDepthVar(): idepth has not been set yet!";
return;
}
if (level == 0)
{
LOG(DEBUG) << "Frame::buildIDepthAndIDepthVar(0): Loading depth from disk is not implemented yet! No-op.";
return;
}
require(IDEPTH, level - 1);
boost::unique_lock<boost::mutex> lock2(buildMutex);
if(data.idepthValid[level] && data.idepthVarValid[level])
return;
LOGF_IF(DEBUG, Conf().print.frameBuildDebugInfo,"CREATE IDepth lvl %d for frame %d", level, id());
const int width = imgSize(level).width;
const int height = imgSize(level).height;
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 = imgSize(level - 1).width;
const float* idepthSource = data.idepth[level - 1];
const float* idepthVarSource = data.idepthVar[level - 1];
float* idepthDest = data.idepth[level];
float* idepthVarDest = data.idepthVar[level];
for(int y=0;y<height;y++)
{
for(int x=0;x<width;x++)
{
int idx = 2*(x+y*sw);
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)
{
LOG(WARNING) << "Frame::releaseIDepth(0): Storing depth on disk is not supported yet! No-op.";
return;
}
FrameMemory::getInstance().returnBuffer(data.idepth[level]);
data.idepth[level] = 0;
}
void Frame::releaseIDepthVar(int level)
{
if (level == 0)
{
LOG(WARNING) << "Frame::releaseIDepthVar(0): Storing depth variance on disk is not supported yet! No-op.";
return;
}
FrameMemory::getInstance().returnBuffer(data.idepthVar[level]);
data.idepthVar[level] = 0;
}
//====================
}