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

Publishing RGB pointclouds #146

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

Publishing RGB pointclouds #146

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4bf6d0d
saving rgb image to Frame::Data
alexanderkoumis Aug 23, 2015
a35d687
Publishing RGB pointcloud
alexanderkoumis Aug 23, 2015
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
29 changes: 29 additions & 0 deletions lsd_slam_core/src/DataStructures/Frame.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,35 @@ 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)
{
Expand Down
7 changes: 7 additions & 0 deletions lsd_slam_core/src/DataStructures/Frame.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ class Frame
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
friend class FrameMemory;

Frame(int id, int width, int height, const Eigen::Matrix3f& K, double timestamp, const unsigned char* image, const unsigned char* rgbImage);

Frame(int id, int width, int height, const Eigen::Matrix3f& K, double timestamp, const unsigned char* image);

Expand Down Expand Up @@ -100,6 +101,7 @@ class Frame
inline double timestamp() const;

inline float* image(int level = 0);
inline float* imageRGB(int level = 0);
inline const Eigen::Vector4f* gradients(int level = 0);
inline const float* maxGradients(int level = 0);
inline bool hasIDepthBeenSet() const;
Expand Down Expand Up @@ -242,6 +244,7 @@ class Frame


float* image[PYRAMID_LEVELS];
float* imageRGB[PYRAMID_LEVELS];
bool imageValid[PYRAMID_LEVELS];

Eigen::Vector4f* gradients[PYRAMID_LEVELS];
Expand Down Expand Up @@ -360,6 +363,10 @@ inline float* Frame::image(int level)
require(IMAGE, level);
return data.image[level];
}
inline float* Frame::imageRGB(int level)
{
return data.imageRGB[level];
}
inline const Eigen::Vector4f* Frame::gradients(int level)
{
if (! data.gradientsValid[level])
Expand Down
2 changes: 1 addition & 1 deletion lsd_slam_core/src/IOWrapper/ROS/ROSImageStreamThread.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,7 @@ void ROSImageStreamThread::vidCb(const sensor_msgs::ImageConstPtr img)
{
if(!haveCalib) return;

cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::RGB8);

if(img->header.seq < (unsigned int)lastSEQ)
{
Expand Down
12 changes: 6 additions & 6 deletions lsd_slam_core/src/IOWrapper/ROS/ROSOutput3DWrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -96,16 +96,16 @@ void ROSOutput3DWrapper::publishKeyframe(Frame* f)

const float* idepth = f->idepth(publishLvl);
const float* idepthVar = f->idepthVar(publishLvl);
const float* color = f->image(publishLvl);
const float* color = f->imageRGB(publishLvl);

for(int idx=0;idx < w*h; idx++)
for(int idx=0,idxRGB=0;idx < w*h; idx++,idxRGB+=3)
{
pc[idx].idepth = idepth[idx];
pc[idx].idepth_var = idepthVar[idx];
pc[idx].color[0] = color[idx];
pc[idx].color[1] = color[idx];
pc[idx].color[2] = color[idx];
pc[idx].color[3] = color[idx];
pc[idx].color[2] = color[idxRGB];
pc[idx].color[1] = color[idxRGB+1];
pc[idx].color[0] = color[idxRGB+2];
pc[idx].color[3] = 100;
}

keyframe_publisher.publish(fMsg);
Expand Down
4 changes: 2 additions & 2 deletions lsd_slam_core/src/LiveSLAMWrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -129,12 +129,12 @@ void LiveSLAMWrapper::newImageCallback(const cv::Mat& img, Timestamp imgTime)
// need to initialize
if(!isInitialized)
{
monoOdometry->randomInit(grayImg.data, imgTime.toSec(), 1);
monoOdometry->randomInit(grayImg.data, img.data, imgTime.toSec(), 1);
isInitialized = true;
}
else if(isInitialized && monoOdometry != nullptr)
{
monoOdometry->trackFrame(grayImg.data,imageSeqNumber,false,imgTime.toSec());
monoOdometry->trackFrame(grayImg.data, img.data, imageSeqNumber,false,imgTime.toSec());
}
}

Expand Down
8 changes: 4 additions & 4 deletions lsd_slam_core/src/SlamSystem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -854,7 +854,7 @@ void SlamSystem::gtDepthInit(uchar* image, float* depth, double timeStamp, int i
}


void SlamSystem::randomInit(uchar* image, double timeStamp, int id)
void SlamSystem::randomInit(uchar* image, uchar* rgbImage, double timeStamp, int id)
{
printf("Doing Random initialization!\n");

Expand All @@ -864,7 +864,7 @@ void SlamSystem::randomInit(uchar* image, double timeStamp, int id)

currentKeyFrameMutex.lock();

currentKeyFrame.reset(new Frame(id, width, height, K, timeStamp, image));
currentKeyFrame.reset(new Frame(id, width, height, K, timeStamp, image, rgbImage));
map->initializeRandomly(currentKeyFrame.get());
keyFrameGraph->addFrame(currentKeyFrame.get());

Expand All @@ -887,10 +887,10 @@ void SlamSystem::randomInit(uchar* image, double timeStamp, int id)

}

void SlamSystem::trackFrame(uchar* image, unsigned int frameID, bool blockUntilMapped, double timestamp)
void SlamSystem::trackFrame(uchar* image, uchar* rgbImage, unsigned int frameID, bool blockUntilMapped, double timestamp)
{
// Create new frame
std::shared_ptr<Frame> trackingNewFrame(new Frame(frameID, width, height, K, timestamp, image));
std::shared_ptr<Frame> trackingNewFrame(new Frame(frameID, width, height, K, timestamp, image, rgbImage));

if(!trackingIsGood)
{
Expand Down
4 changes: 2 additions & 2 deletions lsd_slam_core/src/SlamSystem.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ friend class IntegrationTest;
SlamSystem& operator=(const SlamSystem&) = delete;
~SlamSystem();

void randomInit(uchar* image, double timeStamp, int id);
void randomInit(uchar* image, uchar* rgbImage, double timeStamp, int id);
void gtDepthInit(uchar* image, float* depth, double timeStamp, int id);


Expand All @@ -82,7 +82,7 @@ friend class IntegrationTest;
// first frame will return Identity = camToWord.
// returns camToWord transformation of the tracked frame.
// frameID needs to be monotonically increasing.
void trackFrame(uchar* image, unsigned int frameID, bool blockUntilMapped, double timestamp);
void trackFrame(uchar* image, uchar* rgbImage, unsigned int frameID, bool blockUntilMapped, double timestamp);

// finalizes the system, i.e. blocks and does all remaining loop-closures etc.
void finalize();
Expand Down
10 changes: 8 additions & 2 deletions lsd_slam_core/src/main_on_images.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -214,6 +214,8 @@ int main( int argc, char** argv )


cv::Mat image = cv::Mat(h,w,CV_8U);
cv::Mat imageRGB = cv::Mat(h,w,CV_8UC3);

int runningIDX=0;
float fakeTimeStamp = 0;

Expand All @@ -222,6 +224,7 @@ int main( int argc, char** argv )
for(unsigned int i=0;i<files.size();i++)
{
cv::Mat imageDist = cv::imread(files[i], CV_LOAD_IMAGE_GRAYSCALE);
cv::Mat imageDistRGB = cv::imread(files[i], CV_LOAD_IMAGE_COLOR);

if(imageDist.rows != h_inp || imageDist.cols != w_inp)
{
Expand All @@ -234,14 +237,17 @@ int main( int argc, char** argv )
continue;
}
assert(imageDist.type() == CV_8U);
assert(imageDist.type() == CV_8UC3);
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Shouldn't this line check imageDistRGB's type instead?


undistorter->undistort(imageDist, image);
undistorter->undistort(imageDistRGB, imageRGB);
assert(image.type() == CV_8U);
assert(imageRGB.type() == CV_8UC3);

if(runningIDX == 0)
system->randomInit(image.data, fakeTimeStamp, runningIDX);
system->randomInit(image.data, imageRGB.data, fakeTimeStamp, runningIDX);
else
system->trackFrame(image.data, runningIDX ,hz == 0,fakeTimeStamp);
system->trackFrame(image.data, imageRGB.data, runningIDX ,hz == 0,fakeTimeStamp);
runningIDX++;
fakeTimeStamp+=0.03;

Expand Down