/* * 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 */ #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 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 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); } }