Odometry pose is showing always null

[rasulnrasul]

Hi,

I am writing a wrapper to use Rtabmap library for SLAM.  I am using Realsense ZR300 and had started with finding the Odometry poses by giving rgb, depth and camera model. But the result from Odometry process is always null. I couldnt find any clue why its failing.
--------------------------------------------------------------------------------------------------------------------
Here is the code skeleton
--------------------------------------------------------------------------------------------------------------------
//initialize Odometry

while(1)
{

// get RGBD images

rtabmap::SensorData data(rgb_mat, depth_mat, cam_model, 0, current_time);
rtabmap::OdometryInfo info;
rtabmap::Transform pose = odometry_->process(dataCpy, &info);

// Pose is always null


}

-------------------------------------------------------------------------------------------
I have enabled the debug and it shows
-------------------------------------------------------------------------------------------

[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:182::computeTransformationImpl() Vis/MinInliers=20
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:183::computeTransformationImpl() Vis/InlierDistance=0.100000
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:184::computeTransformationImpl() Vis/Iterations=100
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:185::computeTransformationImpl() Vis/EstimationType=1
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:186::computeTransformationImpl() Vis/ForwardEstOnly=1
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:187::computeTransformationImpl() Vis/EpipolarGeometryVar=0.020000
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:188::computeTransformationImpl() Vis/PnPReprojError=2.000000
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:189::computeTransformationImpl() Vis/PnPFlags=0
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:190::computeTransformationImpl() Vis/CorType=0
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:191::computeTransformationImpl() Vis/CorFlowWinSize=16
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:192::computeTransformationImpl() Vis/CorFlowIterations=30
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:193::computeTransformationImpl() Vis/CorFlowEps=0.010000
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:194::computeTransformationImpl() Vis/CorFlowMaxLevel=3
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:195::computeTransformationImpl() guess=xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:206::computeTransformationImpl() Input(552): from=0 words, 0 3D words, 0 words descriptors,  0 kpts, 0 kpts3D, 0 descriptors, image=640x480
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:217::computeTransformationImpl() Input(0): to=0 words, 0 3D words, 0 words descriptors, 0 kpts, 0 kpts3D, 0 descriptors, image=0x0
[DEBUG] (2017-10-02 15:07:09.976) RegistrationVis.cpp:236::computeTransformationImpl()
[DEBUG] (2017-10-02 15:07:09.977) util2d.cpp:1146::computeRoi() roi ratios = 0.000000, 0.000000, 0.000000, 0.000000
[DEBUG] (2017-10-02 15:07:09.977) util2d.cpp:1147::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2017-10-02 15:07:09.977) util2d.cpp:1174::computeRoi() roi = 0, 0, 640, 480
[DEBUG] (2017-10-02 15:07:09.982) Features2d.cpp:540::generateKeypoints() Keypoints extraction time = 0.005353 s, keypoints extracted = 54 (mask empty=0)
[DEBUG] (2017-10-02 15:07:09.983) RegistrationVis.cpp:470::computeTransformationImpl()
[DEBUG] (2017-10-02 15:07:09.983) RegistrationVis.cpp:510::computeTransformationImpl() kptsFrom=54
[DEBUG] (2017-10-02 15:07:09.983) RegistrationVis.cpp:511::computeTransformationImpl() kptsTo=0
[DEBUG] (2017-10-02 15:07:09.983) Features2d.cpp:584::generateDescriptors() Descriptors extracted = 37, remaining kpts=37
[DEBUG] (2017-10-02 15:07:09.983) RegistrationVis.cpp:605::computeTransformationImpl() generated kptsFrom3D=37
[DEBUG] (2017-10-02 15:07:09.983) RegistrationVis.cpp:710::computeTransformationImpl() descriptorsFrom=37
[DEBUG] (2017-10-02 15:07:09.983) RegistrationVis.cpp:711::computeTransformationImpl() descriptorsTo=0
[DEBUG] (2017-10-02 15:07:09.983) RegistrationVis.cpp:1474::computeTransformationImpl() transform=xyz=[null] rpy=[null]

Any suggestions on why its failing?

Thank you.

[matlabbe]

Hi,

Can you show more log? Can you also show rgb and depth images that you are feeding to odometry? Are you using CameraReleaseSense class to get images from the ZR300? From no-events rgb-d example:

rtabmap::Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
rtabmap::CameraRealSense camera(0, 0, 0, false, 0, opticalRotation);
camera.init();

while(1) 
{ 
rtabmap::SensorData data = camera->takeImage();
rtabmap::OdometryInfo info; 
rtabmap::Transform pose = odometry_->process(data, &info); 
}

[rasulnrasul]

Hi Mathieu,

I have tried the no-events example way (using process method on OdometryF2M )and It worked. I could see the Odometry working.

Previously I was initializing Odometry using parameters which is similar to ROS.


rtabmap::Odometry * odometry_;
odometry_ = Odometry::create(parameters_);

while()
{
rtabmap::OdometryInfo info;
rtabmap::Transform pose = odometry_->process(dataCpy, &info);
}

EDIT:

I found that error is due to some problem in my camera model

Not working

make K,D,R,P from intrinscis and extrinsics

// return rtabmap::CameraModel(
// "my_camera",
// cv::Size(Rgb_width(), Rgb_height()),
// K, D, R, P,
// localTransform);


Working

        return rtabmap::CameraModel(
                        intrinsics.getFx(), //fx
                        intrinsics.getFy(), //fy
                        intrinsics.getPpx(),  //cx
                        intrinsics.getPpy(),  //cy
                        localTransform,
                        0,
                        cv::Size(scan.Rgb_width(), scan.Rgb_height()));


Thanks.