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

Made Freenect dependency optional

Browse Source 
fixed some toro3d warnings

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1327 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-06-04 20:24:54 +00:00
parent 0d437d22d7
commit bb5e5c0084
10 changed files with 110 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/CameraFreenect.h
View File

@@ -62,6 +62,9 @@ class RTABMAP_EXP FreenectDevice {
class RTABMAP_EXP CameraFreenect : public UEventsSender, public UThread
{
public:
static bool available();
public:
// default local transform z in, x right, y down));
CameraFreenect(int deviceId= 0,

21
corelib/src/CMakeLists.txt
View File

@@ -44,9 +44,26 @@ SET(INCLUDE_DIRS
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${ZLIB_INCLUDE_DIRS}
${Freenect_INCLUDE_DIRS}
)
SET(LIBRARIES
${OpenCV_LIBS}
${PCL_LIBRARIES}
${ZLIB_LIBRARIES}
)
IF(Freenect_FOUND)
ADD_DEFINITIONS("-DWITH_FREENECT")
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${Freenect_INCLUDE_DIRS}
)
SET(LIBRARIES
${LIBRARIES}
${Freenect_LIBRARIES}
)
ENDIF(Freenect_FOUND)
####################################
# Generate resources files
####################################
@@ -83,7 +100,7 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
# Add binary that is built from the source file "main.cpp".
# The extension is automatically found.
ADD_LIBRARY(rtabmap_core ${SRC_FILES} ${RESOURCES_HEADERS})
TARGET_LINK_LIBRARIES(rtabmap_core rtabmap_utilite ${OpenCV_LIBS} ${PCL_LIBRARIES} ${ZLIB_LIBRARIES} ${Freenect_LIBRARIES})
TARGET_LINK_LIBRARIES(rtabmap_core rtabmap_utilite ${LIBRARIES})
INSTALL(TARGETS rtabmap_core
RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime

50
corelib/src/CameraFreenect.cpp
View File

@@ -13,7 +13,9 @@
#include <rtabmap/utilite/UEventsManager.h>
#include <opencv2/imgproc/imgproc.hpp>
#ifdef WITH_FREENECT
#include <libfreenect.h>
#endif
namespace rtabmap {
@@ -32,6 +34,7 @@ FreenectDevice::FreenectDevice(freenect_context * ctx, int index) :
UASSERT(ctx_ != 0);
}
#ifdef WITH_FREENECT
FreenectDevice::~FreenectDevice() {
if(device_ && freenect_close_device(device_) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion");
}
@@ -71,6 +74,21 @@ void FreenectDevice::freenect_video_callback(freenect_device *dev, void *video,
FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
device->VideoCallback(video, timestamp);
}
#else
FreenectDevice::~FreenectDevice() {}
void FreenectDevice::startVideo() {}
void FreenectDevice::stopVideo() {}
void FreenectDevice::startDepth() {}
void FreenectDevice::stopDepth() {}
bool FreenectDevice::init()
{
UERROR("RTAB-Map is not built with Freenect support!");
return false;
}
void FreenectDevice::freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {}
void FreenectDevice::freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {}
#endif
// Do not call directly even in child
void FreenectDevice::VideoCallback(void* _rgb, uint32_t timestamp)
@@ -120,8 +138,17 @@ cv::Mat FreenectDevice::getDepth()
//
// CameraOpenKinect
// CameraFreenect
//
bool CameraFreenect::available()
{
#ifdef WITH_FREENECT
return true;
#else
return false;
#endif
}
CameraFreenect::CameraFreenect(int deviceId, float inputRate, const Transform & localTransform) :
deviceId_(deviceId),
rate_(inputRate),
@@ -131,10 +158,12 @@ CameraFreenect::CameraFreenect(int deviceId, float inputRate, const Transform &
ctx_(0),
freenectDevice_(0)
{
#ifdef WITH_FREENECT
if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
// We claim both the motor and camera devices, since this class exposes both.
// It does not support audio, so we do not claim it.
freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
#endif
}
CameraFreenect::~CameraFreenect()
@@ -146,12 +175,15 @@ CameraFreenect::~CameraFreenect()
delete freenectDevice_;
freenectDevice_ = 0;
}
#ifdef WITH_FREENECT
if(freenect_shutdown(ctx_) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion");
#endif
delete frameRateTimer_;
}
bool CameraFreenect::init()
{
#ifdef WITH_FREENECT
if(!this->isRunning())
{
if(freenectDevice_)
@@ -174,13 +206,16 @@ bool CameraFreenect::init()
}
else
{
UERROR("CameraOpenKinect: No devices connected!");
UERROR("CameraFreenect: No devices connected!");
}
}
else
{
UERROR("CameraOpenKinect: Cannot initialize the camera because it is already running...");
UERROR("CameraFreenect: Cannot initialize the camera because it is already running...");
}
#else
UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
#endif
return false;
}
@@ -200,12 +235,14 @@ void CameraFreenect::mainLoopBegin()
}
else
{
UERROR("CameraOpenKinect: init should be called before starting the camera.");
UERROR("CameraFreenect: init should be called before starting the camera.");
this->kill();
}
}
void CameraFreenect::mainLoop()
{
#ifdef WITH_FREENECT
timeval t;
t.tv_sec = 0;
t.tv_usec = 10000;
@@ -226,13 +263,13 @@ void CameraFreenect::mainLoop()
if(depth.empty())
{
UWARN("CameraOpenKinect: Depth not ready! Try to reduce the image rate to avoid this warning...");
UWARN("CameraFreenect: Depth not ready! Try to reduce the image rate to avoid this warning...");
return;
}
if(rgb.empty())
{
UWARN("CameraOpenKinect: Rgb not ready! Try to reduce the image rate to avoid this warning...");
UWARN("CameraFreenect: Rgb not ready! Try to reduce the image rate to avoid this warning...");
return;
}
@@ -240,6 +277,7 @@ void CameraFreenect::mainLoop()
this->post(new CameraEvent(rgb, depth, constant, localTransform_, ++seq_));
}
}
#endif
}
void CameraFreenect::mainLoopEnd()

