i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged freenect2 and dc1394 commits

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-03 15:17:57 -04:00
parent 4ca200fe1a 5ef3194786
commit 87469b39cd
6 changed files with 241 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
CMakeLists.txt
View File

@@ -134,6 +134,7 @@ IF("${VTK_MAJOR_VERSION}" EQUAL 5)
ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5)
FIND_PACKAGE(ZLIB REQUIRED)
FIND_PACKAGE(Freenect)
FIND_PACKAGE(freenect2 QUIET)
FIND_PACKAGE(OpenNI2)
FIND_PACKAGE(DC1394)
FIND_PACKAGE(G2O)
@@ -344,6 +345,12 @@ ELSE()
MESSAGE(STATUS " With OpenNI2 = NO (OpenNI2 not found)")
ENDIF()
IF(freenect2_FOUND)
MESSAGE(STATUS " With Freenect2 = YES")
ELSE()
MESSAGE(STATUS " With Freenect2 = NO (libfreenect2 not found)")
ENDIF()
IF(DC1394_FOUND)
MESSAGE(STATUS " With dc1394 = YES")
ELSE()

42
corelib/include/rtabmap/core/CameraRGBD.h
View File

@@ -54,7 +54,14 @@ class VideoStream;
namespace pcl
{
class Grabber;
class Grabber;
}
namespace libfreenect2
{
class Freenect2;
class Freenect2Device;
class SyncMultiFrameListener;
}
typedef struct _freenect_context freenect_context;
@@ -265,4 +272,37 @@ private:
FreenectDevice * freenectDevice_;
};
/////////////////////////
// CameraFreenect2
/////////////////////////
class RTABMAP_EXP CameraFreenect2 :
public CameraRGBD
{
public:
static bool available();
public:
// default local transform z in, x right, y down));
CameraFreenect2(int deviceId= 0,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity(),
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f);
virtual ~CameraFreenect2();
bool init();
protected:
virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
private:
int deviceId_;
libfreenect2::Freenect2 * freenect2_;
libfreenect2::Freenect2Device *dev_;
libfreenect2::SyncMultiFrameListener * listener_;
};
} // namespace rtabmap

12
corelib/src/CMakeLists.txt
View File

