-
Notifications
You must be signed in to change notification settings - Fork 1
/
event_handler_hdr.h
92 lines (75 loc) · 2.54 KB
/
event_handler_hdr.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
/*
* @Author: Shuyang Zhang
* @Date: 2023-07-24 15:35:34
* @LastEditors: ShuyangUni [email protected]
* @LastEditTime: 2023-08-09 17:07:44
* @Description:
*
* Copyright (c) 2023 by Shuyang Zhang, All Rights Reserved.
*/
#pragma once
#include <SpinGenApi/SpinnakerGenApi.h>
#include <Spinnaker.h>
#include <cv_bridge/cv_bridge.h>
#include <ros/ros.h>
#include <Eigen/Dense>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <opencv2/opencv.hpp>
#include "include/common.h"
#include "include/context/contexts.h"
#include "include/event_handler/event_handler.h"
#include "include/hdr_process/camera_response_function.h"
#include "include/hdr_process/function_seed_sampler.h"
#include "include/hdr_process/gp_regressor.h"
#include "include/spinnaker_encap.h"
namespace hdr_attr_ctrl {
class EventHandlerHDR : public EventHandler {
static constexpr int k_expo_idle_ = 0;
static constexpr int k_expo_major_ = 1;
static constexpr int k_expo_low_ = 2;
static constexpr int k_expo_high_ = 4;
public:
EventHandlerHDR(const ros::NodeHandle &nh, const Spinnaker::CameraPtr &cam,
const std::shared_ptr<ContextHDR> &context);
void OnImageEvent(Spinnaker::ImagePtr image);
private:
std::shared_ptr<ContextHDR> context_;
std::shared_ptr<FunctionSeedSampler> seed_sampler_;
std::shared_ptr<GPRegressor> gpr_;
cv::Mat img_cur_;
Exposure expo_cur_;
FuncSamples seeds_cur_;
bool valid_major_;
bool valid_low_;
bool valid_high_;
Exposure expo_major_;
Exposure expo_low_;
Exposure expo_high_;
FuncSamples seeds_major_;
FuncSamples seeds_low_;
FuncSamples seeds_high_;
double expo_val_tar_major_;
double score_tar_major_;
Range range_expo_t_major_;
Range range_gain_major_;
Range range_expo_t_low_;
Range range_gain_low_;
Range range_expo_t_high_;
Range range_gain_high_;
ros::Publisher pub_image_full_;
void InitCtrl();
ExpoState FindExpoState(const Exposure &expo);
bool UpdateSeedsByExpoState(const ExpoState &expo_state,
const Exposure &expo_cur,
const FuncSamples &seeds_cur);
bool AssembleSeeds(const ExpoState &expo_state, FuncSamples *seeds_train);
void FindBestExposure(const FuncSamples &seeds_predict, double *expo_val_opt,
double *score_opt);
bool JudgeWhetherUpdate(const double &expo_val_cur, const double &score_cur);
void UpdateSequenceStates(SequenceStates *states);
void HandleAmbiguity(Exposure *expo, const Range &range_expo_t);
};
} // namespace hdr_attr_ctrl