From 7ddf6735feb6d6b6545e457d7b58207f5a9a983c Mon Sep 17 00:00:00 2001 From: PleiadesTX2 Date: Thu, 5 Dec 2019 12:00:05 -0400 Subject: [PATCH] Adding Spiri settings to start the cameras --- calibration-left.yaml | 20 ++++ calibration-right.yaml | 20 ++++ camera-controls-utility.sh | 209 +++++++++++++++++++++++++++++++++++++ cameras-manual.launch | 48 +++++++++ cameras.launch | 48 +++++++++ cameras_SLAM.launch | 48 +++++++++ jetson_csi_cam.launch | 2 +- stereoSettings.yaml | 117 +++++++++++++++++++++ video-daylight.launch | 47 +++++++++ 9 files changed, 558 insertions(+), 1 deletion(-) create mode 100644 calibration-left.yaml create mode 100644 calibration-right.yaml create mode 100755 camera-controls-utility.sh create mode 100644 cameras-manual.launch create mode 100644 cameras.launch create mode 100644 cameras_SLAM.launch create mode 100644 stereoSettings.yaml create mode 100644 video-daylight.launch diff --git a/calibration-left.yaml b/calibration-left.yaml new file mode 100644 index 0000000..bd32967 --- /dev/null +++ b/calibration-left.yaml @@ -0,0 +1,20 @@ +image_width: 1280 +image_height: 720 +camera_name: narrow_stereo/left +camera_matrix: + rows: 3 + cols: 3 + data: [872.082713, 0, 629.146801, 0, 872.948685, 329.783337, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.382749, 0.138828, 0.001323, 0.000611, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [0.999842, -0.007606, 0.016049, 0.007604, 0.9999709999999999, 0.000166, -0.01605, -4.4e-05, 0.999871] +projection_matrix: + rows: 3 + cols: 4 + data: [858.0889110000001, 0, 599.495079, 0, 0, 858.0889110000001, 344.665634, 0, 0, 0, 1, 0] diff --git a/calibration-right.yaml b/calibration-right.yaml new file mode 100644 index 0000000..d19ea74 --- /dev/null +++ b/calibration-right.yaml @@ -0,0 +1,20 @@ +image_width: 1280 +image_height: 720 +camera_name: narrow_stereo/right +camera_matrix: + rows: 3 + cols: 3 + data: [868.009016, 0, 627.0717110000001, 0, 868.673534, 357.037587, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.376131, 0.129475, 0.000191, 0.000201, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [0.999533, 0.00838, 0.029402, -0.008376999999999999, 0.999965, -0.000228, -0.029403, -1.8e-05, 0.9995679999999999] +projection_matrix: + rows: 3 + cols: 4 + data: [858.0889110000001, 0, 599.495079, -430.072473, 0, 858.0889110000001, 344.665634, 0, 0, 0, 1, 0] diff --git a/camera-controls-utility.sh b/camera-controls-utility.sh new file mode 100755 index 0000000..834b728 --- /dev/null +++ b/camera-controls-utility.sh @@ -0,0 +1,209 @@ +#!/bin/bash +exposure=0 +gain=0 +data=0 +READGAINCTRL="" +READEXPOSURECTRL="" +WRITEGAINCTRL="" +WRITEEXPOSURECTRL="" +MAXMINCTRL="" +DEVICE="" +CONFIGFILE="" +HELP="" + +for i in "$@" +do + +HELP=`echo $i | grep -wo "\--h"` +READGAINCTRL=`echo $i | grep -wo "\-rg"` +READEXPOSURECTRL=`echo $i | grep -wo "\-re"` +MAXMINCTRL=`echo $i | grep -wo "\-mm"` +LOWLIGHTCOND=`echo $i | grep -wo "\-llc"` +HIGHLIGHTCOND=`echo $i | grep -wo "\-hlc"` + +case $i in + -rg=*|--readgain=*) + READGAINCTRL="${i#*=}" + shift # past argument=value + ;; + -re=*|--readexposure=*) + READEXPOSURECTRL="${i#*=}" + shift # past argument=value + ;; + -wg=*|--writegain=*) + WRITEGAINCTRL="${i#*=}" + shift # past argument=value + ;; + -we=*|--writeexposure=*) + WRITEEXPOSURECTRL="${i#*=}" + shift # past argument=value + ;; + -mm=*|--maxminctrls=*) + MAXMINCTRL="${i#*=}" + shift # past argument=value + ;; + -llc=*|--lightcondlow=*) + LOWLIGHTCOND="${i#*=}" + shift # past argument=value + ;; + -hlc=*|--lightcondhigh=*) + HIGHLIGHTCOND="${i#*=}" + shift # past argument=value + ;; + -d=*|--device=*) + DEVICE="${i#*=}" + shift # past argument=value + ;; + --h*|--help*) + HELP="${i#*=}" + shift # past argument=value + ;; + --default) + DEFAULT=YES + shift # past argument with no value + ;; + *) + # unknown option + ;; +esac +done + +read_gain(){ + if [ "$DEVICE" == "" ] + then + echo "Device needed" + else + v4l2-ctl -d "/dev/video$DEVICE" -C gain + fi +} + +read_exposure(){ + if [ "$DEVICE" == "" ] + then + echo "Device needed" + else + v4l2-ctl -d "/dev/video$DEVICE" -C coarse_time + fi +} + +write_gain(){ + if [ "$DEVICE" == "" ] + then + echo "Device needed" + else + gain=$WRITEGAINCTRL + v4l2-ctl -d "/dev/video$DEVICE" -c gain=$gain + fi +} + +write_exposure(){ + if [ "$DEVICE" == "" ] + then + echo "Device needed" + else + exposure=$WRITEEXPOSURECTRL + v4l2-ctl -d "/dev/video$DEVICE" -c coarse_time=$exposure + fi +} + +min_and_max_ctrls(){ + if [ "$DEVICE" == "" ] + then + echo "Device needed" + else + v4l2-ctl -d "/dev/video$DEVICE" -l | grep -w gain + v4l2-ctl -d "/dev/video$DEVICE" -l | grep -w coarse_time + fi +} + +low_light_cond(){ + if [ "$DEVICE" == "" ] + then + echo "Device needed" + else + v4l2-ctl -d "/dev/video$DEVICE" -c gain=520 + v4l2-ctl -d "/dev/video$DEVICE" -c coarse_time=667 + fi +} + +high_light_cond(){ + if [ "$DEVICE" == "" ] + then + echo "Device needed" + else + v4l2-ctl -d "/dev/video$DEVICE" -c gain=66 + v4l2-ctl -d "/dev/video$DEVICE" -c coarse_time=260 + fi +} + +help(){ +echo "***************************************************************************************************************************" +echo "* ______ _ _____ _____ ___ ______ _____ _____ *" +echo "* | ___ \| | | ___||_ _| / _ \ | _ \| ___|/ ___| *" +echo "* | |_/ /| | | |__ | | / /_\ \| | | || |__ \ \__ *" +echo "* | __/ | | | __| | | | _ || | | || __| \___ \ *" +echo "* | | | |____| |___ _| |_ | | | || |/ / | |___ ____/ / *" +echo "* \_| \_____/\____/ \___/ \_| |_/|___/ \____/ \____/ *" +echo "* *" +echo "********************************************* controls-utility Instructions **********************************************" +echo "* *" +echo "* - Controls values: Display the maximum, minimum, actual and default value of the gain and exposure. *" +echo "* Use -mm and set the device. *" +echo "* example: ./controls-utility.sh -d=\"0\" -mm *" +echo "* *" +echo "* - Read gain value: Use the argument \"-rg\", and the device to read current gain value *" +echo "* example: ./controls-utility.sh -d=\"0\" -rg *" +echo "* *" +echo "* - Read exposure value: Use the argument \"-re\", and the device to read current exposure value *" +echo "* example: ./controls-utility.sh -d=\"0\" -re *" +echo "* *" +echo "* - Write gain value: Use the argument \"-wg\", and the device to write the gain desired *" +echo "* example: ./controls-utility.sh -d=\"0\" -wg=\"200\" *" +echo "* *" +echo "* - Write exposure value: Use the argument \"-we\", and the device to write the gain desired *" +echo "* example: ./controls-utility.sh -d=\"0\" -we=\"200\" *" +echo "* *" +echo "* - Low light conditions: Use the argument \"-llc\", and the device *" +echo "* example: ./controls-utility.sh -d=\"0\" -llc *" +echo "* *" +echo "* - High light conditions: Use the argument \"-hlc\", and the device *" +echo "* example: ./controls-utility.sh -d=\"0\" -hlc *" +echo "* *" +echo "* - Help instructions: Use the argument \"--h\" *" +echo "* example: ./controls-utility.sh --h *" +echo "* *" +echo "***************************************************************************************************************************" +} + +if [ "$READGAINCTRL" != "" ] + then + read_gain +fi +if [ "$READEXPOSURECTRL" != "" ] + then + read_exposure +fi +if [ "$WRITEGAINCTRL" != "" ] + then + write_gain +fi +if [ "$WRITEEXPOSURECTRL" != "" ] + then + write_exposure +fi +if [ "$LOWLIGHTCOND" != "" ] + then + low_light_cond +fi +if [ "$HIGHLIGHTCOND" != "" ] + then + high_light_cond +fi +if [ "$MAXMINCTRL" != "" ] + then + min_and_max_ctrls +fi +if [ "$HELP" != "" ] + then + help +fi diff --git a/cameras-manual.launch b/cameras-manual.launch new file mode 100644 index 0000000..6e4d227 --- /dev/null +++ b/cameras-manual.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/cameras.launch b/cameras.launch new file mode 100644 index 0000000..301afef --- /dev/null +++ b/cameras.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/cameras_SLAM.launch b/cameras_SLAM.launch new file mode 100644 index 0000000..cdc2223 --- /dev/null +++ b/cameras_SLAM.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jetson_csi_cam.launch b/jetson_csi_cam.launch index 4230375..4bf0a25 100644 --- a/jetson_csi_cam.launch +++ b/jetson_csi_cam.launch @@ -15,7 +15,7 @@ - diff --git a/stereoSettings.yaml b/stereoSettings.yaml new file mode 100644 index 0000000..d2df7d3 --- /dev/null +++ b/stereoSettings.yaml @@ -0,0 +1,117 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters +#-------------------------------------------------------------------------------------------- + +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 858.08891142058 +Camera.fy: 858.08891142058 +Camera.cx: 599.4950790405273 +Camera.cy: 344.66563415527344 + +Camera.k1: 0.0 +Camera.k2: 0.0 +Camera.p1: 0.0 +Camera.p2: 0.0 + +Camera.height: 720 +Camera.width: 1280 + +# Camera frames per second +Camera.fps: 60.0 + +# stereo baseline times fx +# Spiri baseline = 53mm = 0.053m +Camera.bf: 45.4787123053 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 0 + +# Close/Far threshold. Baseline times. +ThDepth: 35 + +#-------------------------------------------------------------------------------------------- +# Stereo Rectification. Only if you need to pre-rectify the images. +# Camera.fx, .fy, etc must be the same as in LEFT.P +#-------------------------------------------------------------------------------------------- + +LEFT.height: 720 +LEFT.width: 1280 +LEFT.D: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [-0.38274884059323255, 0.13882784820394012, 0.0013228079981012287, 0.000610515166596169, 0.0] +LEFT.K: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [872.0827132010173, 0.0, 629.146800831531, 0.0, 872.9486851144557, 329.7833373150439, 0.0, 0.0, 1.0] +LEFT.R: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [0.9998422733068986, -0.007605901683869659, 0.016049260670464208, 0.007604212038624664, 0.9999710737331147, 0.0001663018737299101, -0.016050061300968175, -4.4233662283887984e-05, 0.9998711884916068] +LEFT.P: !!opencv-matrix + rows: 3 + cols: 4 + dt: d + data: [858.08891142058, 0.0, 599.4950790405273, 0.0, 0.0, 858.08891142058, 344.66563415527344, 0.0, 0.0, 0.0, 1.0, 0.0] + +RIGHT.height: 720 +RIGHT.width: 1280 +RIGHT.D: !!opencv-matrix + rows: 1 + cols: 5 + dt: d + data: [-0.37613139424849784, 0.12947511985930749, 0.0001911664252283371, 0.00020105328605981988, 0.0] +RIGHT.K: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [868.0090164746218, 0.0, 627.0717114982791, 0.0, 868.6735344383518, 357.03758653892595, 0.0, 0.0, 1.0] +RIGHT.R: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [0.9995325381102218, 0.00837959770719308, 0.029402170008415367, -0.00837650207609846, 0.9999648903939145, -0.00022845668221993547, -0.02940305208489884, -1.7937450689705826e-05, 0.9995676366341303] +RIGHT.P: !!opencv-matrix + rows: 3 + cols: 4 + dt: d + data: [858.08891142058, 0.0, 599.4950790405273, -430.0724725519876, 0.0, 858.08891142058, 344.66563415527344, 0.0, 0.0, 0.0, 1.0, 0.0] + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1200 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize:2 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3 +Viewer.ViewpointX: 0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500 diff --git a/video-daylight.launch b/video-daylight.launch new file mode 100644 index 0000000..3f1e0a8 --- /dev/null +++ b/video-daylight.launch @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +