UZ-SLAMLab / ORB_SLAM3

ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM
GNU General Public License v3.0
6.44k stars 2.53k forks source link

when build with ros in gazebo, an error occurred in the function 'setSize' #714

Closed TioeAre closed 1 year ago

TioeAre commented 1 year ago

I tried to build with ros, The following is the result of running in the terminal

~/catkin_ws/scripts > rosrun ORB_SLAM3 Stereo ~/catkin_ws/src/ORB_SLAM3/Vocabulary/ORBvoc.txt ~/catkin_ws/src/ORB_SLAM3/Examples/Stereo/px4_sitl.yaml true /camera/left/image_raw:=/iris_0/stereo_camera/left/image_raw /camera/right/image_raw:=/iris_0/stereo_camera/right/image_raw /orbslam3/vision_pose/pose:=/iris_0/mavros/vision_pose/pose                        

ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
This program comes with ABSOLUTELY NO WARRANTY;
This is free software, and you are welcome to redistribute it
under certain conditions. See LICENSE.txt.

Input sensor was set to: Stereo

Loading ORB Vocabulary. This could take a while...
Vocabulary loaded!

Initialization of Atlas from scratch 
Creation of new map with id: 0
Creation of new map with last KF id: 0
Seq. Name: 

Camera Parameters: 
- Camera: Pinhole
- Image scale: 1
- fx: 376
- fy: 376
- cx: 375.872
- cy: 240
- k1: 0
- k2: 0
- p1: 0
- p2: 0
- fps: 20
- color order: RGB (ignored if grayscale)

Depth Threshold (Close/Far Points): 5.76

ORB Extractor Parameters: 
- Number of Features: 1200
- Scale Levels: 8
- Scale Factor: 1.2
- Initial Fast Threshold: 20
- Minimum Fast Threshold: 7
There are 1 cameras in the atlas
Camera 0 is pinhole
terminate called after throwing an instance of 'cv::Exception'
  what():  OpenCV(4.5.5) /home/tioeare/packages/opencv-4.5.5/modules/core/src/matrix.cpp:250: error: (-215:Assertion failed) s >= 0 in function 'setSize'

Aborted (core dumped)

with debug, I found the error seems to apper in ros_stereo.cc in line 180.

        Sophus::SE3f Tcw_SE3f = mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());

the following is my ros_stereo.cc

/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include <Eigen/Dense>

#include<ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include<opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>

#include"../../../include/System.h"
#include"../../../include/Converter.h"
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <tf/transform_broadcaster.h>
#include <vector>

using namespace std;

class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM){}

    void GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight);

    ORB_SLAM3::System* mpSLAM;
    bool do_rectify;
    cv::Mat M1l,M2l,M1r,M2r;
    ros::Publisher* orb_pub;
    void SetPub(ros::Publisher* pub);
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "RGBD");
    ros::start();

    if(argc != 4)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo path_to_vocabulary path_to_settings do_rectify" << endl;
        ros::shutdown();
        return 1;
    }    

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::STEREO,true);

    ImageGrabber igb(&SLAM);

    stringstream ss(argv[3]);
    ss >> boolalpha >> igb.do_rectify;

    if(igb.do_rectify)
    {      
        // Load settings related to stereo calibration
        cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);
        if(!fsSettings.isOpened())
        {
            cerr << "ERROR: Wrong path to settings" << endl;
            return -1;
        }

        cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
        fsSettings["LEFT.K"] >> K_l;
        fsSettings["RIGHT.K"] >> K_r;

        fsSettings["LEFT.P"] >> P_l;
        fsSettings["RIGHT.P"] >> P_r;

        fsSettings["LEFT.R"] >> R_l;
        fsSettings["RIGHT.R"] >> R_r;

        fsSettings["LEFT.D"] >> D_l;
        fsSettings["RIGHT.D"] >> D_r;

        int rows_l = fsSettings["LEFT.height"];
        int cols_l = fsSettings["LEFT.width"];
        int rows_r = fsSettings["RIGHT.height"];
        int cols_r = fsSettings["RIGHT.width"];

        if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
                rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0)
        {
            cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
            return -1;
        }
        cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,igb.M1l,igb.M2l);
        cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,igb.M1r,igb.M2r);
    }

    ros::NodeHandle nh;

    ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/orbslam2/vision_pose/pose", 1);// /orbslam3/vision_pose/pose
    igb.SetPub(&pose_pub);

    message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/camera/left/image_raw", 1);// /camera/left/image_raw
    message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/camera/right/image_raw", 1);// /camera/right/image_raw
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), left_sub,right_sub);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2));

    ros::spin();

    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt");
    SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt");
    SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt");

    ros::shutdown();

    return 0;
}

