Ersan11 commented 5 months ago

你好！我用您的代码已经成功的运行了数据集，现在我想要在realsense d435i相机上运行，进行实时的稠密建图。我在自己的尝试中，对ros_rgbd_mapping.cc进行了修改，我注释掉了对数据集处理的部分，订阅的话题换成了相机提供的话题，但是最后在运行时，并没有图像出现。我需要您的帮助，您能给出代码的修改建议么？

lturing commented 5 months ago

感觉应该是ros配置不对。建议先不用ros，试试https://github.com/lturing/ORB_SLAM3_RGBD_PCL

Ersan11 commented 5 months ago

不启动ros的话，没办法与相机完成通讯吧。我的ros_rgbd_mapping.cc，修改思路是：将pulish数据集，改成pulish从相机话题订阅到的图像数据。以下是我修改过的代码。在运行的时候会有这样一个错误：[ERROR] [1711689246.083241407]: cv_bridge exception: [16UC1] is not a color format. but [mono16] is. The conversion does not make sense in depth

`/**

This file is part of ORB-SLAM2.
Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza)
For more information see https://github.com/raulmur/ORB_SLAM2
ORB-SLAM2 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-SLAM2 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-SLAM2. If not, see http://www.gnu.org/licenses/. */

include

include <ros/ros.h>

include <ros/console.h>

include <cv_bridge/cv_bridge.h>

include <tf/transform_listener.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 "System.h"

using namespace std;

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

void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);

private:

ORB_SLAM3::System* mpSLAM;

};

class ImagePub { // for kitti dataset public: ImagePub(ORB_SLAM3::System* pSLAM, image_transport::Publisher& imgPub, imagetransport::Publisher& depPub) :imgPub(imgPub), depPub_(depPub), mpSLAM(pSLAM) {}

    void PubliserImages(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);

    image_transport::Publisher imgPub_, depPub_;

    ORB_SLAM3::System* mpSLAM;

};

int main(int argc, char **argv) { ros::init(argc, argv, "rgbd_mapping"); ros::start(); // ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); ros::Time::init();

if(argc != 3)
{
    cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl;
    ros::shutdown();
    return 1;
}

ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
tf::TransformListener listener;

string voc_dir = argv[2];
string config_dir = argv[1];

// get parameters
bool use_rviz;
private_nh.param("use_rviz", use_rviz, true);

ORB_SLAM3::System SLAM(voc_dir,config_dir,ORB_SLAM3::System::RGBD,!use_rviz);

ImageGrabber igb(&SLAM);

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/aligned_depth_to_color/image_raw", 1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
message_filters::Synchronizer<sync_pol> sync(sync_pol(1), rgb_sub,depth_sub);
sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));

image_transport::ImageTransport img_(nh);
image_transport::Publisher imgPub;
imgPub = img_.advertise("/camera/rgb/image_color", 1);

image_transport::ImageTransport dep_(nh);
image_transport::Publisher depPub;
depPub = dep_.advertise("/camera/depth/image", 1);

ImagePub ipub(&SLAM, imgPub, depPub);

sync.registerCallback(boost::bind(&ImagePub::PubliserImages,&ipub,_1,_2));

ros::spin();

// Stop all threads
SLAM.Shutdown();

// Save camera trajectory
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

ros::shutdown();

return 0;

}

void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD) {

// Copy the ros image message to cv::Mat.
cv_bridge::CvImageConstPtr cv_ptrRGB;
try
{
    cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
}
catch (cv_bridge::Exception& e)
{
    ROS_ERROR("cv_bridge exception: %s in image", e.what());
    return;
}

cv_bridge::CvImagePtr cv_ptrD;
try
{
    cv_ptrD = cv_bridge::toCvCopy(msgD, sensor_msgs::image_encodings::MONO16); 
}
catch (cv_bridge::Exception& e)
{
    ROS_ERROR("cv_bridge exception: %s in depth", e.what());
    return;
}

mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());

}

void ImagePub::PubliserImages(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD) { ros::Rate loop_rate(10);

cv_bridge::CvImageConstPtr cv_ptrRGB;
cv_bridge::CvImagePtr cv_ptrD;

cv::Mat img;
cv::Mat depth;

cv_bridge::CvImage cviImg;
cv_bridge::CvImage cviDepth;

cviImg.header.frame_id = "image";
cviImg.encoding = "bgr8";

cviDepth.header.frame_id = "image";
cviDepth.encoding = "mono16";  //sensor_msgs::image_encodings::MONO16;

while(ros::ok())
{

    cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    img = cv_ptrRGB->image;

    if(img.empty())
    {
        cout << "image read error! in img" << endl;
        return ;
    }

    cv_ptrD = cv_bridge::toCvCopy(msgD,sensor_msgs::image_encodings::MONO16);
    depth = cv_ptrD->image;

    if(depth.empty())
    {
        cout << "image read error! in depth" << endl;
        return ;
    }

    ros::Time time=ros::Time::now();

    cviImg.header.stamp = time;
    cviImg.image = img;

    sensor_msgs::Image imL;
    cviImg.toImageMsg(imL);

    cviDepth.header.stamp = time;
    cviDepth.image = depth;

    sensor_msgs::Image imR;
    cviDepth.toImageMsg(imR);

    imgPub_.publish(imL);
    depPub_.publish(imR);

    ros::spinOnce();
    loop_rate.sleep();
}
return;

}

`

