// Open DUO camera sensor
bool DUOStereoSensorDriver::openCamera()
{
if (camera_open)
return true;
if(!OpenDUOCamera(frameSize.width, frameSize.height, 30))
{
camera_open = false;
return false;
}
camera_open = true;
// Set exposure and LED brightness
SetGain(duoProperties->gain);
SetExposure(duoProperties->exposure);
SetLed(duoProperties->led);
// Un-flip frames
SetVFlip(true);
// Enable retrieval of undistorted (rectified) frames
SetUndistort(true);
return true;
}
void Flea3Camera::Configure(Config& config) {
// Video Mode
SetVideoMode(config.video_mode, config.format7_mode, config.pixel_format,
config.width, config.height);
// Update CameraInfo here after video mode is changed
camera_info_ = GetCameraInfo(camera_);
// Frame Rate
SetFrameRate(config.fps);
// Raw Bayer
SetRawBayerOutput(config.raw_bayer_output);
// White Balance
SetWhiteBalanceRedBlue(config.white_balance, config.auto_white_balance,
config.wb_red, config.wb_blue);
// Exposure
SetExposure(config.exposure, config.auto_exposure, config.exposure_value);
SetShutter(config.auto_shutter, config.shutter_ms);
SetGain(config.auto_gain, config.gain_db);
SetBrightness(config.brightness);
SetGamma(config.gamma);
// Strobe
SetStrobe(config.strobe_control, config.strobe_polarity);
// Trigger
SetTrigger(config.trigger_source, config.trigger_polarity);
// Save this config
config_ = config;
}
int main(int argc, char* argv[])
{
printf("DUOLib Version: v%s\n", GetLibVersion());
// Open DUO camera and start capturing
if(!OpenDUOCamera(WIDTH, HEIGHT, FPS))
{
printf("Could not open DUO camera\n");
return 0;
}
// Create OpenCV windows
cvNamedWindow("Left");
cvNamedWindow("Right");
// Set exposure and LED brightness
SetExposure(50);
SetLed(25);
// Create image headers for left & right frames
IplImage *left = cvCreateImageHeader(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 1);
IplImage *right = cvCreateImageHeader(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 1);
// Run capture loop until <Esc> key is pressed
while((cvWaitKey(1) & 0xff) != 27)
{
// Capture DUO frame
PDUOFrame pFrameData = GetDUOFrame();
if(pFrameData == NULL) continue;
// Set the image data
left->imageData = (char*)pFrameData->leftData;
right->imageData = (char*)pFrameData->rightData;
// Process images here (optional)
// Display images
cvShowImage("Left", left);
cvShowImage("Right", right);
}
// Release image headers
cvReleaseImageHeader(&left);
cvReleaseImageHeader(&right);
// Close DUO camera
CloseDUOCamera();
return 0;
}
bool PS3::ResetCamParam() {
// TODO return false
//! Set the parameters value to default
SetHFlip( false );
SetVFlip( false );
SetAutoGain( true );
SetGainValue( 0 );
SetAutoExposure( true );
SetExposure( 0 );
SetAutoWhiteBalance( true );
SetWhiteBalanceRed( 0 );
SetWhiteBalanceGreen( 0 );
SetWhiteBalanceBlue( 0 );
return true;
}
void OnStart() override
{
auto scene = std::make_shared<asd::Scene>();
auto layer = std::make_shared<asd::Layer2D>();
auto obj= std::make_shared<asd::TextureObject2D>();
scene->SetHDRMode(true);
scene->AddLayer(layer);
layer->AddObject(obj);
asd::Engine::ChangeScene(scene);
auto g = asd::Engine::GetGraphics();
auto texture = g->CreateTexture2D(asd::ToAString("Data/Texture/Sample1.png").c_str());
obj->SetTexture(texture);
obj->SetScale(asd::Vector2DF(1, 1));
auto pe = std::make_shared<asd::PostEffectLightBloom>();
pe->SetIntensity(5.0f);
pe->SetThreshold(0.2f);
pe->SetExposure(3.0f);
layer->AddPostEffect(pe);
}
VisionSTCam(ros::NodeHandle _nh):nh(_nh),it(_nh)
{
startingOffset = 3*(FRAME_WIDTH*(FRAME_HEIGHT-270));
checkSize = 3*640;
endingOffset = startingOffset + checkSize;
avgIntensity, lastAvgIntensity = 0;
lastAccumVal = 0;
curExposure, calcExposure, curGlobalGain;
gainStep = 1;
intensityDiff = 0, lastIntensityDiff = 1;
dividingFactor = DEFAULT_DIVIDING_FACTOR;
left_pub = it.advertise("stereo/left/image_raw", 1);
right_pub = it.advertise("stereo/right/image_raw", 1);
disparity_pub = it.advertise("stereo/disparity/image_raw",1);
//Control Panel
cv::namedWindow("Control Intensity", CV_WINDOW_AUTOSIZE);
cv::resizeWindow("Control Intensity", 480, 400);
cv::createTrackbar("Global Gain", "Control Intensity", &m_cScrollBarGlobalGain, 200);
cv::createTrackbar("Exposure", "Control Intensity", &m_cScrollBarExposure, 100);
m_cScrollBarGlobalGain = DEFAULT_GLOBAL_GAIN;
m_cScrollBarExposure = DEFAULT_EXPOSURE;
leftImg = cv::Mat(FRAME_HEIGHT, FRAME_WIDTH, CV_8UC3);
rightImg = cv::Mat(FRAME_HEIGHT, FRAME_WIDTH, CV_8UC3);
disparityImg = cv::Mat(FRAME_HEIGHT, FRAME_WIDTH, CV_8UC1);
m_sockUdpForFindingCameras = boost::shared_ptr<CVcUdp>(new CVcUdp());
if(!GetMyIpAddr()){
ROS_ERROR("check your NIC. found IP is 127.0.0.1");
isInit=false;
}
m_bMyConfigIsReady = m_clMyConfigInfo.ReadConfigInfo(m_szMyIpAddr);
if(!m_bMyConfigIsReady) // CONFIG file 정보가 제대로 읽혀지지 않았으면
{
ROS_WARN("Configuration Info Err.!");
ROS_WARN("Default Value set.!");
}
if(!CreateSocketUDP()){
ROS_ERROR("error occured when creating UDP socket");
isInit = false;
}
isInit=true;
m_clCmdProc = boost::shared_ptr<CCmdPacket>(new CCmdPacket(&m_sockSendCmdUdp, m_sockResponseUdp.get()));
m_clCmdProc->VstCmdGetStatus_IsStarted(m_clMyConfigInfo.m_strCameraIpAddr);
usleep(200000);
if(m_sockResponseUdp->m_nIsCamStatusRun == STATUS_STOP) // 받은 메시지가 Stop 상태이면
{
ROS_INFO("m_sockResponseUdp STATUS:STOP %s",m_clMyConfigInfo.m_strCameraIpAddr);
int rtn = m_clCmdProc->VstCmdInitialize(m_clMyConfigInfo.m_strCameraIpAddr);
if(rtn == CMD_ERROR)
{
ROS_ERROR("Init Command Error!");
}
else
{
ROS_ERROR("Init Command OK!");
}
m_sockResponseUdp->m_nIsCamStatusRun = STATUS_NO_RESPONSE;
m_clCmdProc->VstCmdGetStatus_IsStarted(m_clMyConfigInfo.m_strCameraIpAddr);
usleep(100000); // 기다렸다가...
///OnBnClickedRun();
m_bForcedStop = false;
// TODO: 여기에 컨트롤 알림 처리기 코드를 추가합니다.
rtn = m_clCmdProc->VstCmdStart(m_clMyConfigInfo.m_strCameraIpAddr);
if(rtn == CMD_ERROR)
{
ROS_ERROR("Run Command Error!");
}
else
{
ROS_INFO("Run Command OK!");
}
m_sockResponseUdp->m_nIsCamStatusRun = STATUS_NO_RESPONSE;
m_clCmdProc->VstCmdGetStatus_IsStarted(m_clMyConfigInfo.m_strCameraIpAddr);
// 표시 화면 생성
///SetTimer(TIMER_IMG_DISPLAY, msec_IMAGE_DISPLAY_CYCLE, NULL);
///SetTimer(TIMER_DISPLAY_IMG_DATA_COMM_STATUS, 1000, NULL);
usleep(100000); // 기다렸다가...
SetGlobalGain(m_cScrollBarGlobalGain);
SetExposure(m_cScrollBarExposure);
}
else if(m_sockResponseUdp->m_nIsCamStatusRun == STATUS_RUN) // 현재 메시지를 송출 중이라면
{
ROS_INFO("m_sockResponseUdp STATUS:RUN");
///OnBnClickedRun();
m_bForcedStop = false;
//TODO: 여기에 컨트롤 알림 처리기 코드를 추가합니다.
int rtn = m_clCmdProc->VstCmdStart(m_clMyConfigInfo.m_strCameraIpAddr);
if(rtn == CMD_ERROR)
{
ROS_ERROR("Run Command Error!");
}
else
{
ROS_INFO("Run Command OK!");
}
m_sockResponseUdp->m_nIsCamStatusRun = STATUS_NO_RESPONSE;
m_clCmdProc->VstCmdGetStatus_IsStarted(m_clMyConfigInfo.m_strCameraIpAddr);
// 표시 화면 생성
///SetTimer(TIMER_IMG_DISPLAY, msec_IMAGE_DISPLAY_CYCLE, NULL);
///SetTimer(TIMER_DISPLAY_IMG_DATA_COMM_STATUS, 1000, NULL);
//usleep(100000); // 기다렸다가...
SetGlobalGain(DEFAULT_GLOBAL_GAIN);
SetExposure(DEFAULT_EXPOSURE);
///SetTimer(TIMER_UDP_DATA_COMM_DISPLAY, msec_IMAGE_DISPLAY_CYCLE, NULL);
}
}
void ControlIntensity()
{
while (nh.ok()&&!leftImg.empty()){
uint64 accumVal = 0;
// 현재 Left 이미지 270번째 라인 Intensity 적산
for(int i=startingOffset; i<endingOffset; i+=2)
{
accumVal += leftImg.data[i];
}
// 평균 Intensity 계산
// 현재 intensity 영향을 반으로 줄이기
avgIntensity = (lastAccumVal + accumVal)/checkSize;
intensityDiff = m_nSetAvgIntensity - avgIntensity;
// 설정한 intensity 범위안에 있거나
// 그보다 바깥 범위에 있지만, 이전 intensity와 현재 intensity 차이가 10이하면 스킵
// 너무 민감하게 반응하지 않게 하기 위한 조건들을 더함.
if((abs(intensityDiff) < 11) ||
(abs(intensityDiff) < 21 && abs(avgIntensity - lastAvgIntensity) < 10))
{
return;
}
lastAccumVal = accumVal;
lastAvgIntensity = avgIntensity;
// 이전의 intensity diff와 방향이 반대라면
// 즉, 이전의 exposure 조정값이 너무 커서
// 목표 intensity 범위를 지나쳐 버렸다면
if((intensityDiff > 0 && lastIntensityDiff < 0) ||
(intensityDiff < 0 && lastIntensityDiff > 0))
{
// Gain도 낮추고, Exposure 조정값의 폭도 줄도록 한다.
curGlobalGain = m_cScrollBarGlobalGain;
if(curGlobalGain >= MIN_GLOBAL_GAIN + gainStep)
SetGlobalGain(curGlobalGain - gainStep);
dividingFactor *= 2;
}
else if(dividingFactor > DEFAULT_DIVIDING_FACTOR)
{
dividingFactor /= 2; // 빠른 반응을 위해 다시 dividingFactor값을 낮춰둔다.
}
lastIntensityDiff = intensityDiff;
// 현재의 카메라 exposure
curExposure = m_cScrollBarExposure;
// 새로 적용할 exposure 값
calcExposure = curExposure + intensityDiff / dividingFactor;
// Exposure 만으로 Intensity 조절이 안될 때, Gain도 수정적용
if(calcExposure < MIN_EXPOSURE)
{
calcExposure = MIN_EXPOSURE;
curGlobalGain = m_cScrollBarGlobalGain;
if(curGlobalGain > MIN_GLOBAL_GAIN + gainStep)
SetGlobalGain(curGlobalGain - gainStep);
return;
}
else if(calcExposure > MAX_EXPOSURE)
{
calcExposure = MAX_EXPOSURE;
curGlobalGain = m_cScrollBarGlobalGain;
if(curGlobalGain < MAX_GLOBAL_GAIN - gainStep)
SetGlobalGain(curGlobalGain + gainStep);
return;
}
ROS_INFO("Gain : %d, Exposure : %d", curGlobalGain - gainStep, calcExposure);
SetExposure(calcExposure);
}
}
