ERROR with viso2_ros package: Cannot get submatrix [3..3] x [0..-1] of a (0x0) matrix
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