From a8d57f3cd7a255062ffc72b2b738bd73c2982320 Mon Sep 17 00:00:00 2001 From: irvingskr <17303038781@163.com> Date: Wed, 15 Jun 2022 23:43:30 +0800 Subject: [PATCH] Add gamma and contrast. --- cfg/camera.cfg | 13 +++++++++++++ include/galaxy_camera.h | 4 ++++ launch/MER-139.launch | 1 + src/galaxy_camera.cpp | 40 +++++++++++++++++++++++++++++++++++++++- 4 files changed, 57 insertions(+), 1 deletion(-) diff --git a/cfg/camera.cfg b/cfg/camera.cfg index b9e5cce..3d20463 100755 --- a/cfg/camera.cfg +++ b/cfg/camera.cfg @@ -30,4 +30,17 @@ gen.add("white_selector", int_t, 0, "White balance channel selector", 0, 0, 2, e gen.add("white_value", double_t, 0, "White balance value(dB) when not in auto mode", 1., 1., 7.9961) gen.add("white_auto", bool_t, 0, "Auto white", True) +# Gamma +gen.add("gamma_param", double_t, 0, "Gamma param", 5, 0, 100) + +# Contrast +gen.add("contrast_param", int_t, 0, "Contrast param", 5, 0, 100) + +mode = gen.enum([gen.const("Both", int_t, 0, "Both open"), + gen.const("Gamma", int_t, 1, "Gamma open"), + gen.const("Contrast", int_t, 2, "Contrast open"), + gen.const("Off", int_t, 3, "Both of")], + "mode") +gen.add("improve_mode", int_t, 0, "Image improve mode selector", 0, 0, 3, edit_method=mode) + exit(gen.generate(PACKAGE, "galaxy_camera", "Camera")) diff --git a/include/galaxy_camera.h b/include/galaxy_camera.h index 7f2ac0f..c691c01 100644 --- a/include/galaxy_camera.h +++ b/include/galaxy_camera.h @@ -43,6 +43,7 @@ class GalaxyCameraNodelet : public nodelet::Nodelet boost::shared_ptr info_manager_; std::string camera_name_, camera_info_url_, pixel_format_, frame_id_, camera_sn_; int image_width_{}, image_height_{}, image_offset_x_{}, image_offset_y_{}, raising_filter_value_{}; + double frame_rate_; static bool enable_imu_trigger_; static char* img_; static image_transport::CameraPublisher pub_; @@ -61,6 +62,9 @@ class GalaxyCameraNodelet : public nodelet::Nodelet static int fifo_rear_; static void fifoWrite(TriggerPacket pkt); static bool fifoRead(TriggerPacket& pkt); + static double gamma_param_; + static int64_t contrast_param_; + static int improve_mode_; }; } // namespace galaxy_camera diff --git a/launch/MER-139.launch b/launch/MER-139.launch index a776c79..4fbe809 100644 --- a/launch/MER-139.launch +++ b/launch/MER-139.launch @@ -19,6 +19,7 @@ + diff --git a/src/galaxy_camera.cpp b/src/galaxy_camera.cpp index b42f4a1..2e450ea 100644 --- a/src/galaxy_camera.cpp +++ b/src/galaxy_camera.cpp @@ -31,6 +31,7 @@ void GalaxyCameraNodelet::onInit() nh_.param("camera_sn", camera_sn_, std::string("")); nh_.param("enable_imu_trigger", enable_imu_trigger_, true); nh_.param("raising_filter_value", raising_filter_value_, 0); + nh_.param("frame_rate", frame_rate_, 10.0); info_manager_.reset(new camera_info_manager::CameraInfoManager(nh_, camera_name_, camera_info_url_)); // check for default camera info @@ -99,6 +100,8 @@ void GalaxyCameraNodelet::onInit() assert(GXSetInt(dev_handle_, GX_INT_OFFSET_X, image_offset_x_) == GX_STATUS_SUCCESS); assert(GXSetInt(dev_handle_, GX_INT_OFFSET_Y, image_offset_y_) == GX_STATUS_SUCCESS); assert(GXSetEnum(dev_handle_, GX_ENUM_BALANCE_WHITE_AUTO, GX_BALANCE_WHITE_AUTO_CONTINUOUS) == GX_STATUS_SUCCESS); + assert(GXSetEnum(dev_handle_,GX_ENUM_ACQUISITION_FRAME_RATE_MODE, GX_ENUM_COVER_FRAMESTORE_MODE_ON) == GX_STATUS_SUCCESS); + assert(GXSetFloat(dev_handle_,GX_FLOAT_ACQUISITION_FRAME_RATE,frame_rate_) ==GX_STATUS_SUCCESS); if (enable_imu_trigger_) { @@ -195,6 +198,37 @@ void GalaxyCameraNodelet::onFrameCB(GX_FRAME_CALLBACK_PARAM* pFrame) info_.header.stamp = now; } DxRaw8toRGB24((void*)pFrame->pImgBuf, img_, pFrame->nWidth, pFrame->nHeight, RAW2RGB_NEIGHBOUR, BAYERBG, false); + assert(GXGetFloat(dev_handle_,GX_FLOAT_GAMMA_PARAM,&gamma_param_)==GX_STATUS_SUCCESS); + + int nLutLength; + assert(DxGetGammatLut(gamma_param_, NULL, &nLutLength)==DX_OK); + float* pGammaLut = new float[nLutLength]; + assert(DxGetGammatLut(gamma_param_,pGammaLut,&nLutLength)==DX_OK); + + assert(GXGetInt(dev_handle_,GX_INT_CONTRAST_PARAM,&contrast_param_)==GX_STATUS_SUCCESS); + assert(DxGetContrastLut(contrast_param_,NULL,&nLutLength)==DX_OK); +// ROS_INFO("%d",nLutLength); + float* pContrastLut = new float[nLutLength]; +// ROS_INFO("%p",pContrastLut); + assert(DxGetContrastLut(contrast_param_,pContrastLut,&nLutLength)==DX_OK); + switch (improve_mode_) { + case 0: + assert(DxImageImprovment(img_,img_,pFrame->nWidth,pFrame->nHeight,0,pContrastLut,pGammaLut)==DX_OK); + break; + case 1: + assert(DxImageImprovment(img_,img_,pFrame->nWidth,pFrame->nHeight,0,NULL,pGammaLut)==DX_OK); + break; + case 2: + assert(DxImageImprovment(img_,img_,pFrame->nWidth,pFrame->nHeight,0,pContrastLut,NULL)==DX_OK); + break; + case 3: + break; + } + + if(pGammaLut!=NULL) + delete[] pGammaLut; + if(pContrastLut!=NULL) + delete[] pContrastLut; memcpy((char*)(&image_.data[0]), img_, image_.step * image_.height); pub_.publish(image_, info_); } @@ -276,6 +310,8 @@ void GalaxyCameraNodelet::reconfigCB(CameraConfig& config, uint32_t level) GXSetEnum(dev_handle_, GX_ENUM_BALANCE_WHITE_AUTO, GX_BALANCE_WHITE_AUTO_OFF); GXSetFloat(dev_handle_, GX_FLOAT_BALANCE_RATIO, config.white_value); } + assert(GXGetFloat(dev_handle_,GX_FLOAT_GAMMA_PARAM,&config.gamma_param)==GX_STATUS_SUCCESS); + improve_mode_ = config.improve_mode; } GalaxyCameraNodelet::~GalaxyCameraNodelet() @@ -298,5 +334,7 @@ int GalaxyCameraNodelet::fifo_front_ = 0; int GalaxyCameraNodelet::fifo_rear_ = 0; struct TriggerPacket GalaxyCameraNodelet::fifo_[FIFO_SIZE]; uint32_t GalaxyCameraNodelet::receive_trigger_counter_ = 0; - +double GalaxyCameraNodelet::gamma_param_{}; +int64_t GalaxyCameraNodelet::contrast_param_{}; +int GalaxyCameraNodelet::improve_mode_ = 1; } // namespace galaxy_camera