ERROR with viso2_ros package: Cannot get submatrix [3..3] x [0..-1] of a (0x0) matrix

Question

Hi everybody, I'm testing viso2_ros package( http://www.ros.org/wiki/viso2_ros# ). I've built a simple application (see below) that take an image of 640x480pixel@BGR format,24depth and try to estimate mono visual odometry.

#include "MonocularOdometry.hpp"
using namespace cv_bridge;
using namespace std;

using namespace sensor_msgs::image_encodings;
VisualOdometryMono *inner_viso;
int32_t dims[3]={0,0,3};//width,height,bytes per pixel
Matrix pose = Matrix::eye(4);
void imageCallback(const sensor_msgs::ImageConstPtr& imageMsg){
    CvImageConstPtr openCvImage = toCvShare(imageMsg,"bgr8");
    IplImage ipl(openCvImage.get()->image);
    if(dims[0]==0){
        dims[0]=openCvImage.get()->image.cols;
        dims[1]=openCvImage.get()->image.rows;
        dims[2]=openCvImage.get()->image.cols*3;
        cout << "dims: "<< dims[0]<<"x"<<dims[1]<< ", " <<dims[2]<<"bpl"<< endl;
    }

    if (inner_viso->process(openCvImage.get()->image.data,&dims[0],false)){
        // on success, update current pose
        pose = pose * Matrix::inv(inner_viso->getMotion());
        // output some statistics
        double num_matches = inner_viso->getNumberOfMatches();
        double num_inliers = inner_viso->getNumberOfInliers();
        cout << ", Matches: " << num_matches;
        cout << ", Inliers: " << 100.0*num_inliers/num_matches << " %" << ", Current pose: " << endl;
    } else {
        cout << " ... failed!" << endl;
    }

}
int main(int argc, char **argv) {
    ros::init(argc, argv, "monocular_visual_odometry");
    ros::NodeHandle nodeHandle;

    VisualOdometryMono::parameters param;
    param.height           = 0.555;//55.5cm
    param.pitch            = -0.523598776;//-30degree
    param.ransac_iters     = 2000;
    param.inlier_threshold = 0.00001;
    param.motion_threshold = 100.0;
    inner_viso=new VisualOdometryMono(param);

    image_transport::ImageTransport it(nodeHandle);
    image_transport::Subscriber sub = it.subscribe("image_topic", 1, imageCallback);


    ros::spin();

    return 0;
}

When I execute it I get the following error:

ERROR: Cannot get submatrix [3..3] x [0..-1] of a (0x0) matrix

This error comes from Matrix::getMat(declared in libviso2/libviso2/src/matrix.cpp) this last is called by VisualOdometryMono::estimateMotion. Can anyone help me to fix it? Regards

---

Originally posted by aldo85ita on ROS Answers with karma: 252 on 2012-08-21

Post score: 0

Answer 1

It seems that you do not pass any camera calibration information to libviso2. Your parameters struct should set focal length and the principal point coordinates.

If you want to use libviso2 directly (without the ROS wrapper), please see the demo file (src/demo.cpp) for an example. If you want to use the ROS wrapper, you should use the mono_odometer node which uses incoming CameraInfo messages to pass the right parameters to libviso2.

---

Originally posted by Stephan with karma: 1924 on 2012-09-05

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by aldo85ita on 2012-09-11:
Thanks @Stephan, If I run mono_odometer and if I set CameraInfo with the following command: rosrun viso2_ros mono_odometer _camera_height:=0.555 _camera_pitch:=-0.523598776 _ransac_iters:=2000 _inlier_threshold:=0.00001 _motion_threshold:=100.0 I'll get the same previous.any suggestions?

Comment by Stephan on 2012-09-11:
With CameraInfo i meant sensor_msgs::CameraInfo, not the parameters camera_pitch and camera_height. If you run rostopic echo /your_camera/camera_info, how does the output look like? Are focal length and calibration matrix K set? Do you undistort your images using the image_proc package?

Comment by aldo85ita on 2012-09-12:
Thank you @Stephan, I fixed the submatrix error setting the right calibration parameters info. now, if I run "rostopic echo /gscam/camera_info", I'll get the right settings: D: [-0.36957, 0.17265000000000003, -0.00128, 0.00119, -0.042850000000000006] K: [0.9999100000000001, 0.00075, -0.01323, etc..

Comment by Astro_sun on 2016-06-13:
@aldo85ita, may i know how you set the calibration parameters info?

Thax