diff --git a/launch/changeParam.launch b/launch/changeParam.launch
index 089d90f..8c0085d 100755
--- a/launch/changeParam.launch
+++ b/launch/changeParam.launch
@@ -3,9 +3,14 @@
-
+
+
+
-
+
+
+
+
diff --git a/launch/stag_and_fisheye_with_exposure_control.launch b/launch/stag_and_fisheye_with_exposure_control.launch
index 5661d7d..514a2aa 100644
--- a/launch/stag_and_fisheye_with_exposure_control.launch
+++ b/launch/stag_and_fisheye_with_exposure_control.launch
@@ -4,14 +4,13 @@
-
+
@@ -20,7 +19,7 @@
-
+
@@ -35,8 +34,8 @@
-
-
+
+
@@ -44,14 +43,14 @@
-
-
+
+
-
+
diff --git a/param/flir_senko_land_calibration.yaml b/param/flir_senko_land_calibration.yaml
index 698d987..249e68f 100644
--- a/param/flir_senko_land_calibration.yaml
+++ b/param/flir_senko_land_calibration.yaml
@@ -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]
+
diff --git a/param/marker_sizes_by_id.yaml b/param/marker_sizes_by_id.yaml
index b625914..2409369 100644
--- a/param/marker_sizes_by_id.yaml
+++ b/param/marker_sizes_by_id.yaml
@@ -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}]
diff --git a/src/changeParam.cpp b/src/changeParam.cpp
index c5d99fc..bb7b399 100755
--- a/src/changeParam.cpp
+++ b/src/changeParam.cpp
@@ -22,6 +22,8 @@ using namespace std;
+
+
using namespace cv;
double deltaT_now;
@@ -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){
@@ -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;
@@ -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;
}
@@ -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;
}
@@ -334,12 +347,24 @@ 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);
@@ -347,6 +372,10 @@ void tag_callback(const stag_ros::StagMarkers::ConstPtr& msg)
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);
}
@@ -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){
@@ -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);
@@ -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 W;
//int N = gradient_result.rows;
//N normalize the sum of W to 1
@@ -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 "<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);
}//
@@ -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);
@@ -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);
@@ -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);
@@ -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