Revision history [back]

Hi, I think that I've found some solution.

The problem apparently is in the initialization process. Since the landmark initialization is delayed (we have to know the first landmarks' 3d position, depth information also, with sufficient accuracy to include it on the map), so we need to adquire two frames with a specific motion and distance between them. Try to move the camera in a pure translation in parallel movement, and try to have sufficient landmarks and make the distance between the keypoints of the two frames sufficiently great. For example, if the landmarks detected are far from the camera, do the movement between the frames sufficient longer and quickly, in order to achieve a better depth computation. However, I'm still having a bad image match, but the program proceeds to calculate pnp in the next steps (you can see that in the program's output below), so I'm assuming that the initialization is done.

Removed 301 projections > 2 pixels error
trans = 5.323930 -3.030132 0.601436
229 points left after sba
finished pose estimation
Dumping tvec in SetPose
-0.691666 
0.722190 
-0.006294 
Dumping rvec in SetPose
0.061715 
0.068404 
-0.289008 
minz = 5.583058, maxz = 193.049911
Frame1: mean z = 19.460421, std z = 12.736793
Frame2: mean z = 19.646008, std z = 12.887172
Input number of matches 964, inliers 39
goodCount 39
filtered inliers: 5
_inliers.size() = 0
[Stereo VO] Inliers: 39  Nodes: 13   Points: 31
[SetupSys] 10 disconnected nodes
[SetupSys] 10 disconnected nodes
[SetupSys] 10 disconnected nodes
[SetupSys] 10 disconnected nodes

******** Bad image match: 

[ INFO] [1315480509.979110334]: In callback, seq = 1092
called PoseEstimator2d::estimate for frames 12 and 13
Number of points: 1331, number of matches: 1035
extractPnPData: frame1.pts.size() = 1331
extractPnPData: good points 35
Running PnP
finished pose estimation
Dumping tvec in SetPose
-0.736821 
0.210926 
-0.235437 
Dumping rvec in SetPose
0.015324 
0.089048 
-0.191074

After that I'm having more problems that I'm trying to solve now. I post the problem here, in case anyone wanted to help:

mono_vslam_node: /home/XXXX/ROSPackages/slam/vslam_system/trunk/posest/src/pe2d.cpp:319: virtual int pe::PoseEstimator2d::estimate(const frame_common::Frame&, const frame_common::Frame&, const std::vector<cv::DMatch, std::allocator<cv::DMatch> >&): Assertion `frame1.pts.size() == frame1.goodPts.size() == frame1.kpts.size()' failed.