std::string FirewireVideo::PixFormat() const
{
dc1394video_mode_t video_mode;
dc1394color_coding_t color_coding;
dc1394_video_get_mode(camera,&video_mode);
dc1394_get_color_coding_from_video_mode(camera,video_mode,&color_coding);
return Dc1394ColorCodingToString(color_coding);
}
/** Get video mode.
*
* @param camera points to DC1394 camera struct
* @param[in,out] video_mode Config parameter for this option,
* updated if the camera does not support the
* requested value
* @return corresponding dc1394video_mode_t enum value selected
*/
dc1394video_mode_t getVideoMode(dc1394camera_t *camera,
std::string &video_mode)
{
for (int vm = DC1394_VIDEO_MODE_MIN;
vm <= DC1394_VIDEO_MODE_MAX;
++vm)
{
if (video_mode_names_[vm-DC1394_VIDEO_MODE_MIN] == video_mode)
{
// found the requested mode
dc1394video_modes_t vmodes;
dc1394error_t err =
dc1394_video_get_supported_modes(camera, &vmodes);
if (err != DC1394_SUCCESS)
{
ROS_FATAL("unable to get supported video modes");
// TODO raise exception
return (dc1394video_mode_t) 0;
}
// see if requested mode is available
for (uint32_t i = 0; i < vmodes.num; ++i)
{
if (vmodes.modes[i] == vm)
return (dc1394video_mode_t) vm; // yes: success
}
// requested mode not available, revert to current mode of camera
ROS_ERROR_STREAM("Video mode " << video_mode
<< " not supported by this camera");
dc1394video_mode_t current_mode;
err = dc1394_video_get_mode(camera, ¤t_mode);
if (err != DC1394_SUCCESS)
{
ROS_FATAL("unable to get current video mode");
// TODO raise exception
return (dc1394video_mode_t) 0;
}
// TODO list available modes
// change video_mode parameter to show current mode of camera
video_mode = videoModeName(current_mode);
return current_mode;
}
}
// request video mode does not match any known string
ROS_FATAL_STREAM("Unknown video_mode:" << video_mode);
ROS_BREAK();
// TODO raise exception
//CAM_EXCEPT(camera1394::Exception, "Unsupported video_mode");
return (dc1394video_mode_t) 0;
}
int capture_frames_init(capture_t *cap, capture_frame_t *frames)
{
int i;
for (i = 0; i < cap->num_active; ++i) {
int width, height;
capture_frame_format_t format;
unsigned int mode;
frames[i].width = 0;
frames[i].height = 0;
frames[i].format = CAPTURE_FRAME_FORMAT_UNKNOWN;
frames[i].timestamp = 0;
frames[i].raw_data = NULL;
//fprintf(stderr, "%d-th frame initializing...", i);
/* query parameters */
if (dc1394_video_get_mode(cap->cameras[i], (dc1394video_mode_t *)&mode) != DC1394_SUCCESS) {
//fprintf(stderr, "dc1394_get_video_mode failed.\n");
goto on_error;
}
if (s_get_capture_size(mode, &width, &height) != CAPTURE_SUCCESS) {
//fprintf(stderr, "dc1394_get_capture_size failed.\n");
goto on_error;
}
if (s_get_capture_format(mode, &format) != CAPTURE_SUCCESS) {
//fprintf(stderr, "dc1394_get_capture_format failed.\n");
goto on_error;
}
/* allocate memory */
switch (format) {
case CAPTURE_FRAME_FORMAT_GRAY:
frames[i].raw_data = malloc(sizeof(unsigned char) * width * height);
break;
case CAPTURE_FRAME_FORMAT_RGB:
frames[i].raw_data = malloc(sizeof(unsigned char) * width * height * 3);
break;
default:
;
}
if (frames[i].raw_data == NULL) {
//fprintf(stderr, "raw_data is NULL.\n");
goto on_error;
}
frames[i].width = width;
frames[i].height = height;
frames[i].format = format;
//fprintf(stderr, "done.\n");
}
//fprintf(stderr, "i = %d\n", i);
return i;
on_error:
while (--i >= 0) {
free(frames[i].raw_data);
}
return -1;
}
static int s_set_parameters(capture_t *cap, const int index, camera_setting_t *s)
{
int ret = CAPTURE_SUCCESS;
dc1394camera_t *cam = cap->cameras[index];
dc1394error_t err;
/* set operation mode to 1394b, if possible */
if (cam->bmode_capable != 0 && cap->prefer_bmode) {
err = dc1394_video_set_operation_mode(cam, DC1394_OPERATION_MODE_1394B);
DC1394_WRN(err, "could not set 1394b mode.");
if (err == DC1394_SUCCESS) {
err = dc1394_video_set_iso_speed(cam, DC1394_ISO_SPEED_800);
DC1394_WRN(err, "could not set iso mode to S800.");
if (err != DC1394_SUCCESS) {
err = dc1394_video_set_iso_speed(cam, DC1394_ISO_SPEED_400);
DC1394_WRN(err, "could not set iso mode to S400.");
}
}
}
else {
err = dc1394_video_set_operation_mode(cam, DC1394_OPERATION_MODE_LEGACY);
DC1394_WRN(err, "could not set 1394-legacy mode.");
err = dc1394_video_set_iso_speed(cam, DC1394_ISO_SPEED_400);
DC1394_WRN(err, "could not set iso mode to S400.");
}
/* mode */
s->mode = s_get_appropriate_mode(cam, ((s->mode >= 0) ? s->mode : DC1394_VIDEO_MODE_640x480_MONO8));
if (dc1394_video_set_mode(cam, s->mode) != DC1394_SUCCESS) {
ret = CAPTURE_ERROR;
}
dc1394_video_get_mode(cam, &s->mode);
/* frame rate */
s->framerate = s_get_appropriate_framerate(cam, s->mode, ((s->framerate >= 0) ? s->framerate : DC1394_FRAMERATE_30));
if (dc1394_video_set_framerate(cam, s->framerate) != DC1394_SUCCESS) {
ret = CAPTURE_ERROR;
}
/*
features
*/
SET_FEATURE(DC1394_FEATURE_BRIGHTNESS, s->brightness);
SET_FEATURE(DC1394_FEATURE_EXPOSURE, s->exposure);
SET_FEATURE(DC1394_FEATURE_SHARPNESS, s->sharpness);
SET_FEATURE(DC1394_FEATURE_HUE, s->hue);
SET_FEATURE(DC1394_FEATURE_SATURATION, s->saturation);
SET_FEATURE(DC1394_FEATURE_GAMMA, s->gamma);
SET_FEATURE(DC1394_FEATURE_SHUTTER, s->shutter);
SET_FEATURE(DC1394_FEATURE_GAIN, s->gain);
SET_FEATURE(DC1394_FEATURE_IRIS, s->iris);
SET_FEATURE(DC1394_FEATURE_FOCUS, s->focus);
//SET_FEATURE(DC1394_FEATURE_TEMPERATURE, s->temperature); is not supported by dc1394_feature_set_value()
SET_FEATURE(DC1394_FEATURE_ZOOM, s->zoom);
SET_FEATURE(DC1394_FEATURE_PAN, s->pan);
SET_FEATURE(DC1394_FEATURE_TILT, s->tilt);
/* white blance */
if (s->white_balance[0] >= 0 && s->white_balance[1] >= 0) {
dc1394_feature_set_mode(cam, DC1394_FEATURE_WHITE_BALANCE, DC1394_FEATURE_MODE_MANUAL);
if (dc1394_feature_whitebalance_set_value(cam, s->white_balance[0], s->white_balance[1]) != DC1394_SUCCESS) {
ret = CAPTURE_ERROR;
}
}
else {
if (dc1394_feature_set_mode(cam, DC1394_FEATURE_WHITE_BALANCE, DC1394_FEATURE_MODE_AUTO) != DC1394_SUCCESS) {
ret = CAPTURE_ERROR;
}
}
return ret;
}
bool CvCaptureCAM_DC1394_v2_CPP::startCapture()
{
int i;
int code = 0;
if (!dcCam)
return false;
if (isoSpeed > 0)
{
code = dc1394_video_set_iso_speed(dcCam,
isoSpeed <= 100 ? DC1394_ISO_SPEED_100 :
isoSpeed <= 200 ? DC1394_ISO_SPEED_200 :
isoSpeed <= 400 ? DC1394_ISO_SPEED_400 :
isoSpeed <= 800 ? DC1394_ISO_SPEED_800 :
isoSpeed == 1600 ? DC1394_ISO_SPEED_1600 :
DC1394_ISO_SPEED_3200);
}
// should a specific mode be used
if (userMode >= 0)
{
dc1394video_mode_t wantedMode;
dc1394video_modes_t videoModes;
dc1394_video_get_supported_modes(dcCam, &videoModes);
//set mode from number, for example the second supported mode, i.e userMode = 1
if (userMode < (int)videoModes.num)
{
wantedMode = videoModes.modes[userMode];
}
//set modes directly from DC134 constants (from dc1394video_mode_t)
else if ((userMode >= DC1394_VIDEO_MODE_MIN) && (userMode <= DC1394_VIDEO_MODE_MAX ))
{
//search for wanted mode, to check if camera supports it
int j = 0;
while ((j< (int)videoModes.num) && videoModes.modes[j]!=userMode)
{
j++;
}
if ((int)videoModes.modes[j]==userMode)
{
wantedMode = videoModes.modes[j];
}
else
{
userMode = -1; // wanted mode not supported, search for best mode
}
}
else
{
userMode = -1; // wanted mode not supported, search for best mode
}
//if userMode is available: set it and update size
if (userMode != -1)
{
code = dc1394_video_set_mode(dcCam, wantedMode);
uint32_t width, height;
dc1394_get_image_size_from_video_mode(dcCam, wantedMode, &width, &height);
frameWidth = (int)width;
frameHeight = (int)height;
}
}
if (userMode == -1 && (frameWidth > 0 || frameHeight > 0))
{
dc1394video_mode_t bestMode = (dc1394video_mode_t) - 1;
dc1394video_modes_t videoModes;
dc1394_video_get_supported_modes(dcCam, &videoModes);
for (i = 0; i < (int)videoModes.num; i++)
{
dc1394video_mode_t mode = videoModes.modes[i];
if (mode >= DC1394_VIDEO_MODE_FORMAT7_MIN && mode <= DC1394_VIDEO_MODE_FORMAT7_MAX)
continue;
int pref = -1;
dc1394color_coding_t colorCoding;
dc1394_get_color_coding_from_video_mode(dcCam, mode, &colorCoding);
uint32_t width, height;
dc1394_get_image_size_from_video_mode(dcCam, mode, &width, &height);
if ((int)width == frameWidth || (int)height == frameHeight)
{
if (colorCoding == DC1394_COLOR_CODING_RGB8 ||
colorCoding == DC1394_COLOR_CODING_RAW8)
{
bestMode = mode;
break;
}
if (colorCoding == DC1394_COLOR_CODING_YUV411 ||
colorCoding == DC1394_COLOR_CODING_YUV422 ||
(colorCoding == DC1394_COLOR_CODING_YUV444 &&
pref < 1))
{
bestMode = mode;
pref = 1;
break;
}
if (colorCoding == DC1394_COLOR_CODING_MONO8)
{
bestMode = mode;
pref = 0;
}
}
}
if ((int)bestMode >= 0)
code = dc1394_video_set_mode(dcCam, bestMode);
}
if (fps > 0)
{
dc1394video_mode_t mode;
dc1394framerates_t framerates;
double minDiff = DBL_MAX;
dc1394framerate_t bestFps = (dc1394framerate_t) - 1;
dc1394_video_get_mode(dcCam, &mode);
dc1394_video_get_supported_framerates(dcCam, mode, &framerates);
for (i = 0; i < (int)framerates.num; i++)
{
dc1394framerate_t ifps = framerates.framerates[i];
double fps1 = (1 << (ifps - DC1394_FRAMERATE_1_875)) * 1.875;
double diff = fabs(fps1 - fps);
if (diff < minDiff)
{
minDiff = diff;
bestFps = ifps;
}
}
if ((int)bestFps >= 0)
code = dc1394_video_set_framerate(dcCam, bestFps);
}
if (cameraId == VIDERE)
{
bayerFilter = DC1394_COLOR_FILTER_GBRG;
nimages = 2;
uint32_t value = 0;
dc1394_get_control_register(dcCam, 0x50c, &value);
colorStereo = (value & 0x80000000) != 0;
}
code = dc1394_capture_setup(dcCam, nDMABufs, DC1394_CAPTURE_FLAGS_DEFAULT);
if (code >= 0)
{
FD_SET(dc1394_capture_get_fileno(dcCam), &dc1394.camFds);
dc1394_video_set_transmission(dcCam, DC1394_ON);
if (cameraId == VIDERE)
{
enum { PROC_MODE_OFF, PROC_MODE_NONE, PROC_MODE_TEST, PROC_MODE_RECTIFIED, PROC_MODE_DISPARITY, PROC_MODE_DISPARITY_RAW };
int procMode = PROC_MODE_RECTIFIED;
usleep(100000);
uint32_t qval1 = 0x08000000 | (0x90 << 16) | ((procMode & 0x7) << 16);
uint32_t qval2 = 0x08000000 | (0x9C << 16);
dc1394_set_control_register(dcCam, 0xFF000, qval1);
dc1394_set_control_register(dcCam, 0xFF000, qval2);
}
started = true;
}
return code >= 0;
}
bool CvCaptureCAM_DC1394_v2_CPP::startCapture()
{
int i;
int code = 0;
if (!dcCam)
return false;
if (isoSpeed > 0)
{
code = dc1394_video_set_iso_speed(dcCam,
isoSpeed <= 100 ? DC1394_ISO_SPEED_100 :
isoSpeed <= 200 ? DC1394_ISO_SPEED_200 :
isoSpeed <= 400 ? DC1394_ISO_SPEED_400 :
isoSpeed <= 800 ? DC1394_ISO_SPEED_800 :
isoSpeed == 1600 ? DC1394_ISO_SPEED_1600 :
DC1394_ISO_SPEED_3200);
}
if (frameWidth > 0 || frameHeight > 0)
{
dc1394video_mode_t bestMode = (dc1394video_mode_t) - 1;
dc1394video_modes_t videoModes;
dc1394_video_get_supported_modes(dcCam, &videoModes);
for (i = 0; i < (int)videoModes.num; i++)
{
dc1394video_mode_t mode = videoModes.modes[i];
if (mode >= DC1394_VIDEO_MODE_FORMAT7_MIN && mode <= DC1394_VIDEO_MODE_FORMAT7_MAX)
continue;
int pref = -1;
dc1394color_coding_t colorCoding;
dc1394_get_color_coding_from_video_mode(dcCam, mode, &colorCoding);
uint32_t width, height;
dc1394_get_image_size_from_video_mode(dcCam, mode, &width, &height);
if ((int)width == frameWidth || (int)height == frameHeight)
{
if (colorCoding == DC1394_COLOR_CODING_RGB8 ||
colorCoding == DC1394_COLOR_CODING_RGB16 ||
colorCoding == DC1394_COLOR_CODING_RAW8 ||
colorCoding == DC1394_COLOR_CODING_RAW16)
{
bestMode = mode;
break;
}
if (colorCoding == DC1394_COLOR_CODING_YUV411 ||
colorCoding == DC1394_COLOR_CODING_YUV422 ||
(colorCoding == DC1394_COLOR_CODING_YUV444 &&
pref < 1))
{
bestMode = mode;
pref = 1;
}
if (colorCoding == DC1394_COLOR_CODING_MONO8 ||
(colorCoding == DC1394_COLOR_CODING_MONO16 &&
pref < 0))
{
bestMode = mode;
pref = 0;
}
}
}
if ((int)bestMode >= 0)
code = dc1394_video_set_mode(dcCam, bestMode);
}
if (fps > 0)
{
dc1394video_mode_t mode;
dc1394framerates_t framerates;
double minDiff = DBL_MAX;
dc1394framerate_t bestFps = (dc1394framerate_t) - 1;
dc1394_video_get_mode(dcCam, &mode);
dc1394_video_get_supported_framerates(dcCam, mode, &framerates);
for (i = 0; i < (int)framerates.num; i++)
{
dc1394framerate_t ifps = framerates.framerates[i];
double fps1 = (1 << (ifps - DC1394_FRAMERATE_1_875)) * 1.875;
double diff = fabs(fps1 - fps);
if (diff < minDiff)
{
minDiff = diff;
bestFps = ifps;
}
}
if ((int)bestFps >= 0)
code = dc1394_video_set_framerate(dcCam, bestFps);
}
if (cameraId == VIDERE)
{
bayerFilter = DC1394_COLOR_FILTER_GBRG;
nimages = 2;
uint32_t value = 0;
dc1394_get_control_register(dcCam, 0x50c, &value);
colorStereo = (value & 0x80000000) != 0;
}
code = dc1394_capture_setup(dcCam, nDMABufs, DC1394_CAPTURE_FLAGS_DEFAULT);
if (code >= 0)
{
FD_SET(dc1394_capture_get_fileno(dcCam), &dc1394.camFds);
dc1394_video_set_transmission(dcCam, DC1394_ON);
if (cameraId == VIDERE)
{
enum { PROC_MODE_OFF, PROC_MODE_NONE, PROC_MODE_TEST, PROC_MODE_RECTIFIED, PROC_MODE_DISPARITY, PROC_MODE_DISPARITY_RAW };
int procMode = PROC_MODE_RECTIFIED;
usleep(100000);
uint32_t qval1 = 0x08000000 | (0x90 << 16) | ((procMode & 0x7) << 16);
uint32_t qval2 = 0x08000000 | (0x9C << 16);
dc1394_set_control_register(dcCam, 0xFF000, qval1);
dc1394_set_control_register(dcCam, 0xFF000, qval2);
}
started = true;
}
return code >= 0;
}
