Hello,
I am trying to run the stereo visual odometry sample with my custom stereo camera and I get the following errors :
ERROR packages/perception/StereoVisualOdometry.cpp@41: Reading input image from proto buffer failed.
ERROR ./messages/image.hpp@83: Number of channels does not match. Proto provides 0 channels while image expected 1 channels.
My code have 2 codelets : one to get rgb to grey and one to set intrinsincs; there is a right and a left version of them but they are almost the same.
The stereo_matching_depth sample have a Extrinsincs input but svo do not have that, how can I set the extinsincs param in SVO ?
My error seems to come from my codelet that process images, could you try to spot my error please ?
This is the data flow : v4l2 camera → deepstream GStreamer pipeline → CUDA crop → set intrinsincs → RGB to Grey → SVO
Thank you !
This is the job statistics :
|=====================================================================================================================|
| Job Statistics Report (regular) |
|=====================================================================================================================|
| Name | Count | Time (Median - 90% - Max) [ms] | Load (%) | Late (%) |
|---------------------------------------------------------------------------------------------------------------------|
| isaac.alice.BufferAllocatorReport | 4 | 0.00 | 0.00 | 0.01 | 0.0 % | 0.0 % |
| isaac.alice.ConfigBridge | 0 | 0.00 | 0.00 | -inf | 0.0 % | 0.0 % |
| InteractiveMarkersBridge | 234 | 0.01 | 0.01 | 0.26 | 0.0 % | 0.0 % |
| isaac.alice.LifecycleReport | 47 | 0.00 | 0.01 | 4.71 | 0.0 % | 0.0 % |
| isaac.alice.MessagePassingReport | 4 | 3.97 | 4.58 | 8.22 | 0.1 % | 0.0 % |
| PoseTreeJsonBridge | 234 | 0.02 | 0.05 | 0.20 | 0.0 % | 0.0 % |
| NodeStatistics | 12 | 0.15 | 0.27 | 0.40 | 0.0 % | 0.0 % |
| isaac.viewers.ColorCameraViewer | 317 | 0.01 | 0.02 | 6.79 | 0.1 % | 0.0 % |
| crop1 | 317 | 4.85 | 5.56 | 143.77 | 9.7 % | 0.0 % |
| crop2 | 316 | 4.27 | 5.35 | 10.45 | 7.3 % | 0.0 % |
| isaac.viewers.ImageKeypointViewer | 0 | 0.00 | 0.00 | -inf | 0.0 % | 0.0 % |
| INTL | 317 | 3.48 | 4.62 | 14.96 | 6.9 % | 0.0 % |
| INTR | 316 | 2.91 | 3.50 | 12.36 | 5.6 % | 0.0 % |
| RGB2GRAYL | 317 | 1.52 | 2.77 | 8.06 | 3.2 % | 0.0 % |
| RGB2GRAYR | 316 | 0.79 | 1.11 | 9.89 | 1.8 % | 0.0 % |
| StereoVisualOdometry | 316 | 0.05 | 0.09 | 0.82 | 0.2 % | 0.0 % |
| WebsightServer | 2270 | 5.07 | 5.10 | 14.98 | 65.1 % | 0.0 % |
|=====================================================================================================================|
This is my codelet to convert rgb to grey :
#include "RGB2GREYL.hpp"
#include <memory>
#include <utility>
#include "engine/core/assert.hpp"
#include "engine/gems/image/conversions.hpp"
#include "engine/gems/image/utils.hpp"
#include "messages/camera.hpp"
#include "messages/math.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/imgproc.hpp"
namespace isaac {
namespace opencv {
void RGB2GREYL::start() {
tickOnMessage(rx_input_imageL());
}
void RGB2GREYL::tick() {
auto input = rx_input_imageL().getProto();
ImageConstView3ub input_image;
bool ok = FromProto(input.getImage(), rx_input_imageL().buffers(), input_image);
ASSERT(ok, "Failed to deserialize the input image");
const size_t rows = input_image.rows();
const size_t cols = input_image.cols();
Image1ub output_image(rows, cols);
cv::Mat image = cv::Mat(rows, cols, CV_8UC3, const_cast<void*>(static_cast<const void*>(input_image.data().pointer())));
cv::Mat gradient = cv::Mat(rows, cols, CV_8U, static_cast<void*>(output_image.data().pointer()));
cv::cvtColor(image, gradient, cv::COLOR_RGB2GRAY);
auto output = tx_output_imageL().initProto();
auto in_pinhole = input.getPinhole();
const Vector2d focal = FromProto(in_pinhole.getFocal());
const Vector2d center = FromProto(in_pinhole.getCenter());
auto out_pinhole = output.initPinhole();
ToProto(focal, out_pinhole.getFocal());
ToProto(center, out_pinhole.getCenter());
out_pinhole.setCols(in_pinhole.getCols());
out_pinhole.setRows(in_pinhole.getRows());
if (input.hasDistortion()) {
auto in_distortion = input.getDistortion();
auto out_distortion = output.initDistortion();
auto distortion_coeff = FromProto(in_distortion.getCoefficients());
out_distortion.setModel(in_distortion.getModel());
ToProto(distortion_coeff, out_distortion.getCoefficients());
}
output.setColorSpace(ColorCameraProto::ColorSpace::GRAYSCALE);
ToProto(std::move(output_image), output.getImage(), tx_output_imageL().buffers());
tx_output_imageL().publish(rx_input_imageL().acqtime());
}
} // namespace opencv
} // namespace isaac
This is my codelet to set intrinsincs :
#include "INTL.hpp"
#include <memory>
#include <utility>
#include "engine/core/assert.hpp"
#include "engine/gems/image/conversions.hpp"
#include "engine/gems/image/utils.hpp"
#include "messages/camera.hpp"
#include "messages/math.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/imgproc.hpp"
namespace isaac {
void INTL::start() {tickOnMessage(rx_inLcam());}
void INTL::tick() {
//CAM PROTO
auto outL = tx_outLcam().initProto();
// Pinhole
auto pinholeL = outL.initPinhole();
ToProto(Vector2d(3147.54098361, 3160.49382716), pinholeL.getFocal());//1280*12/4.86 & 960*12/3.66 Foc len [8.525929042561033e+02,8.549701455135702e+02]
ToProto(Vector2d(519.4719202793973, 693.0736876181506), pinholeL.getCenter());//principal point [6.930736876181506e+02,5.194719202793973e+02]
pinholeL.setCols(1280);
pinholeL.setRows(960);
// Distortion
auto distortionL = outL.initDistortion();
distortionL.setModel(DistortionProto::DistortionModel::BROWN);
Vector5d disto;
disto << 0, 0, 0, 0, 0;
ToProto(disto, distortionL.getCoefficients());
//Color
outL.setColorSpace(ColorCameraProto::ColorSpace::RGB);
//Get Image
auto inL= rx_inLcam().getProto();
ImageConstView3ub inLcam;
bool okL = FromProto(inL.getImage(), rx_inLcam().buffers(), inLcam); ASSERT(okL, "Failed to deserialize the input image L");
//Copy Image
const size_t rows = inLcam.rows();
const size_t cols = inLcam.cols();
Image3ub outLcam(rows, cols);
cv::Mat in_imageL = cv::Mat(rows, cols, CV_8UC3, const_cast<void*>(static_cast<const void*>(inLcam.data().pointer())));
cv::Mat out_imageL = cv::Mat(rows, cols, CV_8UC3, const_cast<void*>(static_cast<const void*>(outLcam.data().pointer())));
in_imageL.copyTo(out_imageL);
//Publish CamProto
ToProto(std::move(outLcam), outL.getImage(), tx_outLcam().buffers());
tx_outLcam().publish(rx_inLcam().acqtime());
//INT PROTO
auto intout = tx_outLint().initProto();
// Pinhole
auto intpinholeL = intout.initPinhole();
ToProto(Vector2d(3147.54098361, 3160.49382716), intpinholeL.getFocal());//1280*12/4.86 & 960*12/3.66 Foc len [8.525929042561033e+02,8.549701455135702e+02]
ToProto(Vector2d(519.4719202793973, 693.0736876181506), intpinholeL.getCenter());//principal point [6.930736876181506e+02,5.194719202793973e+02]
intpinholeL.setCols(1280);
intpinholeL.setRows(960);
// Distortion
auto intdistortionL = intout.initDistortion();
intdistortionL.setModel(DistortionProto::DistortionModel::BROWN);
ToProto(disto, intdistortionL.getCoefficients());
//Publish
tx_outLint().publish(rx_inLcam().acqtime());
//LOG_INFO("INT L OK \n");
}
} // namespace isaac
This is my JSON :
{
"name": "stereo_vo",
"modules": [
"//apps/samples/stereo_vo:rgb2grey",
"//apps/samples/stereo_vo:intext",
"//packages/perception",
"perception:stereo_visual_odometry",
"sensors:v4l2_camera",
"deepstream",
"message_generators",
"viewers",
"deepstream",
"sight",
"utils"
],
"graph": {
"nodes": [
{
"name": "caminputn",
"components": [
{
"name": "message_ledger",
"type": "isaac::alice::MessageLedger"
},
{
"name": "pipeline",
"type": "isaac::deepstream::Pipeline"
}
]
},
{
"name": "cropn",
"components": [
{
"name": "message_ledger",
"type": "isaac::alice::MessageLedger"
},
{
"name": "crop1",
"type": "isaac::perception::CropAndDownsampleCuda"
},
{
"name": "crop2",
"type": "isaac::perception::CropAndDownsampleCuda"
}
]
},
{
"name": "intextn",
"components": [
{
"name": "isaac.alice.MessageLedger",
"type": "isaac::alice::MessageLedger"
},
{
"name": "INTL",
"type": "isaac::INTL"
},
{
"name": "INTR",
"type": "isaac::INTR"
}
]
},
{
"name": "rgbtogreyn",
"components": [
{
"name": "message_ledger",
"type": "isaac::alice::MessageLedger"
},
{
"name": "RGB2GRAYL",
"type": "isaac::opencv::RGB2GREYL"
},
{
"name": "RGB2GRAYR",
"type": "isaac::opencv::RGB2GREYR"
}
]
},
{
"name": "tracker",
"components": [
{
"name": "MessageLedger",
"type": "isaac::alice::MessageLedger"
},
{
"name": "StereoVisualOdometry",
"type": "isaac::StereoVisualOdometry"
}
]
},
{
"name": "image_keypoint_viewer",
"components": [
{
"name": "isaac.alice.MessageLedger",
"type": "isaac::alice::MessageLedger"
},
{
"name": "isaac.viewers.ImageKeypointViewer",
"type": "isaac::viewers::ImageKeypointViewer"
}
]
},
{
"name": "color_camera_viewer",
"components": [
{
"name": "isaac.alice.MessageLedger",
"type": "isaac::alice::MessageLedger"
},
{
"name": "isaac.viewers.ColorCameraViewer",
"type": "isaac::viewers::ColorCameraViewer"
}
]
}
],
"edges": [
{
"source": "caminputn/pipeline/acquired_image",
"target": "cropn/crop1/input_image"
},
{
"source": "caminputn/pipeline/acquired_image",
"target": "cropn/crop2/input_image"
},
{
"source": "cropn/crop1/output_image",
"target": "intextn/INTL/inLcam"
},
{
"source": "cropn/crop2/output_image",
"target": "intextn/INTR/inRcam"
},
{
"source": "intextn/INTL/outLcam",
"target": "rgbtogreyn/RGB2GRAYL/input_imageL"
},
{
"source": "intextn/INTR/outRcam",
"target": "rgbtogreyn/RGB2GRAYR/input_imageR"
},
{
"source": "rgbtogreyn/RGB2GRAYL/output_imageL",
"target": "tracker/StereoVisualOdometry/left_image"
},
{
"source": "rgbtogreyn/RGB2GRAYR/output_imageR",
"target": "tracker/StereoVisualOdometry/right_image"
},
{
"source": "intextn/INTL/outLint",
"target": "tracker/StereoVisualOdometry/left_intrinsics"
},
{
"source": "intextn/INTR/outRint",
"target": "tracker/StereoVisualOdometry/right_intrinsics"
},
{
"source": "tracker/StereoVisualOdometry/coordinates",
"target": "image_keypoint_viewer/isaac.viewers.ImageKeypointViewer/coordinates"
},
{
"source": "tracker/StereoVisualOdometry/features",
"target": "image_keypoint_viewer/isaac.viewers.ImageKeypointViewer/features"
},
{
"source": "rgbtogreyn/RGB2GRAYL/output_imageL",
"target": "color_camera_viewer/isaac.viewers.ColorCameraViewer/color_listener"
}
]
},
"config": {
"caminputn": {
"pipeline": {
"pipeline": "gst-launch-1.0 -v v4l2src device=/dev/video0 ! avdec_mjpeg ! videoconvert ! video/x-raw,format=RGB,height=960,wight=2560,framerate=30/1 ! appsink name = acquired_image "
}
},
"cropn": {
"crop1": {
"crop_start": [0, 0],
"crop_size": [960, 1280]
},
"crop2": {
"crop_start": [0, 1280],
"crop_size": [960, 1280]
}
},
"tracker": {
"StereoVisualOdometry": {
"horizontal_stereo_camera": true,
"process_imu_readings": false,
"lhs_camera_frame": "left_camera",
"rhs_camera_frame": "right_camera",
"imu_frame": "imu"
}
},
"color_camera_viewer": {
"isaac.viewers.ColorCameraViewer": {
"target_fps": 30
}
},
"websight": {
"WebsightServer": {
"port": 3000,
"ui_config": {
"windows": {
"Camera Pose": {
"renderer": "3d",
"dims": {
"width": 1100,
"height": 450
},
"channels": [
{
"name": "stereo_vo/tracker/StereoVisualOdometry/current_pose",
"active": true
},
{
"name": "stereo_vo/tracker/StereoVisualOdometry/pose_trail",
"active": true
}
]
},
"xy": {
"renderer": "plot",
"channels": [
{
"name": "stereo_vo/tracker/StereoVisualOdometry/vo.pos_x"
},
{
"name": "stereo_vo/tracker/StereoVisualOdometry/vo.pos_y"
}
]
},
"up": {
"renderer": "plot",
"channels": [
{
"name": "stereo_vo/tracker/StereoVisualOdometry/vo.pos_z"
}
]
},
"2d Features": {
"renderer": "2d",
"dims": {
"width": 512,
"height": 512
},
"channels": [
{
"name": "stereo_vo/color_camera_viewer/isaac.viewers.ColorCameraViewer/Color",
"active": true
},
{
"name": "stereo_vo/image_keypoint_viewer/isaac.viewers.ImageKeypointViewer/keypoints",
"active": true
}
]
}
}
}
}
}
}
}
This my codelet to set extrinsincs but I don’t know how to send datas to svo :
#include "EXT.hpp"
#include <memory>
#include <utility>
#include "engine/core/assert.hpp"
#include "engine/gems/image/conversions.hpp"
#include "engine/gems/image/utils.hpp"
#include "messages/camera.hpp"
#include "messages/math.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/imgproc.hpp"
namespace isaac {
void EXT::start() {tickOnMessage(rx_inLcam());}
void EXT::tick() {
//Extrinsincs
Pose3d extrinsics;
double qq[4] = {0,0,0,0};
extrinsics.rotation = SO3<double>::FromQuaternion(Quaterniond(qq));
extrinsics.translation = Vector3d(0.06,0,0);
const auto ext = tx_extrinsics().initProto();
ToProto(extrinsics, ext);
tx_extrinsics().publish();
// LOG_INFO("EXT OK \n");
}
} // namespace isaac
This is my build :
load("//engine/build:isaac.bzl", "isaac_app", "isaac_cc_module")
isaac_app(
name = "stereo_vo",
app_json_file = "stereo_vo.app.json",
modules = [
"//apps/samples/stereo_vo:rgb2grey",
"//apps/samples/stereo_vo:intext",
"//packages/message_generators",
"//packages/perception",
"perception:stereo_visual_odometry",
"//packages/viewers",
"sensors:v4l2_camera",
"utils",
"sight",
"deepstream",
],
)
isaac_cc_module(
name = "intext",
srcs = [
"INTL.cpp",
"INTR.cpp",
"EXT.cpp",
],
hdrs = [
"INTL.hpp",
"INTR.hpp",
"EXT.hpp",
],
visibility = ["//visibility:public"],
deps = [
"//engine/core/math",
"//engine/core/image",
"//third_party:opencv",
],
)
isaac_cc_module(
name = "rgb2grey",
srcs = [
"RGB2GREYL.cpp",
"RGB2GREYR.cpp",
],
hdrs = [
"RGB2GREYL.hpp",
"RGB2GREYR.hpp",
],
visibility = ["//visibility:public"],
deps = [
"//engine/core/image",
"//engine/core/math",
"//engine/gems/sight",
"//third_party:opencv",
],
)