1 /*
2 * rk_aiq_algo_orb_itf.c
3 *
4 * Copyright (c) 2019 Rockchip Corporation
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 *
18 */
19
20 #include "rk_aiq_algo_aorb_itf.h"
21 #include "rk_aiq_algo_types.h"
22 #include "rk_aiq_types_priv.h"
23
24 #include "orb_algos.h"
25 #include "xcam_log.h"
26
27 #if OPENCV_SUPPORT
28 #include "orb_algos_opencv.h"
29 #endif
30
31 RKAIQ_BEGIN_DECLARE
32
33 typedef struct ORBContext_s {
34 unsigned char initialized;
35 int output_width;
36 int output_height;
37
38 unsigned char orb_en;
39 unsigned char limit_value;
40 unsigned int max_feature;
41
42 double affine_mat_roi[9];
43 orb_rect_t roi_rect;
44 orb_rect_t ref_roi_rect;
45 rk_aiq_isp_orb_stats_t orb_stats;
46
47 ORBList* roi_points_list;
48
49 } ORBContext_t;
50
51 typedef struct ORBContext_s* ORBHandle_t;
52
53 typedef struct _RkAiqAlgoContext {
54 ORBHandle_t hORB;
55 } RkAiqAlgoContext;
56
57 #define ORB_LIMIT_VALUE_DEF 15
58 #define ORB_MAX_FEATURE_DEF 1000
59
60 #if OPENCV_SUPPORT
61 using namespace cv;
62 using namespace std;
63 #endif
64
ORBCreateContext(RkAiqAlgoContext ** context,const AlgoCtxInstanceCfg * cfg)65 XCamReturn ORBCreateContext(RkAiqAlgoContext** context, const AlgoCtxInstanceCfg* cfg) {
66 XCamReturn result = XCAM_RETURN_NO_ERROR;
67
68 LOGI_ORB("%s: (enter)\n", __FUNCTION__);
69 RkAiqAlgoContext* ctx = new RkAiqAlgoContext();
70 if (ctx == NULL) {
71 LOGE_AORB("%s: create aorb context fail!\n", __FUNCTION__);
72 return XCAM_RETURN_ERROR_MEM;
73 }
74 ctx->hORB = new ORBContext_t;
75 if (ctx->hORB == NULL) {
76 delete ctx;
77 LOGE_AORB("%s: create aorb handle fail!\n", __FUNCTION__);
78 return XCAM_RETURN_ERROR_MEM;
79 }
80 memset(ctx->hORB, 0, sizeof(ORBContext_t));
81 *context = ctx;
82
83 return XCAM_RETURN_NO_ERROR;
84 }
85
ORBDestroyContext(RkAiqAlgoContext * context)86 XCamReturn ORBDestroyContext(RkAiqAlgoContext* context) {
87 XCamReturn result = XCAM_RETURN_NO_ERROR;
88
89 DCT_ASSERT(context != NULL);
90 LOGI_ORB("%s: (enter)\n", __FUNCTION__);
91
92 if (NULL != context) {
93 delete context->hORB;
94 context->hORB = NULL;
95 delete context;
96 context = NULL;
97 }
98
99 return XCAM_RETURN_NO_ERROR;
100 }
101
ORBPrepare(RkAiqAlgoCom * params)102 XCamReturn ORBPrepare(RkAiqAlgoCom* params) {
103 XCamReturn result = XCAM_RETURN_NO_ERROR;
104
105 ORBHandle_t hORB = (ORBHandle_t)params->ctx->hORB;
106 ORBContext_t* ORBctx = (ORBContext_t*)hORB;
107 RkAiqAlgoConfigAorb* rkaiqAorbConfig = (RkAiqAlgoConfigAorb*)params;
108
109 ORBctx->orb_en = rkaiqAorbConfig->orb_calib_cfg.param.orb_en;
110
111 ORBctx->limit_value = ORB_LIMIT_VALUE_DEF;
112 ORBctx->max_feature = ORB_MAX_FEATURE_DEF;
113
114 ORBctx->output_width = params->u.prepare.sns_op_width;
115 ORBctx->output_height = params->u.prepare.sns_op_height;
116
117 ORBctx->roi_rect.left = (ORBctx->output_width / 15) * 5;
118 ORBctx->roi_rect.top = (ORBctx->output_height / 15) * 5;
119 ORBctx->roi_rect.width = (ORBctx->output_width / 15) * 5;
120 ORBctx->roi_rect.height = (ORBctx->output_height / 15) * 5;
121 ORBctx->roi_rect.right = ORBctx->roi_rect.left + ORBctx->roi_rect.width;
122 ORBctx->roi_rect.bottom = ORBctx->roi_rect.top + ORBctx->roi_rect.height;
123 ORBctx->ref_roi_rect = ORBctx->roi_rect;
124
125 LOGI_ORB("%s: (enter) enable: %d\n", __FUNCTION__, ORBctx->orb_en);
126
127 return XCAM_RETURN_NO_ERROR;
128 }
129
ORBPreProcess(const RkAiqAlgoCom * inparams,RkAiqAlgoResCom * outparams)130 XCamReturn ORBPreProcess(const RkAiqAlgoCom* inparams, RkAiqAlgoResCom* outparams) {
131 return XCAM_RETURN_NO_ERROR;
132 }
133
ORBProcessing(const RkAiqAlgoCom * inparams,RkAiqAlgoResCom * outparams)134 XCamReturn ORBProcessing(const RkAiqAlgoCom* inparams, RkAiqAlgoResCom* outparams) {
135 XCamReturn result = XCAM_RETURN_NO_ERROR;
136 ORBHandle_t ORBctx = (ORBHandle_t)inparams->ctx->hORB;
137 RkAiqAlgoProcAorb* rkaiqAorbProcParam = (RkAiqAlgoProcAorb*)inparams;
138 RkAiqAlgoProcResAorb* rkaiqAorbProcOut = (RkAiqAlgoProcResAorb*)outparams;
139
140 if (!ORBctx->orb_en) {
141 return XCAM_RETURN_NO_ERROR;
142 }
143
144 LOGI_ORB("%s: (enter)\n", __FUNCTION__);
145
146 RkAiqOrbStats* orbStats = nullptr;
147 if (rkaiqAorbProcParam->orb_stats_buf) {
148 orbStats = (RkAiqOrbStats*)rkaiqAorbProcParam->orb_stats_buf->map(
149 rkaiqAorbProcParam->orb_stats_buf);
150 }
151
152 if (!ORBctx->initialized) {
153 rkaiqAorbProcOut->orb_pre_result.valid = 0;
154 ORBctx->initialized = 1;
155 ORBctx->orb_stats.num_points = 0;
156 } else {
157 if (ORBctx->orb_stats.num_points > 0 && orbStats->orb_stats.num_points > 0) {
158 rkaiqAorbProcOut->orb_pre_result.valid = 1;
159 #if OPENCV_SUPPORT
160 vector<Point2f> m_queryPoints, m_trainPoints;
161
162 push_orbpoint_cv(orbStats->orb_stats.num_points, orbStats->orb_stats.points,
163 m_queryPoints);
164
165 push_orbpoint_cv(ORBctx->orb_stats.num_points, ORBctx->orb_stats.points, m_trainPoints);
166
167 get_homography_matrix(m_queryPoints, m_trainPoints,
168 rkaiqAorbPreOut->orb_pre_result.homography_matrix,
169 HOMOGRAPHY_MATRIX_SIZE);
170
171 ORBctx->orb_stats.homography_matrix = rkaiqAorbPreOut->orb_pre_result.homography_matrix;
172 #else
173
174 #if CALC_AFFINE_TRANSFORM
175 int ret = 0;
176
177 if (ORBctx->roi_points_list == NULL) {
178 ORBctx->roi_points_list = get_roi_points_list(&ORBctx->orb_stats, ORBctx->roi_rect);
179 }
180
181 if ((ORBctx->roi_points_list->length < 6)) {
182 LOGD_AORB("reset keypoint_num: %d", ORBctx->roi_points_list->length);
183 freeList(ORBctx->roi_points_list);
184 ORBctx->roi_points_list = NULL;
185 ORBctx->orb_stats = orbStats->orb_stats;
186 } else {
187 ORBList* matched_list = matching(ORBctx->roi_points_list,
188 &rkaiqAorbPreParam->orb_stats, ORBctx->roi_rect);
189
190 ret = elimate_affine_transform(matched_list, ORBctx->affine_mat_roi);
191 if (ret < 0) {
192 rkaiqAorbPreOut->orb_pre_result.valid = 0;
193 } else {
194 for (int i = 0; i < 3; i++) {
195 LOGD_AORB("---[%f, %f, %f]", ORBctx->affine_mat_roi[i * 3],
196 ORBctx->affine_mat_roi[i * 3 + 1],
197 ORBctx->affine_mat_roi[i * 3 + 2]);
198 }
199 }
200
201 // fill result params
202 if (rkaiqAorbPreOut->orb_pre_result.valid) {
203 ORBctx->ref_roi_rect = map_rect(ORBctx->affine_mat_roi, &ORBctx->roi_rect);
204 rkaiqAorbPreOut->orb_pre_result.tracking_roi_rect = ORBctx->ref_roi_rect;
205 // ORBctx->roi_rect = ORBctx->ref_roi_rect;
206
207 LOGD_AORB("keypoint_num: %d, matched_list: %d, roi=[%d-%d-%d-%d]",
208 ORBctx->roi_points_list->length, matched_list->length,
209 ORBctx->ref_roi_rect.left, ORBctx->ref_roi_rect.top,
210 ORBctx->ref_roi_rect.right, ORBctx->ref_roi_rect.bottom);
211 } else {
212 ORBctx->ref_roi_rect = ORBctx->roi_rect;
213 // ORBctx->orb_stats = orbStats->orb_stats;
214 }
215
216 orb_matched_point_t* out_matched_keypoints =
217 rkaiqAorbPreOut->orb_pre_result.matched_keypoints;
218 rkaiqAorbPreOut->orb_pre_result.matched_keypoints_num = 0;
219
220 int index = 0;
221 for (Node* point = matched_list->start; point != NULL; point = point->next) {
222 orb_matched_point_t* keypoints = (orb_matched_point_t*)point->data;
223 out_matched_keypoints[index] = *keypoints;
224 rkaiqAorbPreOut->orb_pre_result.matched_keypoints_num++;
225 index++;
226 }
227
228 freeList(matched_list);
229 // freeList(roi_points_list);
230 }
231 #endif
232 #endif
233 } else {
234 ORBctx->orb_stats = orbStats->orb_stats;
235 }
236 }
237
238 // if (inparams->u.proc.init) {
239 rkaiqAorbProcOut->aorb_meas.update = 1;
240 rkaiqAorbProcOut->aorb_meas.orb_en = ORBctx->orb_en;
241 rkaiqAorbProcOut->aorb_meas.limit_value = ORBctx->limit_value;
242 rkaiqAorbProcOut->aorb_meas.max_feature = ORBctx->max_feature;
243 // } else {
244 // rkaiqAorbProcOut->aorb_meas.update = 0;
245
246 return XCAM_RETURN_NO_ERROR;
247 }
248
ORBPostProcess(const RkAiqAlgoCom * inparams,RkAiqAlgoResCom * outparams)249 XCamReturn ORBPostProcess(const RkAiqAlgoCom* inparams, RkAiqAlgoResCom* outparams) {
250 return XCAM_RETURN_NO_ERROR;
251 }
252
253 RkAiqAlgoDescription g_RkIspAlgoDescAorb = {
254 .common =
255 {
256 .version = RKISP_ALGO_ORB_VERSION,
257 .vendor = RKISP_ALGO_ORB_VENDOR,
258 .description = RKISP_ALGO_ORB_DESCRIPTION,
259 .type = RK_AIQ_ALGO_TYPE_AORB,
260 .id = 0,
261 .create_context = ORBCreateContext,
262 .destroy_context = ORBDestroyContext,
263 },
264 .prepare = ORBPrepare,
265 .pre_process = ORBPreProcess,
266 .processing = ORBProcessing,
267 .post_process = ORBPostProcess,
268 };
269
270 RKAIQ_END_DECLARE
271