Adding Spiri settings to start the cameras
This commit is contained in:
parent
b4d839bdfc
commit
7ddf6735fe
20
calibration-left.yaml
Normal file
20
calibration-left.yaml
Normal file
@ -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]
|
20
calibration-right.yaml
Normal file
20
calibration-right.yaml
Normal file
@ -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]
|
209
camera-controls-utility.sh
Executable file
209
camera-controls-utility.sh
Executable file
@ -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
|
48
cameras-manual.launch
Normal file
48
cameras-manual.launch
Normal file
@ -0,0 +1,48 @@
|
||||
<launch>
|
||||
|
||||
<!-- Command Line Arguments -->
|
||||
<arg name="sync_sink" default="true" /> <!-- Synchronize the app sink -->
|
||||
<arg name="width" default="1280" /> <!-- Image Width -->
|
||||
<arg name="height" default="720" /> <!-- Image Height -->
|
||||
<arg name="fps" default="60" /> <!-- Desired framerate -->
|
||||
|
||||
<!-- Make arguments available to parameter server -->
|
||||
<param name="image_width" type="int" value="$(arg width)" />
|
||||
<param name="image_height" type="int" value="$(arg height)" />
|
||||
<param name="target_fps" type="int" value="$(arg fps)" />
|
||||
|
||||
<group ns="stereo">
|
||||
<node name="right" pkg="gscam" type="gscam" >
|
||||
<param name="camera_name" value="right" />
|
||||
<param name="gscam_config" value="nvcamerasrc sensor-id=0 auto-exposure=1 aeLock=1 ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)I420,framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
|
||||
<param name="camera_id" value="0" />
|
||||
<param name="frame_id" value="right_link" />
|
||||
<param name="sync_sink" value="true" />
|
||||
<param name="right/image_width" type="int" value="$(arg width)" />
|
||||
<param name="right/image_height" type="int" value="$(arg height)" />
|
||||
<param name="right/target_fps" type="int" value="$(arg fps)" />
|
||||
<param name="camera_info_url" value="package://jetson_csi_cam/calibration-right.yaml" />
|
||||
<remap from="camera/image_raw" to="right/image_raw" />
|
||||
<remap from="camera/camera_info" to="right/camera_info" />
|
||||
<remap from="set_camera_info" to="camera_info" />
|
||||
</node>
|
||||
|
||||
<node name="left" pkg="gscam" type="gscam" >
|
||||
<param name="camera_name" value="left" />
|
||||
<param name="gscam_config" value="nvcamerasrc sensor-id=1 auto-exposure=1 aeLock=1 ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)I420, framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
|
||||
<param name="frame_id" value="left_link" />
|
||||
<param name="sync_sink" value="true" />
|
||||
<param name="left/image_width" type="int" value="$(arg width)" />
|
||||
<param name="left/image_height" type="int" value="$(arg height)" />
|
||||
<param name="left/target_fps" type="int" value="$(arg fps)" />
|
||||
<param name="camera_info_url" value="package://jetson_csi_cam/calibration-left.yaml" />
|
||||
<remap from="camera/image_raw" to="left/image_raw" />
|
||||
<remap from="camera/camera_info" to="left/camera_info" />
|
||||
<remap from="/set_camera_info" to="/left/set_camera_info" />
|
||||
</node>
|
||||
|
||||
<node name="both" pkg="stereo_image_proc" type="stereo_image_proc" />
|
||||
|
||||
</group>
|
||||
|
||||
</launch>
|
48
cameras.launch
Normal file
48
cameras.launch
Normal file
@ -0,0 +1,48 @@
|
||||
<launch>
|
||||
|
||||
<!-- Command Line Arguments -->
|
||||
<arg name="sync_sink" default="true" /> <!-- Synchronize the app sink -->
|
||||
<arg name="width" default="1280" /> <!-- Image Width -->
|
||||
<arg name="height" default="720" /> <!-- Image Height -->
|
||||
<arg name="fps" default="60" /> <!-- Desired framerate -->
|
||||
|
||||
<!-- Make arguments available to parameter server -->
|
||||
<param name="image_width" type="int" value="$(arg width)" />
|
||||
<param name="image_height" type="int" value="$(arg height)" />
|
||||
<param name="target_fps" type="int" value="$(arg fps)" />
|
||||
|
||||
<group ns="stereo">
|
||||
<node name="right" pkg="gscam" type="gscam" >
|
||||
<param name="camera_name" value="right" />
|
||||
<param name="gscam_config" value="nvcamerasrc sensor-id=0 auto-exposure=2 aeLock=0 ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)I420,framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
|
||||
<param name="camera_id" value="0" />
|
||||
<param name="frame_id" value="right_link" />
|
||||
<param name="sync_sink" value="true" />
|
||||
<param name="right/image_width" type="int" value="$(arg width)" />
|
||||
<param name="right/image_height" type="int" value="$(arg height)" />
|
||||
<param name="right/target_fps" type="int" value="$(arg fps)" />
|
||||
<param name="camera_info_url" value="package://jetson_csi_cam/calibration-right.yaml" />
|
||||
<remap from="camera/image_raw" to="right/image_raw" />
|
||||
<remap from="camera/camera_info" to="right/camera_info" />
|
||||
<remap from="set_camera_info" to="camera_info" />
|
||||
</node>
|
||||
|
||||
<node name="left" pkg="gscam" type="gscam" >
|
||||
<param name="camera_name" value="left" />
|
||||
<param name="gscam_config" value="nvcamerasrc sensor-id=1 auto-exposure=2 aeLock=0 ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)I420, framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
|
||||
<param name="frame_id" value="left_link" />
|
||||
<param name="sync_sink" value="true" />
|
||||
<param name="left/image_width" type="int" value="$(arg width)" />
|
||||
<param name="left/image_height" type="int" value="$(arg height)" />
|
||||
<param name="left/target_fps" type="int" value="$(arg fps)" />
|
||||
<param name="camera_info_url" value="package://jetson_csi_cam/calibration-left.yaml" />
|
||||
<remap from="camera/image_raw" to="left/image_raw" />
|
||||
<remap from="camera/camera_info" to="left/camera_info" />
|
||||
<remap from="/set_camera_info" to="/left/set_camera_info" />
|
||||
</node>
|
||||
|
||||
<node name="both" pkg="stereo_image_proc" type="stereo_image_proc" />
|
||||
|
||||
</group>
|
||||
|
||||
</launch>
|
48
cameras_SLAM.launch
Normal file
48
cameras_SLAM.launch
Normal file
@ -0,0 +1,48 @@
|
||||
<launch>
|
||||
|
||||
<!-- Command Line Arguments -->
|
||||
<arg name="sync_sink" default="true" /> <!-- Synchronize the app sink -->
|
||||
<arg name="width" default="320" /> <!-- Image Width -->
|
||||
<arg name="height" default="240" /> <!-- Image Height -->
|
||||
<arg name="fps" default="60" /> <!-- Desired framerate -->
|
||||
|
||||
<!-- Make arguments available to parameter server -->
|
||||
<param name="image_width" type="int" value="$(arg width)" />
|
||||
<param name="image_height" type="int" value="$(arg height)" />
|
||||
<param name="target_fps" type="int" value="$(arg fps)" />
|
||||
|
||||
<group ns="stereo">
|
||||
<node name="right" pkg="gscam" type="gscam" >
|
||||
<param name="camera_name" value="right" />
|
||||
<param name="gscam_config" value="nvcamerasrc sensor-id=0 auto-exposure=2 aeLock=0 ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)I420,framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw-yuv, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
|
||||
<param name="camera_id" value="0" />
|
||||
<param name="frame_id" value="right_link" />
|
||||
<param name="sync_sink" value="true" />
|
||||
<param name="right/image_width" type="int" value="$(arg width)" />
|
||||
<param name="right/image_height" type="int" value="$(arg height)" />
|
||||
<param name="right/target_fps" type="int" value="$(arg fps)" />
|
||||
<param name="camera_info_url" value="package://jetson_csi_cam/calibration-right.yaml" />
|
||||
<remap from="camera/image_raw" to="right/image_raw" />
|
||||
<remap from="camera/camera_info" to="right/camera_info" />
|
||||
<remap from="set_camera_info" to="camera_info" />
|
||||
</node>
|
||||
|
||||
<node name="left" pkg="gscam" type="gscam" >
|
||||
<param name="camera_name" value="left" />
|
||||
<param name="gscam_config" value="nvcamerasrc sensor-id=1 auto-exposure=2 aeLock=0 ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)I420, framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw-yuv, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
|
||||
<param name="frame_id" value="left_link" />
|
||||
<param name="sync_sink" value="true" />
|
||||
<param name="left/image_width" type="int" value="$(arg width)" />
|
||||
<param name="left/image_height" type="int" value="$(arg height)" />
|
||||
<param name="left/target_fps" type="int" value="$(arg fps)" />
|
||||
<param name="camera_info_url" value="package://jetson_csi_cam/calibration-left.yaml" />
|
||||
<remap from="camera/image_raw" to="left/image_raw" />
|
||||
<remap from="camera/camera_info" to="left/camera_info" />
|
||||
<remap from="/set_camera_info" to="/left/set_camera_info" />
|
||||
</node>
|
||||
|
||||
<!--<node name="both" pkg="stereo_image_proc" type="stereo_image_proc" />-->
|
||||
|
||||
</group>
|
||||
|
||||
</launch>
|
@ -15,7 +15,7 @@
|
||||
<param name="$(arg cam_name)/target_fps" type="int" value="$(arg fps)" />
|
||||
|
||||
<!-- Define the GSCAM pipeline -->
|
||||
<env name="GSCAM_CONFIG" value="nvcamerasrc sensor-id=$(arg sensor_id) ! video/x-raw(memory:NVMM),
|
||||
<env name="GSCAM_CONFIG" value="nvarguscamerasrc sensor-id=$(arg sensor_id) ! video/x-raw(memory:NVMM),
|
||||
width=(int)$(arg width), height=(int)$(arg height), format=(string)I420, framerate=(fraction)$(arg fps)/1 !
|
||||
nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
|
||||
|
||||
|
117
stereoSettings.yaml
Normal file
117
stereoSettings.yaml
Normal file
@ -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
|
47
video-daylight.launch
Normal file
47
video-daylight.launch
Normal file
@ -0,0 +1,47 @@
|
||||
<launch>
|
||||
|
||||
<!-- Command Line Arguments -->
|
||||
<arg name="sync_sink" default="true" /> <!-- Synchronize the app sink -->
|
||||
<arg name="width" default="1280" /> <!-- Image Width -->
|
||||
<arg name="height" default="720" /> <!-- Image Height -->
|
||||
<arg name="fps" default="30" /> <!-- Desired framerate -->
|
||||
|
||||
<!-- Make arguments available to parameter server -->
|
||||
<param name="image_width" type="int" value="$(arg width)" />
|
||||
<param name="image_height" type="int" value="$(arg height)" />
|
||||
<param name="target_fps" type="int" value="$(arg fps)" />
|
||||
|
||||
<group ns="stereo">
|
||||
<node name="right" pkg="gscam" type="gscam" >
|
||||
<param name="camera_name" value="right" />
|
||||
<param name="gscam_config" value="nvcamerasrc sensor-id=1 aeLock=0 wbmode=5 scene-mode=3 intent=3 contrast=0.8 ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)I420,framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
|
||||
<param name="frame_id" value="right_link" />
|
||||
<param name="sync_sink" value="true" />
|
||||
<param name="right/image_width" type="int" value="$(arg width)" />
|
||||
<param name="right/image_height" type="int" value="$(arg height)" />
|
||||
<param name="right/target_fps" type="int" value="$(arg fps)" />
|
||||
<param name="camera_info_url" value="package://jetson_csi_cam/calibration-right.yaml" />
|
||||
<remap from="camera/image_raw" to="right/image_raw" />
|
||||
<remap from="camera/camera_info" to="right/camera_info" />
|
||||
<!-- <remap from="/set_camera_info" to="/right/set_camera_info" /> -->
|
||||
</node>
|
||||
|
||||
<node name="left" pkg="gscam" type="gscam" >
|
||||
<param name="camera_name" value="left" />
|
||||
<param name="gscam_config" value="nvcamerasrc sensor-id=0 aeLock=0 wbmode=5 scene-mode=3 intent=3 contrast=0.8 ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)I420, framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
|
||||
<param name="frame_id" value="left_link" />
|
||||
<param name="sync_sink" value="true" />
|
||||
<param name="left/image_width" type="int" value="$(arg width)" />
|
||||
<param name="left/image_height" type="int" value="$(arg height)" />
|
||||
<param name="left/target_fps" type="int" value="$(arg fps)" />
|
||||
<param name="camera_info_url" value="package://jetson_csi_cam/calibration-left.yaml" />
|
||||
<remap from="camera/image_raw" to="left/image_raw" />
|
||||
<remap from="camera/camera_info" to="left/camera_info" />
|
||||
<!-- <remap from="/set_camera_info" to="/left/set_camera_info" /> -->
|
||||
</node>
|
||||
|
||||
<node name="both" pkg="stereo_image_proc" type="stereo_image_proc" />
|
||||
|
||||
</group>
|
||||
|
||||
</launch>
|
Loading…
Reference in New Issue
Block a user