@@ -83,6 +83,18 @@ IF(OpenNI2_FOUND)
)
ENDIF(OpenNI2_FOUND)
IF(freenect2_FOUND)
ADD_DEFINITIONS("-DWITH_FREENECT2")
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${freenect2_INCLUDE_DIRS}
)
SET(LIBRARIES
${LIBRARIES}
${freenect2_LIBRARIES}
)
ENDIF(freenect2_FOUND)
IF(DC1394_FOUND)
ADD_DEFINITIONS("-DWITH_DC1394")
SET(INCLUDE_DIRS

151
corelib/src/CameraRGBD.cpp
View File

@@ -51,6 +51,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#endif
#endif
#ifdef WITH_FREENECT2
#include <libfreenect2/libfreenect2.hpp>
#include <libfreenect2/frame_listener_impl.h>
#endif
#ifdef WITH_OPENNI2
#include <OniVersion.h>
#include <OpenNI.h>
@@ -940,20 +945,146 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, fl
delete freenectDevice_;
freenectDevice_ = 0;
}
if(depth.empty() || rgb.empty())
{
rgb = cv::Mat();
depth = cv::Mat();
fx = 0.0f;
fy = 0.0f;
cx = 0.0f;
cy = 0.0f;
}
}
if(depth.empty() || rgb.empty())
{
rgb = cv::Mat();
depth = cv::Mat();
fx = 0.0f;
fy = 0.0f;
cx = 0.0f;
cy = 0.0f;
}
#else
UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
#endif
}
//
// CameraFreenect2
//
bool CameraFreenect2::available()
{
#ifdef WITH_FREENECT2
return true;
#else
return false;
#endif
}
CameraFreenect2::CameraFreenect2(int deviceId, float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
deviceId_(deviceId),
freenect2_(0),
dev_(0)
{
#ifdef WITH_FREENECT2
freenect2_ = new libfreenect2::Freenect2();
//listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
UWARN("CameraFreenect2: Images are not yet registered!");
#endif
}
CameraFreenect2::~CameraFreenect2()
{
#ifdef WITH_FREENECT2
if(dev_)
{
dev_->stop();
dev_->close();
}
if(listener_)
{
delete listener_;
}
if(freenect2_)
{
delete freenect2_;
}
#endif
}
bool CameraFreenect2::init()
{
#ifdef WITH_FREENECT2
if(dev_)
{
dev_->stop();
dev_->close();
dev_ = 0;
}
if(deviceId_ <= 0)
{
dev_ = freenect2_->openDefaultDevice();
}
else
{
dev_ = freenect2_->openDevice(deviceId_);
}
if(dev_)
{
dev_->setColorFrameListener(listener_);
dev_->setIrAndDepthFrameListener(listener_);
dev_->start();
UINFO("CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str());
UINFO("CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str());
return true;
}
else
{
UERROR("CameraFreenect2: no device connected or failure opening the default one!");
}
#else
UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
#endif
return false;
}
void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
{
#ifdef WITH_FREENECT2
rgb = cv::Mat();
depth = cv::Mat();
fx = 0.0f;
fy = 0.0f;
cx = 0.0f;
cy = 0.0f;
if(dev_ && listener_)
{
libfreenect2::FrameMap frames;
if(listener_->waitForNewFrame(frames, 1000))
{
libfreenect2::Frame *rgbFrame = frames[libfreenect2::Frame::Color];
//libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
libfreenect2::Frame *depthFrame = frames[libfreenect2::Frame::Depth];
if(rgbFrame && depthFrame)
{
cv::flip(cv::Mat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data), rgb, 1);
cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(depth, depth, 1);
libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
}
listener_->release(frames);
}
else
{
UWARN("CameraFreenect2: Failed to get frames! rtabmap should link on libusb of "
"libfreenect2, this can be done by setting LD_LIBRARY_PATH to "
"\"libfreenect2/depends/libusb/lib\"");
}
}
#else
UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
#endif
}
} // namespace rtabmap

4
corelib/src/Rtabmap.cpp
View File

@@ -977,7 +977,7 @@ bool Rtabmap::process(const SensorData & data)
{
std::string rejectedMsg;
UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
double variance = -1.0;
double variance = 1.0;
int inliers = -1;
Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg, &inliers, &variance);
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
@@ -1407,7 +1407,7 @@ bool Rtabmap::process(const SensorData & data)
{
//Compute transform if metric data are present
Transform transform;
double variance = -1;
double variance = 1;
if(_rgbdSlamMode)
{
std::string rejectedMsg;

51
tools/CameraRGBD/main.cpp
View File

@@ -36,7 +36,12 @@ void showUsage()
{
printf("\nUsage:\n"
"rtabmap-rgbd_camera driver\n"
" driver Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
" driver Driver number to use: 0=OpenNI-PCL\n"
" 1=OpenNI2\n"
" 2=Freenect\n"
" 3=OpenNI-CV\n"
" 4=OpenNI-CV-ASUS\n"
" 5=Freenect2\n\n");
exit(1);
}
@@ -53,9 +58,9 @@ int main(int argc, char * argv[])
else
{
driver = atoi(argv[argc-1]);
if(driver < 0 || driver > 4)
if(driver < 0 || driver > 5)
{
UERROR("driver should be between 0 and 4.");
UERROR("driver should be between 0 and 5.");
showUsage();
}
}
@@ -64,7 +69,7 @@ int main(int argc, char * argv[])
rtabmap::CameraRGBD * camera = 0;
if(driver == 0)
{
camera = new rtabmap::CameraOpenni("", 0);
camera = new rtabmap::CameraOpenni();
}
else if(driver == 1)
{
@@ -73,7 +78,7 @@ int main(int argc, char * argv[])
UERROR("Not built with OpenNI2 support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNI2(0);
camera = new rtabmap::CameraOpenNI2();
}
else if(driver == 2)
{
@@ -82,7 +87,7 @@ int main(int argc, char * argv[])
UERROR("Not built with Freenect support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect(0, 0);
camera = new rtabmap::CameraFreenect();
}
else if(driver == 3)
{
@@ -91,7 +96,7 @@ int main(int argc, char * argv[])
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(false, 0);
camera = new rtabmap::CameraOpenNICV(false);
}
else if(driver == 4)
{
@@ -100,7 +105,16 @@ int main(int argc, char * argv[])
UERROR("Not built with OpenNI from OpenCV support...");
exit(-1);
}
camera = new rtabmap::CameraOpenNICV(true, 0);
camera = new rtabmap::CameraOpenNICV(true);
}
else if(driver == 5)
{
if(!rtabmap::CameraFreenect2::available())
{
UERROR("Not built with Freenect2 support...");
exit(-1);
}
camera = new rtabmap::CameraFreenect2();
}
else
{
@@ -113,25 +127,36 @@ int main(int argc, char * argv[])
delete camera;
exit(1);
}
cv::Mat rgb, depth;
float fx, fy, cx, cy;
camera->takeImage(rgb, depth, fx, fy, cx, cy);
if(rgb.cols != depth.cols || rgb.rows != depth.rows)
{
UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.",
rgb.cols, rgb.rows, depth.cols, depth.rows);
}
cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
cv::namedWindow("Depth", CV_WINDOW_AUTOSIZE); // create window
pcl::visualization::CloudViewer viewer("cloud");
rtabmap::Transform opticalTransform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
while(!rgb.empty() && !viewer.wasStopped())
{
if(depth.type() == CV_32FC1)
{
depth = rtabmap::util3d::cvtDepthFromFloat(depth);
}
cv::Mat tmp;
depth.convertTo(tmp, CV_8UC1, 255.0/2048.0);
cv::imshow("Video", rgb); // show frame
cv::imshow("Depth",tmp);
cv::imshow("Depth", tmp);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy);
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, opticalTransform);
viewer.showCloud(cloud, "cloud");
if(rgb.cols == depth.cols && rgb.rows == depth.rows)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy);
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, opticalTransform);
viewer.showCloud(cloud, "cloud");
}
int c = cv::waitKey(10); // wait 10 ms or for key stroke
if(c == 27)