Detect each frame of image

kankanzheli · July 2, 2022, 8:20am

I now have the SVO video data I have taken. My goal is to obtain RGB images from SVO files through ROS and carry out image processing related work. What I want to achieve is similar to reducing the speed of SVO file output image, so that each frame of image in SVO file can get the result of image detection. (the image must be transmitted through the topic of ROS)

Myzhar · July 2, 2022, 12:56pm

Hi @kankanzheli
You can modify the code of the ZED Nodelet to achieve this goal.

First of all you must force the SVO real-time mode to false here:

github.com

stereolabs/zed-ros-wrapper/blob/master/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp#L182

    
      
          mDepthCamInfoMsg.reset(new sensor_msgs::CameraInfo());
          
          
// Initialization transformation listener
          mTfBuffer.reset(new tf2_ros::Buffer);
          mTfListener.reset(new tf2_ros::TransformListener(*mTfBuffer));
          
          
// Try to initialize the ZED
          if (!mSvoFilepath.empty() || !mRemoteStreamAddr.empty()) {
              if (!mSvoFilepath.empty()) {
                  mZedParams.input.setFromSVOFile(mSvoFilepath.c_str());
                  mZedParams.svo_real_time_mode = true;
          
          
        // TODO ADD PARAMETER FOR SVO REAL TIME
              } else if (!mRemoteStreamAddr.empty()) {
                  std::vector<std::string> configStream = sl_tools::split_string(mRemoteStreamAddr, ':');
                  sl::String ip = sl::String(configStream.at(0).c_str());
          
          
        if (configStream.size() == 2) {
                      mZedParams.input.setFromStream(ip, atoi(configStream.at(1).c_str()));
                  } else {
                      mZedParams.input.setFromStream(ip);

Then you must change the sleep period at the end of the grab loop to get the frequency that you need.
Here

github.com

stereolabs/zed-ros-wrapper/blob/master/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp#L3779

    
      
                      NODELET_DEBUG("*******************************");
          #endif
                      std::chrono::steady_clock::time_point end_elab = std::chrono::steady_clock::now();
          
          
            double elab_usec = std::chrono::duration_cast<std::chrono::microseconds>(end_elab - start_elab).count();
          
          
            double mean_elab_sec = mElabPeriodMean_sec->addValue(elab_usec / 1000000.);
          
          
            static int count_warn = 0;
          
          
            if (!loop_rate.sleep()) {
                          if (mean_elab_sec > (1. / mCamFrameRate)) {
                              if (++count_warn > 10) {
                                  NODELET_DEBUG_THROTTLE(1.0, "Working thread is not synchronized with the Camera frame rate");
                                  NODELET_DEBUG_STREAM_THROTTLE(1.0, "Expected cycle time: " << loop_rate.expectedCycleTime() << " - Real cycle time: " << mean_elab_sec);
                                  NODELET_WARN_STREAM_THROTTLE(10.0, "Elaboration takes longer (" << mean_elab_sec << " sec) than requested "
                                                                                                                      "by the FPS rate ("
                                                                                                  << loop_rate.expectedCycleTime() << " sec). Please consider to "
                                                                                                                                      "lower the 'frame_rate' setting or to "
                                                                                                                                      "reduce the power requirements reducing "
                                                                                                                                      "the resolutions.");

kankanzheli · July 3, 2022, 5:09pm

I don’t quite understand what you mean.
I changed the mZedParams.svo_real_time_mode = true; to mZedParams.svo_real_time_mode = false;
I changed if (!loop_rate.sleep()) { to if (!loop_rate.sleep(10000)) {

Then, I running: roslaunch zed_wrapper zedm.launch svo_file:=/home//Data/my_data.svo
At this time, I can see from rviz that the obtained left view still outputs the image at a very fast speed. It still seems to work at 60fps/s

I want to get an image similar to 1 second output. The first frame of image is output in the first second. Output the second frame image in SVO file next second. In this way, every frame image in SVO can be workd with image detection method.

Myzhar · July 4, 2022, 8:23am

This is not the right way. You must set the loop_rate frequency to the required FPS here:

github.com

stereolabs/zed-ros-wrapper/blob/master/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp#L3169


      
          }
          lastZedTs = grab_ts;
          // <---- Check if a grab has been done before publishing the same images
          
          // Publish the left = rgb image if someone has subscribed to
          if (leftSubnumber > 0)
          {
            sensor_msgs::ImagePtr leftImgMsg = boost::make_shared<sensor_msgs::Image>();
            publishImage(leftImgMsg, mat_left, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp);
          }
          if (rgbSubnumber > 0)
          {
            sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared<sensor_msgs::Image>();
            publishImage(rgbImgMsg, mat_left, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp);
          }
          
          // Publish the left = rgb GRAY image if someone has subscribed to
          if (leftGraySubnumber > 0)
          {
            sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared<sensor_msgs::Image>();
            publishImage(leftGrayImgMsg, mat_left_gray, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp);

kankanzheli · July 6, 2022, 4:37am

You means I should change the following part?
ros::Rate loop_rate(mCamFrameRate); into ros::Rate loop_rate(1);
mZedParams.svo_real_time_mode = true; into mZedParams.svo_real_time_mode = false;

Only this two part? Because when I check the output by rviz, I found the output fps still is 60. It is not changed.

Myzhar · July 6, 2022, 9:28am

Where are you getting such frequency information in Rviz?

The correct way to check for a topic frequency is by using the “Topic Monitor” plugin in rqt or the CLI command rostopic hz <topic_name>

kankanzheli · July 9, 2022, 3:26am

It’s actually very simple. View the output image of topic through rviz. It is found that the output speed does not change after changing FPS. Theoretically, you should see the next frame of image one second after FPS is 1. But the reality is that the output image does not realize one frame per second.

Myzhar · July 11, 2022, 1:03pm

Modifying the ZED Wrapper in order to change the default behaviors is not straightforward.
I think that it’s easier to write a simple node that does what you need leaving behind all the advanced features that you do not need.