From 39305bf39cc0fc79505350b03bb61a38366dbc81 Mon Sep 17 00:00:00 2001 From: Hauke Strasdat Date: Mon, 12 Dec 2011 19:53:35 +0000 Subject: [PATCH] Support for RGB-D cameras (live SLAM, saving sequences) + small changes --- README | 143 +++++++++++++++++++- data/rgbd_example.cfg | 56 ++++++++ data/rgbd_live.cfg | 52 ++++++++ src/CMakeLists.txt | 14 +- src/create_dictionary.cpp | 10 +- src/dump_rgbd.cpp | 262 +++++++++++++++++++++++++++++++++++++ src/frame_grabber-impl.cpp | 18 +++ src/frame_grabber.cpp | 35 ++++- src/frame_grabber.hpp | 7 + src/maths_utils.h | 4 + src/placerecognizer.cpp | 13 +- src/rgbd_grabber.cpp | 13 +- src/slam_graph.hpp | 9 +- src/stereo_camera.cpp | 7 + src/stereo_camera.h | 5 + src/stereo_frontend.cpp | 8 +- src/stereo_frontend.h | 1 + 17 files changed, 618 insertions(+), 39 deletions(-) create mode 100644 data/rgbd_example.cfg create mode 100644 data/rgbd_live.cfg create mode 100644 src/dump_rgbd.cpp diff --git a/README b/README index fedb64e..38a1eb1 100644 --- a/README +++ b/README @@ -209,17 +209,13 @@ Move to main project folder "ScaViSLAM" and install ScaViSLAM: cd ../../../ mkdir svs_build cd svs_build -<< - -In case you would like to compile ScaViSLAM WITHOUT CUDA support, open -"../src/CMakeLists.txt" using your favourite editor and change -"SET(CUDA_SUPPORT ON)" --> "SET(CUDA_SUPPORT OFF)". - ->> cmake ../src make -j8 << +In case you would like to compile ScaViSLAM WITHOUT CUDA support, type +"cmake -DCUDA_SUPPORT=OFF ../src/" instaed of "cmake ../src" + Make sure the "New College" data set is downloaded completely. Then add a sym-link inside "ScaViSLAM/data": @@ -233,3 +229,136 @@ Run ScaViSLAM on new college image sequence: cd ../svs_build ./stereo_slam ../data/newcollege.cfg << + + +*** How to use a camera live *** + +At the moment, only RGB-D cameras following the PrimeSense reference design +(e.g. Ausus Xtion PRO LIVE) are supported. +First, you need to install PCL (which includes OpenNI): + +>> +sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl +sudo apt-get update +sudo apt-get install libpcl-all +<< + +Reinstall ScaViSLAM with PCL support: + +>> +cd MY_SOFTWARE_DIRECTORY +cd /ScaViSLAM/ +mkdir svs_build_with_pcl +cd svs_build_with_pcl +cmake -DCUDA_SUPPORT=ON .. +make -j8 +>> + +Try out ScaViSLAM with your RGB-D camera life: + +>> +./stereo_slam ../data/rgbd-Live.cfg +<< + +To dump RGBD data to the hard disk (to ../data/out; create this directory or +set approriate sym-link.).: + +>> +./dump_rgbd ../data/rgbd_live.cfg +<< + +Finally, you can process the saved image sequence by adapting the configuration +files "../data/rgbd_example.cfg". Basically, you have to point +"framepipe.path_str" to the saved images. + + +*** What about standard stereo cameras ? *** + +This is more tricky since we need to make sure that the image stream is +rectified. Some adaptation in the code might be necessary. + + +*** How to contribute? *** + +You are welcome to report bugs and request features at: +https://github.com/strasdat/ScaViSLAM/issues. +To do so click on "New Issue", fill out the form and select the approriate +label ("Bug" or "Feature Request"). +Please feel free to also browse through the "ToDo" and "Future Features" +issues, so you can find out which features are planned to be included +soon ("ToDo") or eventually ("Future Features"). + +The best and easiest way to contribute is to use the "github" way: + - Register at github (for free). + - Got to: "https://github.com/strasdat/ScaViSLAM" + - Click on "Fork" to create a Fork of ScaViSLAM. + - Commit and push changes into your Fork. + - Click on "Pull Request" in order to ask me whether I would like to merge + the changes into the ScaViSLAM master branch. + +(If it is only a small change/bug fix, you could also simply send me +patch etc.) + +In order to maximize the likelyhood that I will accept your pull request, +please read carefully thourgh the coding style guide: + + +*** Coding Style Guide *** + +The coding style is largely influenced by the google c++ coding style guide: +http://google-styleguide.googlecode.com/svn/trunk/cppguide.xml +(Reading the document is insightful in any case, even if you don't agree with +"google's" decision!) + +I am especially strick about the following rules: + + - "When defining a function, parameter order is: inputs, then outputs." + - "All parameters passed by reference must be labeled const." + Thus, the following is NOT okay: + 'void method(const Vector2d & in, Vector2d & out)' + Use this instead: + 'void method(const Vector2d & in, Vector2d * out)' + - "Of the built-in C++ integer types, the only one used is int. If a program + needs a variable of a different size, use a precise-width integer type + from , such as int16_t." + - "Use 0 for integers, 0.0 for reals, NULL for pointers, and '\0' for chars." + - "We do not use C++ exceptions." + - "Use the specified order of declarations within a class: public: before + private:, methods before data members (variables), etc." + - "Use a struct only for passive objects that carry data; everything else + is a class." + - "Prefer small and focused functions." + - "Use only approved libraries and language extensions from C++11 (formerly + known as C++0x). Currently, none are approved." + - "We do not allow default function parameters, except in a few uncommon + situations..." + - "Use prefix form (++i) of the increment and decrement operators with + iterators and other template objects." + - "We strongly recommend that you use const whenever it makes sense to do so." + +At some points, I deviate from the C++ coding style guide: + - 'Using namespace X;' is okay, but only within(!) the ScaViSLAM namespace. + Never pollute the general namespace '::'. + (However, do not use "using namespace cv" or 'using namespace pangolin'.) + - I use 'boost::thread'. + - In rare cases, it is okay to habe public class members. + - It is okay to use streams such as "cout", "cerr", "stringstream". (But + usually, I'd like to use "cerr" and friends only for debugging purposes...) + +I also deviate slightly from google's formating. Therefore the following rule +is the most important one: + +"Use common sense and BE CONSISTENT. + +If you are editing code, take a few minutes to look at the code around you and +determine its style. If they use spaces around their if clauses, you should, +too. If their comments have little boxes of stars around them, make your +comments have little boxes of stars around them too." + +(All cites in "quatation marks" are from the google coding style guide.) + +If you would like to use automatic identation, I recommend you to use +the standard setting in QtCreator 1.3.1 (or newer), however with +"Tools/Options/Text Editor/Behavior/Indent size = 2" and a maximal number of +80 characters per line (i.e. mark +"Tools/Options/Text Editor/Display/Display right margin at column = 80"). diff --git a/data/rgbd_example.cfg b/data/rgbd_example.cfg new file mode 100644 index 0000000..d1c9d70 --- /dev/null +++ b/data/rgbd_example.cfg @@ -0,0 +1,56 @@ + +cam.dist_right1 = 0; +cam.dist_right2 = 0; +cam.dist_right3 = 0; +cam.dist_right4 = 0; +cam.dist_right5 = 0; + +cam.dist_left1 = 0; +cam.dist_left2 = 0; +cam.dist_left3 = 0; +cam.dist_left4 = 0; +cam.dist_left5 = 0; + + + + +cam.rotx_right = 0; +cam.roty_right = 0; +cam.rotz_right = 0; + +cam.rotx_left = 0; +cam.roty_left = 0; +cam.rotz_left = 0; + + + + + + + +cam.width = 640; +cam.height = 480; +cam.f = 591.52441444; +cam.px = 320; +cam.py = 240; +cam.baseline = 0.07468; +framepipe.skip_imgs = 100; +framepipe.livestream = 0; +framepipe.mask_img = 0; +framepipe.disp_img = 1; +framepipe.right_img = 0; +framepipe.color_img = 0; +framepipe.format_str = png; +framepipe.path_str = ../data/example; +framepipe.base_str = .*rgbd.*; +frontend.save_dense_cloud = 1; +frontend.new_keyframe_featureless_corners_thr = 2; +frontend.new_keyframe_pixel_thr = 70; +frontend.newpoint_clearance = 2; +frontend.num_frames_metric_loop_check = 50; +frontend.covis_thr = 10; +graph.outer_window = 100; +graph.inner_window = 7; +use_n_levels_in_frontent = 3; +ui.parallax_thr = 0.15 +ui.max_reproj_error = 3 diff --git a/data/rgbd_live.cfg b/data/rgbd_live.cfg new file mode 100644 index 0000000..1fe4546 --- /dev/null +++ b/data/rgbd_live.cfg @@ -0,0 +1,52 @@ + +cam.dist_right1 = 0; +cam.dist_right2 = 0; +cam.dist_right3 = 0; +cam.dist_right4 = 0; +cam.dist_right5 = 0; + +cam.dist_left1 = 0; +cam.dist_left2 = 0; +cam.dist_left3 = 0; +cam.dist_left4 = 0; +cam.dist_left5 = 0; + + + + +cam.rotx_right = 0; +cam.roty_right = 0; +cam.rotz_right = 0; + +cam.rotx_left = 0; +cam.roty_left = 0; +cam.rotz_left = 0; + + + + + + + +cam.width = 640; +cam.height = 480; +cam.f = 591.52441444; +cam.px = 320; +cam.py = 240; +cam.baseline = 0.07468; +framepipe.livestream = 1; +framepipe.mask_img = 0; +framepipe.disp_img = 1; +framepipe.right_img = 0; +framepipe.color_img = 1; +frontend.save_dense_cloud = 1; +frontend.new_keyframe_featureless_corners_thr = 2; +frontend.new_keyframe_pixel_thr = 70; +frontend.newpoint_clearance = 2; +frontend.num_frames_metric_loop_check = 50; +frontend.covis_thr = 15; +graph.outer_window = 100; +graph.inner_window = 15; +use_n_levels_in_frontent = 3; +ui.parallax_thr = 0.15 +ui.max_reproj_error = 3 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c9cf70f..b2d9936 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -7,14 +7,21 @@ SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} SET (CMAKE_VERBOSE_MAKEFILE ON) -SET(CUDA_SUPPORT ON) -SET(PCL_SUPPORT OFF) +IF( NOT CUDA_SUPPORT ) + SET(CUDA_SUPPORT ON) +ENDIF() + +IF( NOT PCL_SUPPORT ) + SET(PCL_SUPPORT OFF) +ENDIF() IF(CUDA_SUPPORT) + MESSAGE ("CUDA SUPPORT ON") ADD_DEFINITIONS(-DSCAVISLAM_CUDA_SUPPORT) ENDIF() IF(PCL_SUPPORT) + MESSAGE ("PCL SUPPORT ON") ADD_DEFINITIONS(-DSCAVISLAM_PCL_SUPPORT) FIND_PACKAGE(PCL 1.2 REQUIRED) ADD_DEFINITIONS(${PCL_DEFINITIONS}) @@ -146,5 +153,8 @@ ELSE() ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCES}) ENDIF() ADD_EXECUTABLE(create_dictionary create_dictionary.cpp) +IF(PCL_SUPPORT) + ADD_EXECUTABLE(dump_rgbd dump_rgbd.cpp) +ENDIF() INCLUDE_DIRECTORIES(${INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) diff --git a/src/create_dictionary.cpp b/src/create_dictionary.cpp index bbe4377..11b8917 100644 --- a/src/create_dictionary.cpp +++ b/src/create_dictionary.cpp @@ -164,13 +164,13 @@ calculateWordsAndSaveThem(int TARGET_NUM_WORDS, distance()); cout << "Done: dictionary of " << num_centers << " words created!" << endl; assert(sizeof(float)==4); - cv::Mat centers_floats_as_4uchars(num_centers, - descriptors.cols*4, - CV_8U, - centers.data); + cv::Mat centers_float_as_four_uint8(num_centers, + descriptors.cols*4, + CV_8U, + centers.data); stringstream str_stream; str_stream<< "surfwords" << num_centers << ".png"; - cv::imwrite(str_stream.str(), centers_floats_as_4uchars); + cv::imwrite(str_stream.str(), centers_float_as_four_uint8); cout << "Saved as file: " << str_stream.str() << endl; } diff --git a/src/dump_rgbd.cpp b/src/dump_rgbd.cpp new file mode 100644 index 0000000..f4fd2bf --- /dev/null +++ b/src/dump_rgbd.cpp @@ -0,0 +1,262 @@ +// This file is part of ScaViSLAM. +// +// Copyright 2011 Hauke Strasdat (Imperial College London) +// +// ScaViSLAM is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published +// by the Free Software Foundation, either version 3 of the License, or +// any later version. +// +// ScaViSLAM is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with ScaViSLAM. If not, see . + +#include +#include + +#include + +#include "global.h" + +#include "accessor_macros.h" +#include "stereo_camera.h" +#include "draw2d.h" +#include "draw3d.h" +#include "frame_grabber.hpp" + +using namespace ScaViSLAM; + +struct Views +{ + pangolin::OpenGlRenderState pangolin_cam; + pangolin::View * rgbd; + pangolin::View * disp; + pangolin::DataLog logger; +}; + +struct Modules +{ + Modules() + : frame_grabber(NULL), + per_mon(NULL) {} + + ~Modules() + { + if(frame_grabber!=NULL) + delete frame_grabber; + if(per_mon!=NULL) + delete per_mon; + } + + FrameGrabber * frame_grabber; + PerformanceMonitor * per_mon; +}; + +struct ImagePair +{ + ImagePair() {} + ImagePair(const cv::Mat & rgb, + const cv::Mat & disp) : rgb(rgb), disp(disp){} + cv::Mat rgb; + cv::Mat disp; +}; + +class FileWriter +{ +public: + class FileWriterMonitor + { + public: + void + pushImages (const ImagePair & images) + { + boost::mutex::scoped_lock lock(my_mutex_); + image_queue_.push(images); + } + + bool + getImages (ImagePair * images) + { + boost::mutex::scoped_lock lock(my_mutex_); + if (image_queue_.size()==0) + return false; + + cerr << image_queue_.size() << endl; + + *images = image_queue_.front(); + image_queue_.pop(); + + return true; + } + + private: + queue image_queue_; + boost::mutex my_mutex_; + }; + + void + operator()() + { + ImagePair images; + int counter = 0; + char str_rgb[80]; + char str_disp[80]; + while(true) + { + if (monitor.getImages(&images)) + { + sprintf(str_rgb, "../data/out/img_%06d_left.png", counter); + sprintf(str_disp, "../data/out/img_%06d_disp.png", counter); + ++counter; + cv::imwrite(string(str_rgb), + images.rgb); + cv::imwrite(string(str_disp), + images.disp); + } + boost::this_thread::sleep(boost::posix_time::milliseconds(1)); + } + } + + FileWriterMonitor monitor; +}; + + +Modules +startModules(const StereoCamera & stereo_camera, + Views * views) +{ + Modules modules; + modules.per_mon = new PerformanceMonitor; + modules.frame_grabber + = new FrameGrabber(stereo_camera, + Vector4d(0,0,0,0), + modules.per_mon); + modules.per_mon->add("drawing"); + modules.per_mon->add("grab frame"); + modules.per_mon->add("preprocess"); + modules.per_mon->setup(&views->logger); + + modules.frame_grabber->initialise(); + modules.frame_grabber->processNextFrame(); + return modules; +} + + +Views +initializeViews(const StereoCamera & stereo_camera) +{ + pangolin::CreateGlutWindowAndBind("Main", + stereo_camera.width()*2, + stereo_camera.height()); + Views views; + views.rgbd + = &(pangolin::Display("rgbd") + .SetBounds(1.0,0.0,0.0,0.5,4./3.)); + views.disp + = &(pangolin::Display("depth") + .SetBounds(1.0,0.0,0.5,1.0,4./3.)); + + views.pangolin_cam.Set(stereo_camera.getOpenGlMatrixSpec()); + views.pangolin_cam.Set(pangolin::IdentityMatrix(pangolin::GlModelViewStack)); + return views; +} + +//TODO: main method to long... +int main(int argc, const char* argv[]) +{ + if (argc<2) + { + cout << "please specify configuration file!" << endl; + exit(0); + } + + pangolin::ParseVarsFile(argv[1]); + pangolin::Var cam_width("cam.width",640); + pangolin::Var cam_height("cam.height",480); + pangolin::Var cam_f("cam.f",570.342); + pangolin::Var cam_px("cam.px",320); + pangolin::Var cam_py("cam.py",240); + pangolin::Var cam_baseline("cam.baseline",0.075); + + StereoCamera stereo_camera((double)cam_f, + Vector2d((double)cam_px,(double)cam_py), + cv::Size((int)cam_width,(int)cam_height), + cam_baseline); + + Views views = initializeViews(stereo_camera); + + Modules modules = startModules(stereo_camera, + &views); + pangolin::Var ui_fps("ui.fps",false); + + FileWriter writer; + boost::thread writer_thread(boost::ref(writer)); + + while(true) + { + modules.per_mon->new_frame(); + ui_fps = modules.per_mon->fps(); + modules.frame_grabber->processNextFrame(); + cerr << ui_fps << endl; + + + cv::Mat float_as_4uint + (modules.frame_grabber->frame_data.disp.size().height, + modules.frame_grabber->frame_data.disp.size().width*4, + CV_8U, + modules.frame_grabber->frame_data.disp.data); + + + writer.monitor.pushImages( + ImagePair(modules.frame_grabber->frame_data.cur_left().color_uint8, + float_as_4uint)); + + modules.per_mon->start("drawing"); + views.rgbd->ActivateScissorAndClear(); + Draw2d::activate(modules.frame_grabber + ->frame_data.cur_left().uint8.size()); + Draw2d::texture(modules.frame_grabber + ->frame_data.cur_left().uint8); + vector hsv_array(3); + hsv_array[0] = cv::Mat(modules.frame_grabber + ->frame_data.disp.size(), CV_8UC1); + + modules.frame_grabber->frame_data.disp + .convertTo(hsv_array[0],CV_8UC1, 5.,0.); + hsv_array[1] = cv::Mat(modules.frame_grabber + ->frame_data.disp.size(), CV_8UC1, 255); + hsv_array[2] = cv::Mat(modules.frame_grabber + ->frame_data.disp.size(), CV_8UC1, 255); + + cv::Mat hsv(modules.frame_grabber + ->frame_data.disp.size(), CV_8UC3); + cv::merge(hsv_array, hsv); + cv::cvtColor(hsv, modules.frame_grabber + ->frame_data.color_disp, CV_HSV2BGR); + views.disp->ActivateScissorAndClear(); + Draw2d::activate(modules.frame_grabber + ->frame_data.cur_left().uint8.size()); + Draw2d::texture(modules.frame_grabber + ->frame_data.color_disp); + + modules.per_mon->stop("drawing"); + glutSwapBuffers(); + glutMainLoopEvent(); + + if (pangolin::ShouldQuit()) + { + boost::this_thread::sleep(boost::posix_time::milliseconds(10000)); + exit(0); + } + if(pangolin::HasResized()) + pangolin::DisplayBase().ActivateScissorAndClear(); + + + } + return 0; +} + diff --git a/src/frame_grabber-impl.cpp b/src/frame_grabber-impl.cpp index e049cdc..bb1059c 100644 --- a/src/frame_grabber-impl.cpp +++ b/src/frame_grabber-impl.cpp @@ -133,6 +133,24 @@ intializeRectifier() rect_map_right_[0], rect_map_right_[1]); } +template <> +void FrameGrabber:: +depthToDisp(const cv::Mat & depth_img, + cv::Mat * disp_img) const +{ + assert(depth_img.type()==CV_16U); + *disp_img = cv::Mat(depth_img.size(), CV_32F); + static const float FACTOR = 1./5000.; + for (int v=0; v(v,u)*FACTOR; + disp_img->at(v,u) = frame_data.cam.depthToDisp(depth); + } + +} + template class FrameGrabber; template class FrameGrabber; } diff --git a/src/frame_grabber.cpp b/src/frame_grabber.cpp index 0184eb0..47a8ffb 100644 --- a/src/frame_grabber.cpp +++ b/src/frame_grabber.cpp @@ -17,6 +17,8 @@ #include "frame_grabber.hpp" +#include + #include #include #include @@ -165,11 +167,6 @@ processNextFrame() frame_data.cur_left().uint8 = cv::imread(left_sstr.str(),0); } - if (params_.right_img) - { - right_sstr << basename << "right." << params_.format_str ; - frame_data.right.uint8 = cv::imread(right_sstr.str(),0); - } if (params_.disp_img) { disp_sstr << basename << "disp." << params_.format_str ; @@ -181,6 +178,23 @@ processNextFrame() CV_32F, float_as_4uint.data).clone(); cerr << frame_data.disp.size().width << " " << frame_data.disp.size().height << endl; + frame_data.have_disp_img = true; + } + else if (params_.depth_img) + { + std::stringstream depth_sstr; + depth_sstr << basename << "depth." << params_.format_str ; + cerr << depth_sstr.str() << endl; + //frame_data.right.uint8 = cv::imread(right_sstr.str(),0); + cv::Mat depth = cv::imread(depth_sstr.str(),-1); + cerr << depth.type() << endl; + depthToDisp(depth, &frame_data.disp); + frame_data.have_disp_img = true; + } + else if (params_.right_img) + { + right_sstr << basename << "right." << params_.format_str ; + frame_data.right.uint8 = cv::imread(right_sstr.str(),0); } if (params_.rectify_frame) rectifyFrame(); @@ -194,6 +208,14 @@ processNextFrame() ++frame_data.frame_id; } +template +void FrameGrabber:: +depthToDisp(const cv::Mat & depth_img, + cv::Mat * disp_img) const +{ + assert(false); +} + template void FrameGrabber:: loadParams() @@ -235,6 +257,8 @@ loadParams() ("framepipe.right_img",false); params_.disp_img = pangolin::Var ("framepipe.disp_img",false); + params_.depth_img = pangolin::Var + ("framepipe.depth_img",false); params_.rectify_frame = pangolin::Var ("framepipe.rectify_frame",false); @@ -275,6 +299,7 @@ frameFromLiveCamera() cv::cvtColor(frame_data.cur_left().color_uint8, frame_data.cur_left().uint8, CV_BGR2GRAY); + frame_data.have_disp_img = true; #else assert(false); #endif diff --git a/src/frame_grabber.hpp b/src/frame_grabber.hpp index 3c746ed..a19c1b2 100644 --- a/src/frame_grabber.hpp +++ b/src/frame_grabber.hpp @@ -98,6 +98,7 @@ class FrameData gpu_pyr_float32_dx(NUM_PYR_LEVELS), gpu_pyr_float32_dy(NUM_PYR_LEVELS), #endif + have_disp_img(false), offset(0) {} @@ -146,6 +147,7 @@ class FrameData vector pyr_float32_dy; #endif int frame_id; + bool have_disp_img; private: int offset; }; @@ -169,6 +171,7 @@ class FrameGrabber bool color_img; bool right_img; bool disp_img; + bool depth_img; bool rectify_frame; }; @@ -210,6 +213,10 @@ class FrameGrabber void preprocessing (); + void + depthToDisp (const cv::Mat & depth_img, + cv::Mat * disp_img) const; + PerformanceMonitor * per_mon_; cv::Mat rect_map_left_[2]; cv::Mat rect_map_right_[2]; diff --git a/src/maths_utils.h b/src/maths_utils.h index e00128c..21ea744 100644 --- a/src/maths_utils.h +++ b/src/maths_utils.h @@ -50,12 +50,16 @@ interpolateMat_8u (const cv::Mat & mat, const Vector2f & uv); Vector2d project2d (const Vector3d&); + Vector3d project3d (const Vector4d&); + Vector3d unproject2d (const Vector2d&); + Vector4d unproject3d (const Vector3d&); + double norm_max (const Eigen::VectorXd & v); diff --git a/src/placerecognizer.cpp b/src/placerecognizer.cpp index d75b369..f92afdb 100644 --- a/src/placerecognizer.cpp +++ b/src/placerecognizer.cpp @@ -82,15 +82,16 @@ PlaceRecognizer ::PlaceRecognizer(const StereoCamera & stereo_cam) : stereo_cam_(stereo_cam) { - cv::Mat words_floats_as_4uchars + cv::Mat words_float_as_four_uint8 = cv::imread(string("../data/surfwords10000.png"),-1); - assert(words_floats_as_4uchars.size().area()>0); - assert(words_floats_as_4uchars.type()==CV_8U); + assert(words_float_as_four_uint8.size().area()>0); + assert(words_float_as_four_uint8.type()==CV_8U); assert(sizeof(float)==4); - words_ = cv::Mat(words_floats_as_4uchars.rows, - words_floats_as_4uchars.cols/4, + assert(words_float_as_four_uint8.cols%4==0); + words_ = cv::Mat(words_float_as_four_uint8.rows, + words_float_as_four_uint8.cols/4, CV_32F, - words_floats_as_4uchars.data).clone(); + words_float_as_four_uint8.data).clone(); flann_index_ = tr1::shared_ptr (new cv::flann::GenericIndex( diff --git a/src/rgbd_grabber.cpp b/src/rgbd_grabber.cpp index 42725ab..d88d87d 100644 --- a/src/rgbd_grabber.cpp +++ b/src/rgbd_grabber.cpp @@ -7,6 +7,7 @@ using namespace std; namespace ScaViSLAM { boost::mutex global_mutex; + bool new_frame; cv::Mat global_rgb_img(cv::Size(640, 480), CV_8UC3); cv::Mat global_disp_img(cv::Size(640, 480), CV_32F); @@ -22,15 +23,21 @@ cloud_cb_(const boost::shared_ptr& rgb, reinterpret_cast(global_rgb_img.data)); depth->fillDisparityImage(640, 480, reinterpret_cast(global_disp_img.data)); + new_frame = true; } bool RgbdGrabber ::getFrame(cv::Mat * rgb, cv::Mat * disp) { boost::mutex::scoped_lock lock(global_mutex); - *rgb = global_rgb_img; - *disp = global_disp_img; - return true; + if (new_frame) + { + *rgb = global_rgb_img; + *disp = global_disp_img; + new_frame = false; + return true; + } + return false; } void RgbdGrabber diff --git a/src/slam_graph.hpp b/src/slam_graph.hpp index e2ebde3..b5fee3f 100644 --- a/src/slam_graph.hpp +++ b/src/slam_graph.hpp @@ -554,11 +554,10 @@ class SlamGraph unmargPosesEnteringInnerW(); void - computeConstraint (const Vertex & v1, - const Vertex & v2, - Pose * T_2_from_1, - Matrix * Lambda); - + computeConstraint (const Vertex & v1, + const Vertex & v2, + Pose * T_2_from_1, + Matrix * Lambda); void setupG2o (g2o::SparseOptimizer * optimizer); diff --git a/src/stereo_camera.cpp b/src/stereo_camera.cpp index e81f9c3..521340a 100644 --- a/src/stereo_camera.cpp +++ b/src/stereo_camera.cpp @@ -51,4 +51,11 @@ ::unmap_uvu( const Vector3d& uvu) const return unproject2d(LinearCamera::unmap(uvu.head(2)))*z; } +float StereoCamera +::depthToDisp(float depth) const +{ + float scaled_disparity = focal_length_/depth; + return scaled_disparity/baseline_; +} + } diff --git a/src/stereo_camera.h b/src/stereo_camera.h index 3d53c31..92be4b8 100644 --- a/src/stereo_camera.h +++ b/src/stereo_camera.h @@ -69,11 +69,16 @@ class StereoCamera : public VisionTools::LinearCamera Matrix4d Q () const; + Vector3d map_uvu (const Vector3d & xyz) const; + Vector3d unmap_uvu (const Vector3d& uvu) const; + float + depthToDisp (float depth) const; + const double& baseline() const { return baseline_; diff --git a/src/stereo_frontend.cpp b/src/stereo_frontend.cpp index 9ef4029..96304b3 100644 --- a/src/stereo_frontend.cpp +++ b/src/stereo_frontend.cpp @@ -102,10 +102,8 @@ ::processFirstFrame() actkey_id = getNewUniqueId(); neighborhood_->T_me_from_w_map.insert(make_pair(actkey_id,T_cur_from_actkey_)); - static pangolin::Var disp_img - ("framepipe.disp_img",false); - if (disp_img) + if (frame_data_->have_disp_img) { vector hsv_array(3); hsv_array[0] = cv::Mat(frame_data_->disp.size(), CV_8UC1); @@ -181,10 +179,8 @@ ::processFrame(bool * is_frame_dropped) per_mon_->stop("dense tracking"); per_mon_->start("stereo"); - static pangolin::Var disp_img - ("framepipe.disp_img",false); - if (disp_img) + if (frame_data_->have_disp_img) { vector hsv_array(3); hsv_array[0] = cv::Mat(frame_data_->disp.size(), CV_8UC1); diff --git a/src/stereo_frontend.h b/src/stereo_frontend.h index 9b642ab..7ae3dbc 100644 --- a/src/stereo_frontend.h +++ b/src/stereo_frontend.h @@ -177,6 +177,7 @@ class StereoFrontend bool shallWeDropNewKeyframe (const PointStatistics & point_stats); + void addNewKeyframe (const ALIGNED >::vector & feature_tree,