1 /*
2 * Copyright 2020 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include "CameraUtils.h"
18
19 #include <android-base/logging.h>
20 #include <android/hardware/automotive/evs/1.1/types.h>
21
22 #include <math.h>
23
24 using namespace android::hardware::automotive::evs::V1_1;
25
26 using ::android::sp;
27 using ::std::string;
28 using ::std::vector;
29 using ::std::map;
30
31 namespace android {
32 namespace hardware {
33 namespace automotive {
34 namespace sv {
35 namespace V1_0 {
36 namespace implementation {
37
isLogicalCamera(const camera_metadata_t * metadata)38 bool isLogicalCamera(const camera_metadata_t* metadata) {
39 if (metadata == nullptr) {
40 // A logical camera device must have a valid camera metadata.
41 return false;
42 }
43
44 // Looking for LOGICAL_MULTI_CAMERA capability from metadata.
45 camera_metadata_ro_entry_t entry;
46 int rc =
47 find_camera_metadata_ro_entry(metadata,
48 ANDROID_REQUEST_AVAILABLE_CAPABILITIES,
49 &entry);
50 if (0 != rc) {
51 // No capabilities are found.
52 return false;
53 }
54
55 for (size_t i = 0; i < entry.count; ++i) {
56 uint8_t cap = entry.data.u8[i];
57 if (cap ==
58 ANDROID_REQUEST_AVAILABLE_CAPABILITIES_LOGICAL_MULTI_CAMERA) {
59 return true;
60 }
61 }
62
63 return false;
64 }
65
getPhysicalCameraIds(sp<IEvsCamera> camera)66 vector<string> getPhysicalCameraIds(sp<IEvsCamera> camera) {
67 if (camera == nullptr) {
68 LOG(WARNING) << __FUNCTION__ << "The EVS camera object is invalid";
69 return {};
70 }
71
72 CameraDesc desc;
73 camera->getCameraInfo_1_1([&desc](const CameraDesc& info) {
74 desc = info;
75 });
76
77 vector<string> physicalCameras;
78 const camera_metadata_t* metadata =
79 reinterpret_cast<camera_metadata_t*>(&desc.metadata[0]);
80
81 if (!isLogicalCamera(metadata)) {
82 // EVS assumes that the device w/o a valid metadata is a physical
83 // device.
84 LOG(INFO) << desc.v1.cameraId << " is not a logical camera device.";
85 physicalCameras.emplace_back(desc.v1.cameraId);
86 return physicalCameras;
87 }
88
89 // Look for physical camera identifiers
90 camera_metadata_ro_entry entry;
91 int rc =
92 find_camera_metadata_ro_entry(metadata,
93 ANDROID_LOGICAL_MULTI_CAMERA_PHYSICAL_IDS,
94 &entry);
95 if (rc != 0) {
96 LOG(ERROR) << "No physical camera ID is found for "
97 << desc.v1.cameraId;
98 return {};
99 }
100
101 const uint8_t* ids = entry.data.u8;
102 size_t start = 0;
103 for (size_t i = 0; i < entry.count; ++i) {
104 if (ids[i] == '\0') {
105 if (start != i) {
106 string id(reinterpret_cast<const char*>(ids + start));
107 physicalCameras.emplace_back(id);
108 }
109 start = i + 1;
110 }
111 }
112
113 LOG(INFO) << desc.v1.cameraId << " consists of " << physicalCameras.size()
114 << " physical camera devices";
115 return physicalCameras;
116 }
117
tagToString(uint32_t tag)118 string tagToString(uint32_t tag) {
119 switch (tag) {
120 case ANDROID_LENS_DISTORTION:
121 return "ANDROID_LENS_DISTORTION";
122 case ANDROID_LENS_INTRINSIC_CALIBRATION:
123 return "ANDROID_LENS_INTRINSIC_CALIBRATION";
124 case ANDROID_LENS_POSE_TRANSLATION:
125 return "ANDROID_LENS_POSE_TRANSLATION";
126 case ANDROID_LENS_POSE_ROTATION:
127 return "ANDROID_LENS_POSE_ROTATION";
128 default:
129 LOG(WARNING) << "Cannot recognize the tag: " << tag;
130 return {};
131 }
132 }
133
getParam(const camera_metadata_t * metadata,uint32_t tag,int size,float * param)134 bool getParam(const camera_metadata_t* metadata,
135 uint32_t tag,
136 int size,
137 float* param) {
138 camera_metadata_ro_entry_t entry = camera_metadata_ro_entry_t();
139 int rc = find_camera_metadata_ro_entry(metadata, tag, &entry);
140
141 if (rc != 0) {
142 LOG(ERROR) << "No metadata found for " << tagToString(tag);
143 return false;
144 }
145
146 if (entry.count != size || entry.type != TYPE_FLOAT) {
147 LOG(ERROR) << "Unexpected size or type for " << tagToString(tag);
148 return false;
149 }
150
151 const float* lensParam = entry.data.f;
152 for (int i = 0; i < size; i++) {
153 param[i] = lensParam[i];
154 }
155 return true;
156 }
157
getAndroidCameraParams(sp<IEvsCamera> camera,const string & cameraId,AndroidCameraParams & params)158 bool getAndroidCameraParams(sp<IEvsCamera> camera,
159 const string& cameraId,
160 AndroidCameraParams& params) {
161 if (camera == nullptr) {
162 LOG(WARNING) << __FUNCTION__ << "The EVS camera object is invalid";
163 return {};
164 }
165
166 CameraDesc desc = {};
167 camera->getPhysicalCameraInfo(cameraId, [&desc](const CameraDesc& info) {
168 desc = info;
169 });
170
171 if (desc.metadata.size() == 0) {
172 LOG(ERROR) << "No metadata found for " << desc.v1.cameraId;
173 return false;
174 }
175
176 const camera_metadata_t* metadata =
177 reinterpret_cast<camera_metadata_t*>(&desc.metadata[0]);
178
179 // Look for ANDROID_LENS_DISTORTION
180 if (!getParam(metadata,
181 ANDROID_LENS_DISTORTION,
182 kSizeLensDistortion,
183 ¶ms.lensDistortion[0])) {
184 return false;
185 }
186
187 // Look for ANDROID_LENS_INTRINSIC_CALIBRATION
188 if (!getParam(metadata,
189 ANDROID_LENS_INTRINSIC_CALIBRATION,
190 kSizeLensIntrinsicCalibration,
191 ¶ms.lensIntrinsicCalibration[0])) {
192 return false;
193 }
194
195 // Look for ANDROID_LENS_POSE_TRANSLATION
196 if (!getParam(metadata,
197 ANDROID_LENS_POSE_TRANSLATION,
198 kSizeLensPoseTranslation,
199 ¶ms.lensPoseTranslation[0])) {
200 return false;
201 }
202
203 // Look for ANDROID_LENS_POSE_ROTATION
204 if (!getParam(metadata,
205 ANDROID_LENS_POSE_ROTATION,
206 kSizeLensPoseRotation,
207 ¶ms.lensPoseRotation[0])) {
208 return false;
209 }
210
211 return true;
212 }
213
convertToSurroundViewCameraParams(const map<string,AndroidCameraParams> & androidCameraParamsMap)214 vector<SurroundViewCameraParams> convertToSurroundViewCameraParams(
215 const map<string, AndroidCameraParams>& androidCameraParamsMap) {
216 vector<SurroundViewCameraParams> result;
217
218 // TODO(b/156101189): the cameras are in random order now. They need to be
219 // sorted based on the camera position info from config file.
220 for (const auto& entry : androidCameraParamsMap) {
221 SurroundViewCameraParams svParams;
222
223 // Android Camera format for intrinsics: [f_x, f_y, c_x, c_y, s]
224 //
225 // To corelib:
226 // SurroundViewCameraParams.intrinsics =
227 // [ f_x, s, c_x,
228 // 0, f_y, c_y,
229 // 0, 0, 1 ];
230 const float* intrinsics = &entry.second.lensIntrinsicCalibration[0];
231 svParams.intrinsics[0] = intrinsics[0];
232 svParams.intrinsics[1] = intrinsics[4];
233 svParams.intrinsics[2] = intrinsics[2];
234 svParams.intrinsics[3] = 0;
235 svParams.intrinsics[4] = intrinsics[1];
236 svParams.intrinsics[5] = intrinsics[3];
237 svParams.intrinsics[6] = 0;
238 svParams.intrinsics[7] = 0;
239 svParams.intrinsics[8] = 1;
240
241 // Android Camera format for lens distortion:
242 // Radial: [kappa_1, kappa_2, kappa_3]
243 // Tangential: [kappa_4, kappa_5]
244 //
245 // To corelib:
246 // SurroundViewCameraParams.distortion =
247 // [kappa_1, kappa_2, kappa_3, kappa_4];
248 const float* distortion = &entry.second.lensDistortion[0];
249 svParams.distorion[0] = distortion[0];
250 svParams.distorion[1] = distortion[1];
251 svParams.distorion[2] = distortion[2];
252 svParams.distorion[3] = distortion[3];
253
254 // Android Camera format for rotation:
255 // quaternion coefficients (x,y,z,w)
256 //
257 // To corelib:
258 // theta = 2 * acos(w)
259 // a_x = x / sin(theta/2)
260 // a_y = y / sin(theta/2)
261 // a_z = z / sin(theta/2)
262 // SurroundViewCameraParams.rvec =
263 // [theta * a_x, theta * a_y, theta * a_z];
264 const float* rotation = &entry.second.lensPoseRotation[0];
265 const float theta = 2 * acos(rotation[3]);
266 const float a_x = rotation[0] / sin(theta / 2);
267 const float a_y = rotation[1] / sin(theta / 2);
268 const float a_z = rotation[2] / sin(theta / 2);
269 svParams.rvec[0] = theta * a_x;
270 svParams.rvec[1] = theta * a_y;
271 svParams.rvec[2] = theta * a_z;
272
273 // Android Camera format for translation: Translation = (x,y,z)
274 //
275 // To corelib:
276 // SurroundViewCameraParams.tvec = [x, y, z];
277 const float* translation = &entry.second.lensPoseTranslation[0];
278 svParams.tvec[0] = translation[0];
279 svParams.tvec[1] = translation[1];
280 svParams.tvec[2] = translation[2];
281
282 LOG(INFO) << "Camera parameters for " << entry.first
283 << " have been converted to SV core lib format successfully";
284 result.emplace_back(svParams);
285 }
286
287 return result;
288 }
289
290 } // namespace implementation
291 } // namespace V1_0
292 } // namespace sv
293 } // namespace automotive
294 } // namespace hardware
295 } // namespace android
296
297