Hello,
I did a code to copy custom intrinsincs to a color camera proto but I have issues doing the image copying part.
I need to copy my input image into my output proto but I don’t know how to do that so I tried with opencv but maybe their is an easier way, please help me. (This code compile but no image is sent. Yes there is L and R because I use a stereo camera)
This is my code to get the color camera proto and send a modified color camera proto, my issue is in the Copy image section of the code :
#include "TX.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 TX::start() {
tickOnMessage(rx_inLcam());
}
void TX::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();
//Intrinsincs
auto outL = tx_outLcam().initProto();
auto outR = tx_outRcam().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);
auto pinholeR = outR.initPinhole();
ToProto(Vector2d(3147.54098361, 3160.49382716), pinholeR.getFocal());//theoric :1280*12/4.86 & 960*12/3.66 Pratic : [8.525929042561033e+02,8.549701455135702e+02]
ToProto(Vector2d(519.4719202793973, 693.0736876181506), pinholeR.getCenter());//principal point [6.930736876181506e+02,5.194719202793973e+02]
pinholeR.setCols(1280);
pinholeR.setRows(960);
// Distortion
auto distortionL = outL.initDistortion();
auto distortionR = outR.initDistortion();
distortionL.setModel(DistortionProto::DistortionModel::BROWN);
distortionR.setModel(DistortionProto::DistortionModel::BROWN);
Vector5d disto;
disto << 0.035, 0.035, 0, 0, 0;
ToProto(disto, distortionL.getCoefficients());
ToProto(disto, distortionR.getCoefficients());
//Color
outL.setColorSpace(ColorCameraProto::ColorSpace::RGB);
outR.setColorSpace(ColorCameraProto::ColorSpace::RGB);
//Get Image
auto inL= rx_inLcam().getProto();
auto inR = rx_inRcam().getProto();
ImageConstView3ub inLcam;
ImageConstView3ub inRcam;
bool okL = FromProto(inL.getImage(), rx_inLcam().buffers(), inLcam); ASSERT(okL, "Failed to deserialize the input image L");
bool okR = FromProto(inR.getImage(), rx_inRcam().buffers(), inRcam); ASSERT(okR, "Failed to deserialize the input image R");
//Copy Image
const size_t rows = inLcam.rows();
const size_t cols = inLcam.cols();
Image3ub outLcam(rows, cols);
Image3ub outRcam(rows, cols);
cv::Mat imageL = cv::Mat(rows, cols, CV_8UC3, const_cast<void*>(static_cast<const void*>(inLcam.data().pointer())));
cv::Mat imageR = cv::Mat(rows, cols, CV_8UC3, const_cast<void*>(static_cast<const void*>(inRcam.data().pointer())));
cv::Mat outLcam_image = imageL.clone();
cv::Mat outRcam_image = imageR.clone();
// NEED A outLcam = outLcam_image; FUNCTION HERE, PLEASE HELP ME WITH THIS LINE
//Publish CamProto
ToProto(std::move(outLcam), outL.getImage(), tx_outLcam().buffers());
ToProto(std::move(outRcam), outR.getImage(), tx_outRcam().buffers());
tx_outLcam().publish(rx_inLcam().acqtime());
tx_outRcam().publish(rx_inRcam().acqtime());
LOG_INFO("OK \n");
}
} // namespace isaac
For example the following code convert a RGB image into Grey and work perfectly :
#include "RGB2GREY.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 RGB2GREY::start() {
tickOnMessage(rx_input_image());
}
void RGB2GREY::tick() {
auto input = rx_input_image().getProto();
ImageConstView3ub input_image;
bool ok = FromProto(input.getImage(), rx_input_image().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_image().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_image().buffers());
tx_output_image().publish(rx_input_image().acqtime());
}
} // namespace opencv
} // namespace isaac