4
corelib/src/toro3d/posegraph.hxx
View File

@@ -63,7 +63,7 @@ template <class Ops>
typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::edge(int id1, int id2){
Vertex* v1=vertex(id1);
if (!v1)
return false;
return 0;
typename EdgeList::iterator it=v1->edges.begin();
while(it!=v1->edges.end()){
if ((*it)->v1->id==id1 && (*it)->v2->id==id2)
@@ -117,7 +117,7 @@ typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::removeVertex (int id){
Vertex* v=it->second;
if (v==0)
return false;
return 0;
typename TreePoseGraph<Ops>::EdgeList el=v->edges;
for(typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){

4
corelib/src/toro3d/posegraph3.cpp
View File

@@ -326,7 +326,7 @@ void TreePoseGraph3::collapseEdge(Edge* e){
Transformation T12=e->transformation;
Pose p12=T12.toPoseType();
Transformation iT12=T12.inv();
//Transformation iT12=T12.inv();
//compute the marginal information of the nodes in the path v1-v2-v*
for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
@@ -339,7 +339,7 @@ void TreePoseGraph3::collapseEdge(Edge* e){
//compute the estimate of the vertex based on the path v1-v2-vx
Transformation tr=iT12*T2x;
//Transformation tr=iT12*T2x;
CovarianceMatrix CM=C2x;

8
corelib/src/toro3d/treeoptimizer3_iteration.cpp
View File

@@ -98,7 +98,7 @@ void TreeOptimizer3::computePreconditioner(){
DEBUG(1) << "m";
Edge* e=*it;
Transformation t=e->transformation;
//Transformation t=e->transformation;
InformationMatrix W=e->informationMatrix;
Vertex* top=e->top;
@@ -189,8 +189,8 @@ void TreeOptimizer3::propagateErrors(bool usePreconditioner){
}
//store the transformations relative to the top node
Transformation topTransformation=top->transformation;
Transformation topParameters=top->parameters;
//Transformation topTransformation=top->transformation;
//Transformation topParameters=top->parameters;
//END: Path and weight computation
@@ -258,7 +258,7 @@ void TreeOptimizer3::propagateErrors(bool usePreconditioner){
recomputeTransformations(v2,top);
//BEGIN: Translational Error
Translation topTranslation=top->transformation.translation();
//Translation topTranslation=top->transformation.translation();
Transformation tr12=v1->transformation*e->transformation;
Translation tR=tr12.translation()-v2->transformation.translation();