| /* |
| * calibration_parser.cpp - parse fisheye calibration file |
| * |
| * Copyright (c) 2016-2017 Intel Corporation |
| * |
| * Licensed under the Apache License, Version 2.0 (the "License"); |
| * you may not use this file except in compliance with the License. |
| * You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| * |
| * Author: Junkai Wu <[email protected]> |
| */ |
| |
| #include "calibration_parser.h" |
| #include "file_handle.h" |
| |
| namespace XCam { |
| |
| CalibrationParser::CalibrationParser() |
| { |
| } |
| |
| #define CHECK_NULL(ptr) \ |
| if(ptr == NULL) { \ |
| XCAM_LOG_ERROR("Parse file failed"); \ |
| return XCAM_RETURN_ERROR_FILE; \ |
| } |
| |
| XCamReturn |
| CalibrationParser::parse_intrinsic_param(char *file_body, IntrinsicParameter &intrinsic_param) |
| { |
| char *line_str = NULL; |
| char *line_endptr = NULL; |
| char *tok_str = NULL; |
| char *tok_endptr = NULL; |
| static const char *line_tokens = "\r\n"; |
| static const char *str_tokens = " \t"; |
| |
| do { |
| line_str = strtok_r(file_body, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| while(tok_str == NULL || tok_str[0] == '#') { |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| } |
| |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| while(tok_str == NULL || tok_str[0] == '#') { |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| } |
| intrinsic_param.poly_length = strtol(tok_str, NULL, 10); |
| |
| XCAM_FAIL_RETURN ( |
| ERROR, intrinsic_param.poly_length <= XCAM_INTRINSIC_MAX_POLY_SIZE, |
| XCAM_RETURN_ERROR_PARAM, |
| "intrinsic poly length:%d is larger than max_size:%d.", |
| intrinsic_param.poly_length, XCAM_INTRINSIC_MAX_POLY_SIZE); |
| |
| for(uint32_t i = 0; i < intrinsic_param.poly_length; i++) { |
| tok_str = strtok_r(NULL, str_tokens, &tok_endptr); |
| CHECK_NULL(tok_str); |
| intrinsic_param.poly_coeff[i] = (strtof(tok_str, NULL)); |
| } |
| |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| while(tok_str == NULL || tok_str[0] == '#') { |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| } |
| intrinsic_param.yc = strtof(tok_str, NULL); |
| |
| tok_str = strtok_r(NULL, str_tokens, &tok_endptr); |
| CHECK_NULL(tok_str); |
| intrinsic_param.xc = strtof(tok_str, NULL); |
| |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| while(tok_str == NULL || tok_str[0] == '#') { |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| } |
| intrinsic_param.c = strtof(tok_str, NULL); |
| |
| tok_str = strtok_r(NULL, str_tokens, &tok_endptr); |
| CHECK_NULL(tok_str); |
| intrinsic_param.d = strtof(tok_str, NULL); |
| |
| tok_str = strtok_r(NULL, str_tokens, &tok_endptr); |
| CHECK_NULL(tok_str); |
| intrinsic_param.e = strtof(tok_str, NULL); |
| } while(0); |
| |
| return XCAM_RETURN_NO_ERROR; |
| } |
| |
| XCamReturn |
| CalibrationParser::parse_extrinsic_param(char *file_body, ExtrinsicParameter &extrinsic_param) |
| { |
| char *line_str = NULL; |
| char *line_endptr = NULL; |
| char *tok_str = NULL; |
| char *tok_endptr = NULL; |
| static const char *line_tokens = "\r\n"; |
| static const char *str_tokens = " \t"; |
| |
| do { |
| line_str = strtok_r(file_body, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| while(tok_str == NULL || tok_str[0] == '#') { |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| } |
| extrinsic_param.trans_x = strtof(tok_str, NULL); |
| |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| while(tok_str == NULL || tok_str[0] == '#') { |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| } |
| extrinsic_param.trans_y = strtof(tok_str, NULL); |
| |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| while(tok_str == NULL || tok_str[0] == '#') { |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| } |
| extrinsic_param.trans_z = strtof(tok_str, NULL); |
| |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| while(tok_str == NULL || tok_str[0] == '#') { |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| } |
| extrinsic_param.roll = strtof(tok_str, NULL); |
| |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| while(tok_str == NULL || tok_str[0] == '#') { |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| } |
| extrinsic_param.pitch = strtof(tok_str, NULL); |
| |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| while(tok_str == NULL || tok_str[0] == '#') { |
| line_str = strtok_r(NULL, line_tokens, &line_endptr); |
| CHECK_NULL(line_str); |
| tok_str = strtok_r(line_str, str_tokens, &tok_endptr); |
| } |
| extrinsic_param.yaw = strtof(tok_str, NULL); |
| } while(0); |
| |
| return XCAM_RETURN_NO_ERROR; |
| } |
| |
| XCamReturn |
| CalibrationParser::parse_intrinsic_file(const char *file_path, IntrinsicParameter &intrinsic_param) |
| { |
| XCAM_ASSERT (file_path); |
| |
| FileHandle file_reader; |
| XCamReturn ret = XCAM_RETURN_NO_ERROR; |
| std::vector<char> context; |
| size_t file_size = 0; |
| |
| XCAM_FAIL_RETURN ( |
| WARNING, xcam_ret_is_ok (ret = file_reader.open (file_path, "r")), ret, |
| "open intrinsic file(%s) failed.", file_path); |
| XCAM_FAIL_RETURN ( |
| WARNING, xcam_ret_is_ok (ret = file_reader.get_file_size (file_size)), ret, |
| "read intrinsic file(%s) failed to get file size.", file_path); |
| context.resize (file_size + 1); |
| XCAM_FAIL_RETURN ( |
| WARNING, xcam_ret_is_ok (ret = file_reader.read_file (&context[0], file_size)), ret, |
| "read intrinsic file(%s) failed, file size:%d.", file_path, (int)file_size); |
| file_reader.close (); |
| context[file_size] = '\0'; |
| |
| return parse_intrinsic_param (&context[0], intrinsic_param); |
| } |
| |
| XCamReturn |
| CalibrationParser::parse_extrinsic_file(const char *file_path, ExtrinsicParameter &extrinsic_param) |
| { |
| XCAM_ASSERT (file_path); |
| |
| FileHandle file_reader; |
| XCamReturn ret = XCAM_RETURN_NO_ERROR; |
| std::vector<char> context; |
| size_t file_size = 0; |
| |
| XCAM_FAIL_RETURN ( |
| WARNING, xcam_ret_is_ok (ret = file_reader.open (file_path, "r")), ret, |
| "open extrinsic file(%s) failed.", file_path); |
| XCAM_FAIL_RETURN ( |
| WARNING, xcam_ret_is_ok (ret = file_reader.get_file_size (file_size)), ret, |
| "read extrinsic file(%s) failed to get file size.", file_path); |
| context.resize (file_size + 1); |
| XCAM_FAIL_RETURN ( |
| WARNING, xcam_ret_is_ok (ret = file_reader.read_file (&context[0], file_size)), ret, |
| "read extrinsic file(%s) failed, file size:%d.", file_path, (int)file_size); |
| file_reader.close (); |
| context[file_size] = '\0'; |
| |
| return parse_extrinsic_param (&context[0], extrinsic_param); |
| } |
| |
| } |