lturing commented 5 months ago

你参考https://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython 试试，检查一下你相机depth的格式。官方的https://github.com/IntelRealSense/realsense-ros 也可以结合着看看

Ersan11 commented 5 months ago

我检查了相机depth的格式：[ INFO] [1711962209.093470445]: Received depth image with encoding: 16UC1 [ INFO] [1711962209.093496242]: Image dimensions: 1280 x 720 是16UC1的格式，在这个报错中，它说mono16是一个彩色图的格式，根据你给的参考中了解到，mono16应该也是一个灰度图的格式。mono16: CV_16UC1, 16-bit grayscale image

当我把指定格式删除后，不会报错了，但是没有图像显示，是不是没有pulish到？ cv_ptrD = cv_bridge::toCvCopy(msgD);

然后我查看了话题的计算图 ![Uploading rosgraph.png…]()

我发现，少了/orb_slam对两个话题返回的箭头，不知道是不是这里的问题。

Ersan11 commented 5 months ago

rosgraph

lturing commented 5 months ago

你参考一下https://github.com/IntelRealSense/realsense-ros 以及谷歌看看，我还没用过realsense相机，也不能调试这个相机

Ersan11 commented 5 months ago

谢谢你的提醒！我觉得我的问题出现在了回调函数，我想问一下，你的建图是先将数据集全部publish到 /camera/rgb/image_color和/camera/depth/image俩个话题，然后在通过回调函数 sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));订阅话题，捕捉图像输送给SLAM吗？

qinqi-mm commented 4 months ago

谢谢你的提醒！我觉得我的问题出现在了回调函数，我想问一下，你的建图是先将数据集全部publish到 /camera/rgb/image_color和/camera/depth/image俩个话题，然后在通过回调函数 sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));订阅话题，捕捉图像输送给SLAM吗？

能请问一下您最后成功实时运行相机了嘛

Ersan11 commented 4 months ago

成功的实现了，对于他修改过的run_rgbd_mapping.sh文件，你直接替换成原来的文件，再把话题换成相机话题就可以了。我现在没有在使用这个稠密建图，我是第一次接触SLAM，我不知道这个地图的数据怎么使用（比如获取地图数据），我觉得我后续的工作可能会用到，如果你了解的话，可以指教一下吗。 ---- 回复的原邮件 ---- | 发件人 | @.> | | 发送日期 | 2024年5月14日 21:51 | | 收件人 | @.> | | 抄送人 | @.> , State @.> | | 主题 | Re: [lturing/ORB_SLAM3_ROS] 怎么实现在相机上运行 (Issue #6) |

谢谢你的提醒！我觉得我的问题出现在了回调函数，我想问一下，你的建图是先将数据集全部publish到 /camera/rgb/image_color和/camera/depth/image俩个话题，然后在通过回调函数 sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));订阅话题，捕捉图像输送给SLAM吗？

能请问一下您最后成功实时运行相机了嘛

— Reply to this email directly, view it on GitHub, or unsubscribe. You are receiving this because you modified the open/close state.Message ID: @.***>

qinqi-mm commented 1 month ago

成功的实现了，对于他修改过的run_rgbd_mapping.sh文件，你直接替换成原来的文件，再把话题换成相机话题就可以了。我现在没有在使用这个稠密建图，我是第一次接触SLAM，我不知道这个地图的数据怎么使用（比如获取地图数据），我觉得我后续的工作可能会用到，如果你了解的话，可以指教一下吗。 ---- 回复的原邮件 ---- | 发件人 | @.> | | 发送日期 | 2024年5月14日 21:51 | | 收件人 | @.> | | 抄送人 | @.> , State @.> | | 主题 | Re: [lturing/ORB_SLAM3_ROS] 怎么实现在相机上运行 (Issue #6) | 谢谢你的提醒！我觉得我的问题出现在了回调函数，我想问一下，你的建图是先将数据集全部publish到 /camera/rgb/image_color和/camera/depth/image俩个话题，然后在通过回调函数 sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));订阅话题，捕捉图像输送给SLAM吗？能请问一下您最后成功实时运行相机了嘛 — Reply to this email directly, view it on GitHub, or unsubscribe. You are receiving this because you modified the open/close state.Message ID: @.***>

很感谢您的回复！抱歉，现在才有时间继续slam的项目，所以才回复。不好意思啊，我也是第一次接触slam，后面也需要得到地图，但是我目前也不知道如何获取地图数据。。如果之后知道了，我再来告诉您。我基础可能差一点，我现在将相机话题改了之后重新编译了，也将sh文件替换了，使用rosrun命令运行是这个嘛：rosrun orb_slam3 ros_rgbd_mapping ORB_SLAM3_ROS/Vocabulary/ORBvoc.txt ORB_SLAM3_ROS/Examples/RGB-D/gemini2.yaml？但是终端会提示：Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings。或许能向您再请教一下嘛？非常感谢！

lturing / ORB_SLAM3_ROS

怎么实现在相机上运行 #6

include

include

include

include

include

include <ros/ros.h>

include <ros/console.h>

include <cv_bridge/cv_bridge.h>

include <tf/transform_listener.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 "System.h"