Re: Cplusplus Loop Closure Detection

Posted by alexfr on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Cplusplus-Loop-Closure-Detection-tp686p717.html

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;
}