OAK-D Liteを使って人検出を行う

[ShunjiHashimoto]

• 公式ドキュメント https://docs.luxonis.com/projects/hardware/en/latest/pages/DM9095.html#depth-perception

[ShunjiHashimoto]

DFOV / HFOV / VFOV

[ShunjiHashimoto]

参考にしたい内容

porizouさんのrosで使ってみた記事

URL

https://qiita.com/porizou1/items/b7c10f6e4d3e3cbca6e9

[ShunjiHashimoto]

• realsenseの水平視野角
  • 85.2°
• OAKD-Liteの水平視野角
  • 81.3

[ShunjiHashimoto]

https://github.com/luxonis/depthai-ros/blob/main/README.md これの個々までやった pythonはデフォルト3.6になってたのを2に戻すとrosdepできた

rosdep install --from-paths src --ignore-src -r -y

[ShunjiHashimoto]

ros使って、トピックとして人の位置を出力するデモはできた（ドキュメントどおり） 画面上に人の位置を出力するには以下のサイトのパッケージが必要 https://github.com/luxonis/depthai

[ShunjiHashimoto]

現状

物体検出のコード読んでるなう 画面上に推論結果を出力できないか模索中

ここで、depthの閾値を変更できる yolov4_spatial_publisher.cpp

spatialDetectionNetwork->setDepthLowerThreshold(100);
    spatialDetectionNetwork->setDepthUpperThreshold(5000);

[ShunjiHashimoto]

