Skip to content

added #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Rennylex wants to merge 3 commits into
base: master
Choose a base branch
from
Open

added #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
0f03279
added
Jul 20, 2023
50a4666
Update changeParam.launch
Rennylex Jul 20, 2023
62eb631
Update changeParam.launch
Rennylex Jul 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 7 additions & 2 deletions launch/changeParam.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,14 @@
<node pkg="exposure_control" type="changeParam" name="changeParam" args="" output="screen" respawn="false" >
</node>
<!-- <param name="deltaT" type="double" value="7000" /> -->
<param name="step_length_aec_" type="double" value="0.000000001" />
<param name="step_length_aec_" type="double" value=" 0.000000000001" />
<!-- 0.000000000001for bright marker,0.000000001for dark -->


<param name="active_aec_" type="bool" value="false" />
<param name="step_length_gec_" type="double" value="0.00000001" />
<!-- 0.00000001for bright marker,0.000000001for dark,"0.000000001fordmbbg" -->

<param name="active_aec_" type="bool" value="true" />

<param name="active_gec_" type="bool" value="false" />

Expand Down
17 changes: 8 additions & 9 deletions launch/stag_and_fisheye_with_exposure_control.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,13 @@


<node pkg="rosbag" type="record" name="rosbag_cam_record"
args="record -o /tmp/cameras /camera_array/cam0/image_raw/compressed /bluerov_controller/ar_tag_detector_2

args="record -o /home/rlab/lex_data /camera_array/cam0/image_raw/compressed /bluerov_controller/ar_tag_detector_2
"/>



<!--<include file="$(find droplet_underwater_assembly)/description.launch" /> -->
<include file="$(find storage_0)/description.launch" />
<include file="$(find exposure_control)/launch/description.launch" />
<!--<node pkg="nodelet" type="nodelet" name="bluerov_nodelet_manager" args="manager" output="screen" respawn="true"> -->
<node pkg="nodelet" type="nodelet" name="bluerov_nodelet_manager" args="manager" output="screen" respawn="true">
<param name="num_worker_threads" value="1" />
Expand All @@ -20,7 +19,7 @@
<include file="$(find spinnaker_sdk_camera_driver)/launch/acquisition_node.launch">
<!--<arg name="nodelet_manager_name" value="bluerov_nodelet_manager"/> -->
<!--<arg name="config_file" value="$(find downward_camera_localizer)/param/flir_senko_land_calibration.yaml"/> -->
<arg name="config_file" value="$(find storage_0)/param/flir_senko_land_calibration.yaml"/>
<arg name="config_file" value="$(find exposure_control)/param/flir_senko_land_calibration.yaml"/>
<!--<arg name="start_nodelet_manager" value="true"/>-->
<arg name="color" value="false"/>
<arg name="exposure_time" value="7000"/>
Expand All @@ -35,23 +34,23 @@
<param name="camera_info_topic" type="string" value="/camera_array/cam0/camera_info" />
<param name="tag_id_type" type="int" value="21" />
<param name="marker_frame_prefix" type="string" value="/ar_marker_" />
<param name="default_marker_size" type="double" value="0.100" />
<param name="output_frame_id" type="string" value="/base_link" />
<param name="default_marker_size" type="double" value="0.162" />
<param name="output_frame_id" type="string" value="/cam_0_optical_frame" />
<param name="image_frame_id" type="string" value="/cam_0_optical_frame" />
<param name="marker_message_topic" type="string" value="/bluerov_controller/ar_tag_detector_2" />
<param name="process_images_in_parallel" type="bool" value="false" />
<param name="use_marker_bundles" type="bool" value="false" />
<param name="publish_debug_images" type="bool" value="true" />
<!--<rosparam command="load" file="$(find droplet_underwater_assembly)/param/bundle-four-stags-aluminum.json" /> -->
<!--<rosparam command="load" param="marker_sizes_by_id" file="$(find droplet_underwater_assembly)/param/marker_sizes_by_id.yaml" /> -->
<rosparam command="load" file="$(find storage_0)/param/bundle-four-stags-aluminum-and-large-six-tags.json" />
<rosparam command="load" param="marker_sizes_by_id" file="$(find storage_0)/param/marker_sizes_by_id.yaml" />
<rosparam command="load" file="$(find exposure_control)/param/bundle-four-stags-aluminum-and-large-six-tags.json" />
<rosparam command="load" param="marker_sizes_by_id" file="$(find exposure_control)/param/marker_sizes_by_id.yaml" />

