Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

RGB Pointcloud and Live Camera Tracking on Pointcloud Viewer #12

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open

RGB Pointcloud and Live Camera Tracking on Pointcloud Viewer #12

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
c733f38
Added RGB capability and Pointcloud publishing to topic
Sep 8, 2018
8bfc810
Added RGB capability and Pointcloud publishing to topic
Sep 8, 2018
8eb1e2f
Fixed a bug for outputting color RGB
Sep 9, 2018
fbb2880
Added an option to enable/disable live camera tracking and RGB pointc…
Sep 9, 2018
372d885
Minor code cleanup
Sep 9, 2018
edecad8
Minor code cleanup
Sep 9, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions lsd_slam_core/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,11 +111,11 @@ target_link_libraries(live_slam lsdslam ${catkin_LIBRARIES} ${G2O_LIBRARIES})


# build image node
add_executable(dataset src/main_on_images.cpp)
add_executable(dataset_slam src/main_on_images.cpp)
add_dependencies(lsdslam lsd_slam_viewer_generate_messages_cpp)
add_dependencies(live_slam lsd_slam_viewer_generate_messages_cpp)
add_dependencies(dataset lsd_slam_viewer_generate_messages_cpp)
target_link_libraries(dataset lsdslam ${catkin_LIBRARIES} ${G2O_LIBRARIES})
add_dependencies(dataset_slam lsd_slam_viewer_generate_messages_cpp)
target_link_libraries(dataset_slam lsdslam ${catkin_LIBRARIES} ${G2O_LIBRARIES})
target_link_libraries(live_slam lsdslam ${OpenCV_LIBRARIES})

# TODO add INSTALL
3 changes: 2 additions & 1 deletion lsd_slam_core/cfg/LSDParams.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,9 @@ gen.add("relocalizationTH", double_t, 0, "How good a relocalization-attempt has

gen.add("depthSmoothingFactor", double_t, 0, "How much to smooth the depth map. Larger -> Less Smoothing", 1, 0, 10)

gen.add("fullResetRequested", bool_t, 0, "Reset the LSD SLAM system.", True)



exit(gen.generate(PACKAGE, "Config", "LSDParams"))

exit(gen.generate(PACKAGE, "Config", "LSDParams"))
93 changes: 61 additions & 32 deletions lsd_slam_core/src/DataStructures/Frame.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,9 @@

/**
* This file is part of LSD-SLAM.
*
* Copyright 2013 Jakob Engel <engelj at in dot tum dot de> (Technical University of Munich)
* For more information see <http://vision.in.tum.de/lsdslam>
* For more information see <http://vision.in.tum.de/lsdslam>
*
* LSD-SLAM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
Expand All @@ -28,14 +29,35 @@ namespace lsd_slam

int privateFrameAllocCount = 0;


Frame::Frame(int id, int width, int height, const Eigen::Matrix3f& K, double timestamp, const unsigned char* image, const unsigned char* rgbImage)
{
initialize(id, width, height, K, timestamp);
data.image[0] = FrameMemory::getInstance().getFloatBuffer(data.width[0]*data.height[0]);
data.imageRGB[0] = FrameMemory::getInstance().getFloatBuffer(data.width[0]*data.height[0]*3);
float* maxPt = data.image[0] + data.width[0]*data.height[0];
float* maxPtRGB = data.imageRGB[0] + data.width[0]*data.height[0]*3;
for(float* pt = data.image[0]; pt < maxPt; pt++)
{
*pt = *image;
image++;
}
for(float* pt = data.imageRGB[0]; pt < maxPtRGB; pt++)
{
*pt = *rgbImage;
rgbImage++;
}
data.imageValid[0] = true;
privateFrameAllocCount++;
if(enablePrintDebugInfo && printMemoryDebugInfo)
printf("ALLOCATED frame %d, now there are %d\n", this->id(), privateFrameAllocCount);
}



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* maxPt = data.image[0] + data.width[0]*data.height[0];

Expand All @@ -56,7 +78,7 @@ Frame::Frame(int id, int width, int height, const Eigen::Matrix3f& K, double tim
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(data.image[0], image, data.width[0]*data.height[0] * sizeof(float));
data.imageValid[0] = true;
Expand All @@ -83,6 +105,7 @@ Frame::~Frame()
for (int level = 0; level < PYRAMID_LEVELS; ++ level)
{
FrameMemory::getInstance().returnBuffer(data.image[level]);
FrameMemory::getInstance().returnBuffer(data.imageRGB[level]);
FrameMemory::getInstance().returnBuffer(reinterpret_cast<float*>(data.gradients[level]));
FrameMemory::getInstance().returnBuffer(data.maxGradients[level]);
FrameMemory::getInstance().returnBuffer(data.idepth[level]);
Expand Down Expand Up @@ -210,7 +233,7 @@ void Frame::setDepth(const DepthMapPixelHypothesis* newDepth)
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;

Expand All @@ -230,7 +253,7 @@ void Frame::setDepth(const DepthMapPixelHypothesis* newDepth)
*pyrIDepthVar = -1;
}
}

meanIdepth = sumIdepth / numIdepth;
numPoints = numIdepth;

Expand Down Expand Up @@ -283,7 +306,7 @@ void Frame::setDepthFromGroundTruth(const float* depth, float cov_scale)
++ pyrIDepthVar;
}
}

data.idepthValid[0] = true;
data.idepthVarValid[0] = true;
// data.refIDValid[0] = true;
Expand Down Expand Up @@ -397,29 +420,29 @@ bool Frame::minimizeInMemory()
void Frame::initialize(int id, int width, int height, const Eigen::Matrix3f& K, double timestamp)
{
data.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)
Expand All @@ -434,14 +457,15 @@ void Frame::initialize(int id, int width, int height, const Eigen::Matrix3f& K,
data.idepthVarValid[level] = false;

data.image[level] = 0;
data.imageRGB[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;
Expand Down Expand Up @@ -495,7 +519,7 @@ void Frame::buildImage(int level)
printf("Frame::buildImage(0): Loading image from disk is not implemented yet! No-op.\n");
return;
}

require(IMAGE, level - 1);
boost::unique_lock<boost::mutex> lock2(buildMutex);

Expand Down Expand Up @@ -562,36 +586,36 @@ void Frame::buildImage(int level)
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"

Expand Down Expand Up @@ -638,6 +662,11 @@ void Frame::releaseImage(int level)
}
FrameMemory::getInstance().returnBuffer(data.image[level]);
data.image[level] = 0;
/* check using imageRGB and release */
if(data.image[level] != 0){
FrameMemory::getInstance().returnBuffer(data.imageRGB[level]);
data.imageRGB[level] = 0;
}
}

void Frame::buildGradients(int level)
Expand All @@ -658,7 +687,7 @@ void Frame::buildGradients(int level)
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;
Expand Down Expand Up @@ -701,7 +730,7 @@ void Frame::buildMaxGradients(int 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);


Expand Down Expand Up @@ -787,16 +816,16 @@ void Frame::buildIDepthAndIDepthVar(int level)

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)
Expand All @@ -808,7 +837,7 @@ void Frame::buildIDepthAndIDepthVar(int level)
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++)
Expand Down Expand Up @@ -857,7 +886,7 @@ void Frame::buildIDepthAndIDepthVar(int level)
idepthSumsSum += ivar * idepthSource[idx+sw+1];
num++;
}

if(num > 0)
{
float depth = ivarSumsSum / idepthSumsSum;
Expand All @@ -883,7 +912,7 @@ void Frame::releaseIDepth(int level)
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;
}
Expand Down
Loading