void ImageGrabber::SetPub(ros::Publisher* pub)
{
    orb_pub = pub;
}

void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptrLeft;
    cv::Mat Tcw, Rwc, twc;
    double x, y, z; 
    try
    {
        cv_ptrLeft = cv_bridge::toCvShare(msgLeft);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrRight;
    try
    {
        cv_ptrRight = cv_bridge::toCvShare(msgRight);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    if(do_rectify)
    {
        cv::Mat imLeft, imRight;
        cv::remap(cv_ptrLeft->image,imLeft,M1l,M2l,cv::INTER_LINEAR);
        cv::remap(cv_ptrRight->image,imRight,M1r,M2r,cv::INTER_LINEAR);
        Sophus::SE3f Tcw_SE3f = mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
        Eigen::Matrix4f Tcw_Matrix = Tcw_SE3f.matrix();
        cv::eigen2cv(Tcw_Matrix, Tcw);
    }
    else
    {
        Sophus::SE3f Tcw_SE3f = mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());
        Eigen::Matrix4f Tcw_Matrix = Tcw_SE3f.matrix();
        cv::eigen2cv(Tcw_Matrix, Tcw);
    }
    if (Tcw.empty())
        return;
    geometry_msgs::PoseStamped pose;
    pose.header.stamp = cv_ptrLeft->header.stamp;
    pose.header.frame_id ="map";

    Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); // Rotation information
    twc = -Rwc*Tcw.rowRange(0,3).col(3); // translation information
    vector<float> q = ORB_SLAM3::Converter::toQuaternion(Rwc);

    tf::Transform new_transform;
    new_transform.setOrigin(tf::Vector3(twc.at<float>(0, 0), twc.at<float>(0, 1), twc.at<float>(0, 2)));

    tf::Quaternion quaternion(q[0], q[1], q[2], q[3]);
    new_transform.setRotation(quaternion);

    tf::poseTFToMsg(new_transform, pose.pose);
    x = pose.pose.position.x;
    y = pose.pose.position.y;
    z = pose.pose.position.z;
    pose.pose.position.x = z;
    pose.pose.position.y = -x;
    pose.pose.position.z = -y;

    orb_pub->publish(pose);

}

my computer is intel ubuntu20.04 ros:noetic opencv: 4.5.5 eigen:3.4 pangolin:0.8

please tell me how to solve it, thank you very much.

TioeAre commented 1 year ago

Solved, reason is the default opencv of cv_bridge is 4.2 in ros noetic, and the 4.5.5 I installed before compiling orb_slam3 is not compatible. Solution: Create a new workspace to compile your own cv_bridge. Then use set to set cv_bridge to the newly compiled path in the cmakelists.txt in the ros routine of orb_slam3. Reference link: https://github.com/opencv/opencv/issues/21367

https://blog.csdn.net/weixin_43868576/article/details/122688385

https://blog.csdn.net/u014015324/article/details/110850609

https://cyaninfinite.com/ros-cv-bridge-with-python-3/#Dependencies

https://blog.csdn.net/bigdog_1027/article/details/79092263