Cplusplus Loop Closure Detection

[alexfr]

Hi!
I am trying to run this example https://github.com/introlab/rtabmap/wiki/Cplusplus-Loop-Closure-Detection
When I build it I have got this error:

/home/mugiro/alex_package/src/check_point/src/loop_closure_detection.cpp: In function ‘int main(int, char**)’:
/home/mugiro/alex_package/src/check_point/src/loop_closure_detection.cpp:128:14: error: ‘class rtabmap::SensorData’ has no member named ‘imageRaw’
/home/mugiro/alex_package/src/check_point/src/loop_closure_detection.cpp:131:24: error: ‘class rtabmap::SensorData’ has no member named ‘imageRaw’
make[2]: *** [check_point/CMakeFiles/getIDLoopClosure.dir/src/loop_closure_detection.cpp.o] Error 1
make[1]: *** [check_point/CMakeFiles/getIDLoopClosure.dir/all] Error 2
make: *** [all] Error 2
Invoking "make" failed

Does someone have an idea to solve it?

Thanks!

[matlabbe]

Hi,

rtabmap::SensorData::imageRaw() method exists since version 0.10.0 (May 29 2015). Make sure you have at least RTAB-Map 0.10.0 for this example to work. Otherwise, check the old version of the example in your installation directory, which is in rtabmap/examples/BOWMapping/main.cpp.

Note that if you are linking to RTAB-Map ROS Hydro binaries, the version is stuck at 0.8.12. In this case, the corresponding loop closure example is here.

cheers

[alexfr]

Hi,
Thank you for your help now it works.

Tell me if I am wrong but if I understand well, this code keep comparing the image between the sample file and the database (./ros/rtabmap.db) by default until there is no more image available in the samples file. The samples file is use to simulate a camera with the function CameraImage, isn't it ?
I am trying to make my own code. I want to create a node that will take the current image from the kinect (with the function takeImage()) and use getloopclosureID with a data base I made before to get the current pose of the robot.

I just start by initiate my camera and the database path but I get the same error each time I try to catkin it.
here the code :

#include "rtabmap/core/CameraThread.h"
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/Camera.h"
#include <opencv2/core/core.hpp>
#include "rtabmap/utilite/UFile.h"
#include <stdio.h>

#include "std_msgs/String.h"
#include <iostream>
#include <sstream>

#include "ros/ros.h"

using namespace rtabmap;

int main(int argc, char * argv[])
{
  ros::init(argc, argv, "getIDLoopClosure");

  int driver = 0;

  driver=1;

  rtabmap::CameraRGBD * camera = 0;

        rtabmap::Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);

        camera = new rtabmap::CameraOpenNI2("", 0, opticalRotation);

        rtabmap::CameraThread cameraThread(camera);

        if(!cameraThread.init())
        {
                UERROR("Camera init failed!");
                exit(1);
        }

  bool localizationMode = true;
  std::string databasePath = "/home/mugiro/Documents/RTAB-Map/test2.db";

  // Create RTAB-Map
        rtabmap::Rtabmap rtabmap;

        rtabmap.setTimeThreshold(700.0f); // Time threshold : 700 ms, 0 ms means no limit

        rtabmap::ParametersMap parameters;
        parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapLoopThr(), "0.11"));
        parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDEnabled(), "false"));
  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
        parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpIncrementalDictionary(), "false"));
        parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1"));

  rtabmap.init(parameters, databasePath);

  printf("\nDatabase directory \"%s\"\n", databasePath.c_str());

        int countLoopDetected=0;
        int i=0;

  int nextIndex = rtabmap.getLastLocationId()+1;

  printf("\nProcessing images... from directory \"%d\"\n", nextIndex);

  return 0;

}

------------------------------------------------------------------------
and here the error:

CMakeFiles/getIDLoopClosure.dir/src/loop_closure_detection.cpp.o: In function `main':
loop_closure_detection.cpp:(.text+0x17d): undefined reference to `rtabmap::Transform::Transform(float, float, float, float, float, float, float, float, float, float, float, float)'
loop_closure_detection.cpp:(.text+0x1d9): undefined reference to `rtabmap::CameraOpenNI2::CameraOpenNI2(std::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, float, rtabmap::Transform const&)'
loop_closure_detection.cpp:(.text+0x21c): undefined reference to `rtabmap::CameraThread::CameraThread(rtabmap::CameraRGBD*)'
loop_closure_detection.cpp:(.text+0x22c): undefined reference to `rtabmap::CameraThread::init()'
loop_closure_detection.cpp:(.text+0x257): undefined reference to `ULogger::write(ULogger::Level, char const*, int, char const*, char const*, ...)'
loop_closure_detection.cpp:(.text+0x2b3): undefined reference to `rtabmap::Rtabmap::Rtabmap()'
loop_closure_detection.cpp:(.text+0x2c8): undefined reference to `rtabmap::Rtabmap::setTimeThreshold(float)'
loop_closure_detection.cpp:(.text+0x83e): undefined reference to `rtabmap::Rtabmap::init(std::map<std::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&, std::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)'
loop_closure_detection.cpp:(.text+0x883): undefined reference to `rtabmap::Rtabmap::getLastLocationId() const'
loop_closure_detection.cpp:(.text+0x8c7): undefined reference to `rtabmap::Rtabmap::~Rtabmap()'
loop_closure_detection.cpp:(.text+0x8e7): undefined reference to `rtabmap::CameraThread::~CameraThread()'
loop_closure_detection.cpp:(.text+0xbd8): undefined reference to `rtabmap::Rtabmap::~Rtabmap()'
loop_closure_detection.cpp:(.text+0xc02): undefined reference to `rtabmap::CameraThread::~CameraThread()'
CMakeFiles/getIDLoopClosure.dir/src/loop_closure_detection.cpp.o: In function `xn::NodeWrapper::SetHandle(XnInternalNodeData*)':
loop_closure_detection.cpp:(.text._ZN2xn11NodeWrapper9SetHandleEP18XnInternalNodeData[xn::NodeWrapper::SetHandle(XnInternalNodeData*)]+0x38): undefined reference to `xnGetRefContextFromNodeHandle'
loop_closure_detection.cpp:(.text._ZN2xn11NodeWrapper9SetHandleEP18XnInternalNodeData[xn::NodeWrapper::SetHandle(XnInternalNodeData*)]+0x53): undefined reference to `xnContextUnregisterFromShutdown'
loop_closure_detection.cpp:(.text._ZN2xn11NodeWrapper9SetHandleEP18XnInternalNodeData[xn::NodeWrapper::SetHandle(XnInternalNodeData*)]+0x5f): undefined reference to `xnContextRelease'
loop_closure_detection.cpp:(.text._ZN2xn11NodeWrapper9SetHandleEP18XnInternalNodeData[xn::NodeWrapper::SetHandle(XnInternalNodeData*)]+0x6e): undefined reference to `xnProductionNodeRelease'
loop_closure_detection.cpp:(.text._ZN2xn11NodeWrapper9SetHandleEP18XnInternalNodeData[xn::NodeWrapper::SetHandle(XnInternalNodeData*)]+0x81): undefined reference to `xnProductionNodeAddRef'
loop_closure_detection.cpp:(.text._ZN2xn11NodeWrapper9SetHandleEP18XnInternalNodeData[xn::NodeWrapper::SetHandle(XnInternalNodeData*)]+0x90): undefined reference to `xnGetRefContextFromNodeHandle'
loop_closure_detection.cpp:(.text._ZN2xn11NodeWrapper9SetHandleEP18XnInternalNodeData[xn::NodeWrapper::SetHandle(XnInternalNodeData*)]+0xb1): undefined reference to `xnContextRegisterForShutdown'
loop_closure_detection.cpp:(.text._ZN2xn11NodeWrapper9SetHandleEP18XnInternalNodeData[xn::NodeWrapper::SetHandle(XnInternalNodeData*)]+0xc0): undefined reference to `xnContextRelease'
collect2: ld returned 1 exit status
make[2]: *** [/home/mugiro/alex_package/devel/lib/check_point/getIDLoopClosure] Error 1
make[1]: *** [check_point/CMakeFiles/getIDLoopClosure.dir/all] Error 2
make: *** [all] Error 2
Invoking "make" failed




I installed rtabmap using sudo apt-get install. How can I solve the problem?

Thanks so much for your help.

[matlabbe]

Hi,

These are linker errors, the compiler doesn't find RTAB-Map's libraries. As the "rtabmap" package is not really a catkin package, you should use the standard CMake way to link the RTAB-Map's libraries. In your top CMakeLists.txt:

find_package(RTABMap REQUIRED)
include_directories(${RTABMap_INCLUDE_DIRS})

add_executable(my_node src/my_node.cpp)
target_link_libraries(my_node ${RTABMap_LIBRARIES})

Yes, in the example using images from a directory, the CameraImages class reads images until the end of the directory. The CameraImages can be replaced by any Camera classes.

cheers,
Mathieu

[alexfr]

Hi!
Thank you now the problem is solved.
Now I have another question due to my weak level in opencv. I would like to use the function takeImage from the cameraRGBD class but after looking for an answer I have no idea about how to use it (which parameters should I put?).

void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)

Could you give me some advises or tips, I would be very grateful.

thanks.

[matlabbe]

Hi,

This is the old interface, the outputs are the parameters:

CameraOpenni camera;
if(camera.init())
{
   cv::Mat rgb,depth;
   float fx,fy,cx,cy;
   camera.takeImage(rgb, depth, fx, fy, cx, cy);

   if(!rgb.empty())
   {
      // process data...
   }
}

The rtabmap/tools/CameraRGBD/main.cpp (version 0.8) show a simple example of usage.

cheers

[alexfr]

Thanks it works. I just change openni for openni2 because I use an ASUS xtion pro live.
Now I have finished the code but each time i run the node the function getloopclsureID() return 0 like there is no matchng between the database and the current kinect image. But I checked with the gui and I am supposed to get a loop closure.

Do you have any idea how to solve that problem or what is going wrong in my code?
Thanks so much for you support.

here my code:

int main(int argc, char * argv[])
{
  ros::init(argc, argv, "getIDLoopClosure");


   rtabmap::Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);

  rtabmap::CameraOpenNI2 camera;

  if(camera.init())
  {
    cv::Mat rgb,depth;

    float fx,fy,cx,cy;

    camera.takeImage(rgb, depth, fx, fy, cx, cy);

    if(!rgb.empty())
    {
    bool localizationMode = true;
    std::string databasePath = "/home/mugiro/Documents/RTAB-Map/test3.db";

    // Create RTAB-Map
          rtabmap::Rtabmap rtabmap;

          rtabmap.setTimeThreshold(700.0f); // Time threshold : 700 ms, 0 ms means no limit

          rtabmap::ParametersMap parameters;
          parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapLoopThr(), "0.11"));
          parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDEnabled(), "false"));
          parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
          parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpIncrementalDictionary(), "false"));
          parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1"));

    rtabmap.init(parameters, databasePath);
    printf("\nDatabase directory \"%s\"\n", databasePath.c_str());
    int id = rtabmap.getLoopClosureId();
    printf("\nloop closure id = %d\n", id);
    }

  }
  return 0;
}

[matlabbe]

Hi,

You are not calling rtabmap.process() method. Also, a loop closure would not be detected using only 1 image. The loop closure hypotheses need to grow with a sequence of images. In your example, you could try to process more images:

int main(int argc, char * argv[]) 
{ 
  
  // Create RTAB-Map 
  rtabmap::Rtabmap rtabmap; 

  rtabmap.setTimeThreshold(700.0f); // Time threshold : 700 ms, 0 ms means no limit 

  rtabmap::ParametersMap parameters; 
  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapLoopThr(), "0.11")); 
  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDEnabled(), "false")); 
  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false")); 
  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpIncrementalDictionary(), "false")); 
  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1")); 

  std::string databasePath = "/home/mugiro/Documents/RTAB-Map/test3.db"; 
  rtabmap.init(parameters, databasePath); 

  rtabmap::CameraOpenNI2 camera; 
  if(camera.init()) 
  { 
    cv::Mat rgb,depth; 
    float fx,fy,cx,cy; 

    camera.takeImage(rgb, depth, fx, fy, cx, cy); 
    while(!rgb.empty()) 
    {      
       rtabmap.process(rgb);
       
       int id = rtabmap.getLoopClosureId(); 
       printf("\nloop closure id = %d\n", id);

       camera.takeImage(rgb, depth, fx, fy, cx, cy);  
    } 

  } 
  return 0; 
} 

[alex]

Thank you! Your package is really practical and well design and the support you bring it makes it even better !
Cheers!