Re: RTABMAP standalone for localization in xyz (Kinect Xbox 360)

Posted by rit2014006 on
URL: http://official-rtab-map-forum.206.s1.nabble.com/RTABMAP-standalone-for-localization-in-xyz-Kinect-Xbox-360-tp2706p2781.html

#include <rtabmap/core/RegistrationVis.h>
#include <rtabmap/core/CameraRGBD.h>
#include <rtabmap/core/util3d_transforms.h>
#include <iostream>
using namespace rtabmap;

int main(int argc, char * argv[])
{

//CameraFreenect camera; // Assume Kinect for XBOX 360
//camera.setImageRate(1); // capture at 1 Hz

CameraRGBDImages camera("/home/rahul/uimages/rgb/", "/home/rahul/uimages/depth/");
//Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
//camera.setLocalTransform(opticalRotation);
camera.init(".", "calibration");
/*if(camera.init(".", "calibration")) // use "calibration.yaml" in the current directory
{
    
}*/

//Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
//camera.setLocalTransform(opticalRotation); // from camera frame to rtabmap/ros frame
if(!camera.init())
{
   printf("Failed to initialize the camera!\n");
   return -1;
}

RegistrationVis registration;

SensorData frame = camera.takeImage();
SensorData previousFrame = frame;

int k = 1;
cv::Point3f p(0,0,0);

while(frame.isValid())
{
   Transform t = registration.computeTransformation(previousFrame, frame); 
   
   if(t.isNull())
   {
      printf("Could not compute motion\n");
   }
   else
   {
	p = util3d::transformPoint(p,t);
	std::cout<<p<<"\n";
   /*     Transform t2 = t1;
	t2 = t2.to3DoF();
	t1 = t1 * t;      
	//printf("Motion: %s\n\n%s\n", t.prettyPrint().c_str(),t1.prettyPrint().c_str());
	 printf("%d Motion: %s\n", k,t2.prettyPrint().c_str());*/
   }
   previousFrame = frame;
   frame = camera.takeImage();
   k++;   
}

return 0;
}

 
The above code is returning wrong output if i am trying to localize and why are we using an optical rotation matrix and it is giving wrong output with and without it can you please explain what is wrong with my code. Thank you :)