<param name="marker_track_height_offset" type="int" value="50" />
<param name="marker_track_width_offset" type="int" value="50" />
<param name="track_markers" type="bool" value="false" />

<param name="is_fisheye" type="bool" value="true" />
<param name="is_fisheye" type="bool" value="false" />
<param name="undistort_rectify_alpha" type="double" value="0.030" />
</node>

Expand Down
19 changes: 16 additions & 3 deletions param/flir_senko_land_calibration.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,16 +22,29 @@ distortion_model: plumb_bob
image_height: 1080
image_width: 1440
distortion_coeffs:
- [-0.296079, 0.099771, 0.000222, 0.000109, 0.000000]
- [-0.2829269707979299, 0.09063489416915381, -0.0010544086139528514, 0.0008801177625359006, 0.0]


# [-0.296079, 0.099771, 0.000222, 0.000109, 0.000000]



#specified as [fx 0 cx 0 fy cy 0 0 1]
intrinsic_coeffs:
- [1173.854081, 0.000000, 747.788206, 0.000000, 1170.565083, 574.700374, 0.000000, 0.000000, 1.000000]
- [982.0513584835674, 0.0, 703.4619839309272, 0.0, 982.5738872661499, 557.3461327093406, 0.0, 0.0, 1.0]

#[1173.854081, 0.000000, 747.788206, 0.000000, 1170.565083, 574.700374, 0.000000, 0.000000, 1.000000]

#[1173.854081, 0.000000, 747.788206, 0.000000, 1170.565083, 574.700374, 0.000000, 0.000000, 1.000000]
#[1168.015284, 0.000000, 743.458457, 0.000000, 1171.654497, 566.096535, 0.000000, 0.000000, 1.000000]

rectification_coeffs:
- [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]

#- [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]

