ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

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

asked 2012-08-21 05:06:05 -0600

aldo85ita gravatar image

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2012-09-05 00:14:28 -0600

Stephan gravatar image

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.

edit flag offensive delete link more

Comments

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?

aldo85ita gravatar image aldo85ita  ( 2012-09-11 02:39:01 -0600 )edit

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?

Stephan gravatar image Stephan  ( 2012-09-11 02:52:35 -0600 )edit

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..

aldo85ita gravatar image aldo85ita  ( 2012-09-12 22:38:45 -0600 )edit

@aldo85ita, may i know how you set the calibration parameters info?

Thax

Astro_sun gravatar image Astro_sun  ( 2016-06-13 23:09:11 -0600 )edit

Question Tools

Stats

Asked: 2012-08-21 05:06:05 -0600

Seen: 578 times

Last updated: Sep 10 '12