yolov4_spatial_publisher.cppの

    dai::rosBridge::BridgePublisher<depthai_ros_msgs::SpatialDetectionArray, dai::SpatialImgDetections> detectionPublish(detectionQueue,

を解読中 http://ext-web.edu.sgu.ac.jp/koike/semi/VC2005/c20.html これがクラステンプレートの説明で一番わかり易い

[ShunjiHashimoto]

画像をSubしているところはどこか？？？

[ShunjiHashimoto]

SimMsg = SpatialImgDetections RosMsg = SpatialDetectionArray

[ShunjiHashimoto]

現状

画像はinDataPtrという変数に格納されていそう

        while(opMsgs.size()) {
            RosMsg currMsg = opMsgs.front();
            if(mainSubCount > 0) {
                std::cout << "型：" << typeid(inDataPtr).name() << std::endl;
                // TODO: 画像処理のコードを追記する
                // NOTE: 画像に人のバウンディングボックスと確率をOpenCVを使って表示する
                // TODO: 画像はどこから取得するのか調べる
                // SpatialImgDetectionsクラスで画像をもとに推論してそう
                _rosPublisher->publish(currMsg);
            }

frame = in_rgb.getCvFrame()

のようにgetCvFrame関数を使って画像を取得できないかな inDataPtrの型を調べたところこんな感じだった

型：St10shared_ptrIN3dai8ImgFrameEE

今後について

こちらを参考に画像にバウンディングボックスを表示して出力する方法を模索中

[ShunjiHashimoto]

参考にしたい内容

画像のバウンディングボックスを表示するサンプルコード

URL

https://docs.luxonis.com/projects/api/en/latest/samples/SpatialDetection/spatial_tiny_yolo/#:~:text=auto%20detections%20%3D%20inDet,0%3B%0A%20%20%20%20%20%20%20%20%7D%0A%20%20%20%20%7D%0A%20%20%20%20return%200%3B

[ShunjiHashimoto]

現状

こんな感じでcolorQueueをCVの画像データに変換しようとしたが、OpenCvがサポートしていないとかなんちゃらで変換できなかった

    auto imgFrame = colorQueue->get<dai::ImgFrame>();
    std::cout << "型：" << typeid(imgFrame->getCvFrame()).name() << std::endl;
    cv::Mat frame = imgFrame->getCvFrame();

/usr/local/include/depthai/pipeline/datatype/ImgFrame.hpp:199:9: error: static assertion failed: Library not configured with OpenCV support
         static_assert(dependent_false<T...>::value, "Library not configured with OpenCV support");

この行でcolorQueueをCVの画像データに変換している なので、このtoRosMsg後に得られたcolorデータを画像処理していけばよいのでは？

if(planarEncodingEnumMap.find(inData->getType()) != planarEncodingEnumMap.end()) {
        // cv::Mat inImg = inData->getCvFrame();
        cv::Mat mat, output;
        cv::Size size = {0, 0};
        int type = 0;
        switch(inData->getType()) {
            case dai::RawImgFrame::Type::BGR888p:
            case dai::RawImgFrame::Type::RGB888p:
                size = cv::Size(inData->getWidth(), inData->getHeight());
                type = CV_8UC3;
                break;
            case dai::RawImgFrame::Type::YUV420p:
            case dai::RawImgFrame::Type::NV12:
                size = cv::Size(inData->getWidth(), inData->getHeight() * 3 / 2);
                type = CV_8UC1;
                break;

            default:
                std::runtime_error("Invalid dataType inputs..");
                break;
        }
        mat = cv::Mat(size, type, inData->getData().data());

今後について

[ShunjiHashimoto]

参考にしたい内容

以下の関数のstd::bindがわからなかったが、std::bind(使用する関数, classのインスタンスメソッド, 使用する関数の引数１, 引数２)

    auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, 416, 416);
    rgbPublish = std::make_unique<dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame>>(colorQueue, 
                                                                                     pnh, 
                                                                                     std::string("color/image"),
                                                                                     std::bind(&dai::rosBridge::ImageConverter::toRosMsg, 
                                                                                     &rgbConverter, // since the converter has the same frame name
                                                                                                      // and image type is also same we can reuse it
                                                                                     std::placeholders::_1, 
                                                                                     std::placeholders::_2) , 
                                                                                     30,
                                                                                     rgbCameraInfo,
                                                                                     "color");

URL

https://qiita.com/tkyaji/items/1aa7bb01658e848d3ef4 https://qiita.com/tkyaji/items/1aa7bb01658e848d3ef4#:~:text=%E3%81%A1%E3%81%AA%E3%81%BF%E3%81%AB%E3%82%A4%E3%83%B3%E3%82%B9%E3%82%BF%E3%83%B3%E3%82%B9%E3%83%A1%E3%82%BD%E3%83%83%E3%83%89%E3%82%92%E6%8C%87%E5%AE%9A%E3%81%97%E3%81%9F%E3%81%84%E5%A0%B4%E5%90%88%E3%81%AF%E3%80%81std%3A%3Abind%E3%81%AE%E7%AC%AC2%E5%BC%95%E6%95%B0%E3%81%AB%E3%82%A4%E3%83%B3%E3%82%B9%E3%82%BF%E3%83%B3%E3%82%B9%E3%82%92%E6%B8%A1%E3%81%97%E3%81%BE%E3%81%99%E3%80%82

[ShunjiHashimoto]

[ShunjiHashimoto]

Bride内で、もしRosMsgがrgb系ならdetection結果をもとに描画するというif文を追加すればできそうだが、あまり綺麗ではないよね。 せっかく、Bridge前で分けられているのに、またBrideで分岐処理をするのはなんか違う気がする でも、逆にcolorQueueの段階で画像処理しようとすると、OpenCVの型に変換できないため詰む RosMsgに変換された後に画像処理しようにもBride内で完結されているため、分岐処理を書かないといけない

[ShunjiHashimoto]

なので、今回は、シンプルにC++でconvert_deteced_img.cppみたいなコードを書いて、 imgをSubして、それを画像処理してまた違うトピックとしてPubする

[ShunjiHashimoto]

Subscriberまで書いたからあとはPublisherを書く

[ShunjiHashimoto]

現状

以下のように書いた。 無事、推論結果を出力できた。

#include "ros/ros.h"
#include <sensor_msgs/Image.h>

#include <opencv2/opencv.hpp>
#include <depthai_bridge/ImageConverter.hpp>
#include <depthai_bridge/SpatialDetectionConverter.hpp>

// Inludes common necessary includes for development using depthai library

dai::rosBridge::ImageConverter inputConverter(true);
cv::Mat rgbImage;
depthai_ros_msgs::SpatialDetectionArray detectArray;

void rgbCallback(const sensor_msgs::ImagePtr& rgbImageMsg){
    try {
        rgbImage = cv_bridge::toCvCopy(rgbImageMsg, sensor_msgs::image_encodings::BGR8)->image;
        }
    catch (cv_bridge::Exception& e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
    }
}

void detectCallback(const depthai_ros_msgs::SpatialDetectionArrayPtr& detectMsg){
    detectArray = *detectMsg;
    return;
}

int main(int argc, char** argv){
    ros::init(argc, argv, "rgb_subscriber_node");
    ros::NodeHandle nh;

    ros::Subscriber rgb_sub = nh.subscribe("/yolov4_publisher/color/image", 5, rgbCallback);
    ros::Subscriber detection_sub = nh.subscribe("/yolov4_publisher/color/yolov4_Spatial_detections", 5, detectCallback);
    auto color = cv::Scalar(255, 255, 255);
    ros::Rate loop_rate(10);

    while(ros::ok()){
        for(const auto& detection : detectArray.detections) {
            int x1 = detection.bbox.center.x - detection.bbox.size_x/2;
            int y1 = detection.bbox.center.y - detection.bbox.size_y/2;
            int x2 = detection.bbox.center.x + detection.bbox.size_x/2;
            int y2 = detection.bbox.center.y + detection.bbox.size_y/2;

            std::string labelStr = detection.tracking_id;
            cv::putText(rgbImage, labelStr, cv::Point(x1 + 10, y1 + 35), cv::FONT_HERSHEY_TRIPLEX, 5.0, 255);

            std::stringstream confStr;
            confStr << std::fixed << std::setprecision(2) << detection.results[0].score * 100;
            cv::putText(rgbImage, confStr.str(), cv::Point(x1 + 10, y1 + 35), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255);

            std::stringstream depthX;
            depthX << "X: " << (double)detection.position.x * 1000 << " mm";
            cv::putText(rgbImage, depthX.str(), cv::Point(x1 + 10, y1 + 50), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255);
            std::stringstream depthY;
            depthY << "Y: " << (double)detection.position.y * 1000 << " mm";
            cv::putText(rgbImage, depthY.str(), cv::Point(x1 + 10, y1 + 65), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255);
            std::stringstream depthZ;
            depthZ << "Z: " << (double)detection.position.z * 1000 << " mm";
            cv::putText(rgbImage, depthZ.str(), cv::Point(x1 + 10, y1 + 80), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255);

            cv::rectangle(rgbImage, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX);
        }
        if(!rgbImage.empty()) {
            cv::imshow("rgb", rgbImage);
            cv::waitKey(1);
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

今後について

次は、Jetsonで環境構築して、出力させてみる

[ShunjiHashimoto]

参考にしたい内容

rosの画像からOpenCVの画像に変換するコード

URL

https://sy-base.com/myrobotics/ros/ros_cvbridge/

[ShunjiHashimoto]

Jetson用にGitHubにフォークしたコードをpushしておいた あとは、これらをcloneしてビルドして動作確認するだけだ

[ShunjiHashimoto]

同じ方法で、Jetsonでもビルドできた！ ビルド時間は30分くらいかかったが

[ShunjiHashimoto]

次は、Rviz上に推論結果を出すのと、せっかくpositionも出てるので、Marker使って推論した物体をworld座標かなんかで表示するのをやってみたい、今日やる

[ShunjiHashimoto]

何をしたか？

実行コマンド

cm

エラー文

Errors     << depthai_examples:make /home/hashimoto/catkin_ws/logs/depthai_examples/build.make.020.log                                                                                                     
/home/hashimoto/catkin_ws/src/hs_ros/depthai-ros-examples/depthai_examples/ros1_src/rgb_video_subscriber.cpp: In function ‘int main(int, char**)’:
/home/hashimoto/catkin_ws/src/hs_ros/depthai-ros-examples/depthai_examples/ros1_src/rgb_video_subscriber.cpp:94:52: error: invalid initialization of reference of type ‘depthai_ros_msgs::SpatialDetectionPtr& {aka boost::shared_ptr<depthai_ros_msgs::SpatialDetection_<std::allocator<void> > >&}’ from expression of type ‘depthai_ros_msgs::SpatialDetection_<std::allocator<void> >’
                 marker_pub.publish(markerPublisher(detection));
                                                    ^~~~~~~~~
/home/hashimoto/catkin_ws/src/hs_ros/depthai-ros-examples/depthai_examples/ros1_src/rgb_video_subscriber.cpp:29:6: note: in passing argument 1 of ‘auto markerPublisher(depthai_ros_msgs::SpatialDetectionPtr&)’
 auto markerPublisher(depthai_ros_msgs::SpatialDetectionPtr& spatialdetected){
      ^~~~~~~~~~~~~~~
make[2]: *** [CMakeFiles/rgb_subscriber_node.dir/ros1_src/rgb_video_subscriber.cpp.o] Error 1
make[1]: *** [CMakeFiles/rgb_subscriber_node.dir/all] Error 2
make: *** [all] Error 2
cd /home/hashimoto/catkin_ws/build/depthai_examples; catkin build --get-env depthai_examples | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -

解決策

• ‘depthai_ros_msgs::SpatialDetectionPtr& {aka boost::shared_ptr<depthai_rosmsgs::SpatialDetection<std::allocator > >&}
• depthai_rosmsgs::SpatialDetection<std::allocator >

これらの型が異なっているのが問題 関数の引数のPtrをなくすことで解決

marker_pub.publish(markerPublisher(detection));

ここでは、Ptrではなくアドレス演算子が宣言された変数やただの変数を渡す一方で、関数の定義ではPtrがついた変数が来ることが想定されていた。それを一致させるように編集した 4e0b8db9c7dbfe6c29a47e06ec2d343f94cec8be

[ShunjiHashimoto]

Rviz上にマーカーを表示させられたが、TFの軸とカメラの軸がイマイチで動きが連動していない、見栄えが良くない 直す

[ShunjiHashimoto]

現状

推定結果をMarkerではなく、3Dモデルを使って表示できるようにした、またTFの軸も修正した 734d287c6e756c7376ee9bd3d003d1658009dcef

今後について

実験動画を撮影後、Twitterに投稿 それで今回のissueはcloseする

[ShunjiHashimoto]

Twitterに投稿した！ https://twitter.com/23232871hashimo/status/1511643626555822082

[ShunjiHashimoto]

実行コマンド $ roslaunch depthai_e xamples yolov4_publisher.launch