projection_coeffs:
- [1030.774170, 0.000000, 755.305608, 0.000000, 0.000000, 1090.456787, 579.516081, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
- [820.6452026367188, 0.0, 699.7615677625763, 0.0, 0.0, 891.7987670898438, 559.1087947706983, 0.0, 0.0, 0.0, 1.0, 0.0]

# - [1030.774170, 0.000000, 755.305608, 0.000000, 0.000000, 1090.456787, 579.516081, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

2 changes: 1 addition & 1 deletion param/marker_sizes_by_id.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1 +1 @@
[{size: 0.1685, id: 0}, {size: 0.1685, id: 11}, {size: 0.1685, id: 1}, {size: 0.1685, id: 7}, {size: 0.1685, id: 2}, {size: 0.1685, id: 3}, {size: 0.1685, id: 5}, {size: 0.1685, id: 8}, {size: 0.1685, id: 9}]
[{size: 0.1685, id: 0}, {size: 0.1685, id: 11}, {size: 0.1685, id: 1}, {size: 0.1685, id: 7}, {size: 0.1685, id: 2}, {size: 0.1685, id: 3}, {size: 0.1685, id: 5}, {size: 0.1685, id: 8}, {size: 0.162, id: 9}]
126 changes: 103 additions & 23 deletions src/changeParam.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@ using namespace std;





using namespace cv;

double deltaT_now;
Expand All @@ -36,10 +38,15 @@ int width;

int count_skip = 0;
double step_len_aec;//=0.000000001;
double step_len_gec;
bool active_aec;// = false;
bool active_gec;// = false;
double max_bound;//=70000;

double momentum_pre=0;
image_transport::Publisher pub_image;
// ros::NodeHandle n;

void set_exposure_param(double new_deltaT){


Expand Down Expand Up @@ -236,6 +243,8 @@ double get_gamma(Mat img){
//normalize(grad1, grad1, 0, 1, NORM_MINMAX);
//double max_score=grad_score2(grad1);
double max_score=grad_score(grad1);
double second_max_score=0;


for(int i=0;i<11;i++){
cv::Mat img_powed;
Expand All @@ -251,11 +260,14 @@ double get_gamma(Mat img){
double score=grad_score(grad);
//ROS_INFO("score: %f, %d",score,i);
if(score>max_score){
second_max_score=max_score;
max_score=score;
//ROS_INFO("max_score: %f",max_score);
gamma=gamma_anchor[i];
}
}
ROS_INFO("max_score: %f",max_score);
ROS_INFO("second_max_score: %f",second_max_score);
return gamma;
}

Expand Down Expand Up @@ -285,6 +297,7 @@ double update_exposure_nonlinear(double step_length, double exposure_time, doubl


double new_exposure_time = exposure_time *(1+alpha*step_length*(R-1));
ROS_INFO("New exposure time: %f",new_exposure_time*1000000);
return new_exposure_time;
}

Expand Down Expand Up @@ -334,19 +347,35 @@ void tag_callback(const stag_ros::StagMarkers::ConstPtr& msg)

//find the min and max of the x and y
//make sure it's int
int padding_x=std::abs((upperleft_x-lowerleft_x));
int padding_y=std::abs((upperleft_y-upperright_y));
min_x = (int)std::min(std::min(upperleft_x,lowerleft_x),std::min(upperright_x,lowerright_x))-150;//-padding_x/8;//10;//25;
min_y = (int)std::min(std::min(upperleft_y,upperright_y),std::min(lowerleft_y,lowerright_y))-150;//-padding_y/8;//10;//25;
max_x = (int)std::max(std::max(upperright_x,lowerright_x),std::max(lowerleft_x,upperleft_x))+150;//+padding_x/8;//10;//25;
max_y = (int)std::max(std::max(upperright_y,lowerright_y),std::max(lowerleft_y,upperleft_y))+150;//+padding_y/8;//10;//25;
min_x = (int)std::min(std::min(upperleft_x,lowerleft_x),std::min(upperright_x,lowerright_x));//-200;//-padding_x/8;//10;//25;
min_y = (int)std::min(std::min(upperleft_y,upperright_y),std::min(lowerleft_y,lowerright_y));//-200;//-padding_y/8;//10;//25;
max_x = (int)std::max(std::max(upperright_x,lowerright_x),std::max(lowerleft_x,upperleft_x));//+200;//+padding_x/8;//10;//25;
max_y = (int)std::max(std::max(upperright_y,lowerright_y),std::max(lowerleft_y,upperleft_y));//+200;//+padding_y/8;//10;//25;


ROS_INFO("MAX_X: [%d]", max_x);
ROS_INFO("MIN_X: [%d]", min_x);


//padding
float padding_x=(max_x-min_x)*0.1;
float padding_y=(max_y-min_y)*0.1;

min_x=min_x-padding_x;
min_y=min_y-padding_y;
max_x=max_x+padding_x;
max_y=max_y+padding_y;

min_x=std::max(0,min_x);
min_y=std::max(0,min_y);

max_x=std::min(height,max_x);
max_y=std::min(width,max_y);

ROS_INFO("MAX_X: [%d]", max_x);
ROS_INFO("MIN_X: [%d]", min_x);
ROS_INFO("MAX_Y: [%d]", max_y);
ROS_INFO("MIN_Y: [%d]", min_y);


}
Expand All @@ -370,7 +399,17 @@ void tag_callback(const stag_ros::StagMarkers::ConstPtr& msg)




//publish the image
void publish_image(Mat image, ros::Publisher pub_image, ros::Time timestamp){
//convert image to CV_8UC1
image.convertTo(image, CV_8UC1);
//convert image to ros msg
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", image).toImageMsg();
//set the timestamp
msg->header.stamp = timestamp;
//publish the image
pub_image.publish(msg);
}


void update_exposure(double deltaT){
Expand Down Expand Up @@ -412,20 +451,33 @@ void update_exposure(double deltaT){
// max_x = image.cols;
// max_y = image.rows;
//Mat image0 = frames_[0];

// //print the size of image
// ROS_INFO("I heard image.cols: [%d]", image.cols);
// ROS_INFO("I heard image.rows: [%d]", image.rows);



// ROS_INFO("min_x: [%d]", min_x);
// ROS_INFO("min_y: [%d]", min_y);
// ROS_INFO("max_x: [%d]", max_x);
// ROS_INFO("max_y: [%d]", max_y);
//crop the image
Mat image_with_box = img.clone();

// image_with_box.convertTo(image_with_box, CV_8UC3);
//convert to rgb
cvtColor(image_with_box, image_with_box, CV_GRAY2RGB);


image = image(Rect(min_x,min_y,max_x-min_x,max_y-min_y));

//draw the bounding box using red color
rectangle(image_with_box, Point(min_x,min_y), Point(max_x,max_y), Scalar(255,0,0), 2, 8, 0);

//publish the image with bounding box
//define a publisher
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "rgb8", image_with_box).toImageMsg();
pub_image.publish(msg);
//print the min and max
// ROS_INFO("I heard min_x: [%d]", min_x);
// ROS_INFO("I heard min_y: [%d]", min_y);
Expand Down Expand Up @@ -546,7 +598,7 @@ void update_exposure(double deltaT){
// //else
// //W[i]=(1/N)*sin(pi/2-(pi*i)/(2*(len(Gradient)-scale_down(p*len(Gradient)))))^k
// //k=5
double p=0.7;
double p=0.9;
vector<double> W;
//int N = gradient_result.rows;
//N normalize the sum of W to 1
Expand Down Expand Up @@ -686,22 +738,40 @@ void update_exposure(double deltaT){
}



ROS_INFO("pMpdT is %f", pMpdT);
// //step 5: update deltaT using the gradient descent method
// //deltaT = deltaT - alpha*pMpdT
double alpha =step_len_aec;//0.000000001;//0.000001;//0.0000001; //0.00000000005;//0.0000000001;//0.000000005;//0.00000005
double nextdeltaT = deltaT;
if(abs(pMpdT)>200)
nextdeltaT = deltaT + alpha * pMpdT;
ROS_INFO_STREAM("Attempted Exposure time: " << nextdeltaT*1000000);
exposure_time_=nextdeltaT*1000000;
if(exposure_time_<=12)
exposure_time_=13;

if(exposure_time_>max_bound)
exposure_time_=max_bound;
//cams[0].setFloatValue(exposure_time_);
set_exposure_param(exposure_time_);
// if(abs(pMpdT)>15000 && abs(alpha*pMpdT)*1000000>1){

// nextdeltaT = deltaT + alpha * pMpdT;

// }

//MOMENTUM VERSION
double momentum = 0.9*momentum_pre+alpha*pMpdT;
ROS_INFO_STREAM("momentum is "<<momentum*1000000);

if(abs(momentum)*1000000>1){

momentum_pre=momentum;
nextdeltaT = deltaT + momentum;


//nextdeltaT = deltaT + alpha * pMpdT;
}
//MOMENTUM VERSION ENDS
ROS_INFO_STREAM("Attempted Exposure time: " << nextdeltaT*1000000);
exposure_time_=nextdeltaT*1000000;
if(exposure_time_<=12)
exposure_time_=13;

if(exposure_time_>max_bound)
exposure_time_=max_bound;
//cams[0].setFloatValue(exposure_time_);
set_exposure_param(exposure_time_);
//cams[0].setFloatValue("AutoExposureTargetGreyValue", 90);
}//

Expand All @@ -717,6 +787,11 @@ void update_exposure(double deltaT){
// min_y=min_y-50;
// max_x+=50;
// max_y+=50;

min_x=max(0,min_x-25);
min_y=max(0,min_y-25);
max_x=min(max_x+25,image.cols);
max_y=min(max_y+25,image.rows);
image = image(Rect(min_x,min_y,max_x-min_x,max_y-min_y));
// //compress the image in half
//resize(image, image, Size(), 1, 1, INTER_LINEAR);
Expand All @@ -731,7 +806,7 @@ void update_exposure(double deltaT){


double gamma =0;
double step_len=0.4;
double step_len=step_len_gec;


gamma=get_gamma(image);
Expand Down Expand Up @@ -832,6 +907,9 @@ int main(int argc, char** argv) {

n.getParam("step_length_aec_", step_len_aec);


n.getParam("step_length_gec_", step_len_gec);

n.getParam("active_aec_", active_aec);

n.getParam("active_gec_", active_gec);
Expand All @@ -840,7 +918,9 @@ int main(int argc, char** argv) {
ROS_INFO("Got upper_bound: %f",max_bound);



image_transport::ImageTransport it(n);
pub_image = it.advertise("camera_array/cam0/image_with_box", 1);



//subscribe to /camera_array/cam0/image_raw
Expand Down