| 133 | } |
| 134 | |
| 135 | bool RosBaseDataProvider::parseCameraData(StereoCalibration* stereo_calib) { |
| 136 | CHECK_NOTNULL(stereo_calib); |
| 137 | // Parse camera calibration info (from param server) |
| 138 | |
| 139 | // Rate |
| 140 | double rate; |
| 141 | nh_private_.getParam("camera_rate_hz", rate); |
| 142 | |
| 143 | // Resoltuion |
| 144 | std::vector<int> resolution; |
| 145 | CHECK(nh_private_.getParam("camera_resolution", resolution)); |
| 146 | CHECK_EQ(resolution.size(), 2); |
| 147 | |
| 148 | // Get distortion/intrinsics/extrinsics for each camera |
| 149 | for (int i = 0; i < 2; i++) { |
| 150 | std::string camera_name; |
| 151 | CameraParams camera_param_i; |
| 152 | // Fill in rate and resolution |
| 153 | camera_param_i.image_size_ = cv::Size(resolution[0], resolution[1]); |
| 154 | // Terminology wrong but following rest of the repo |
| 155 | camera_param_i.frame_rate_ = 1.0 / rate; |
| 156 | |
| 157 | if (i == 0) { |
| 158 | camera_name = "left_camera_"; |
| 159 | } else { |
| 160 | camera_name = "right_camera_"; |
| 161 | } |
| 162 | // Parse intrinsics (camera matrix) |
| 163 | std::vector<double> intrinsics; |
| 164 | nh_private_.getParam(camera_name + "intrinsics", intrinsics); |
| 165 | CHECK_EQ(intrinsics.size(), 4u); |
| 166 | camera_param_i.intrinsics_ = intrinsics; |
| 167 | // Conver intrinsics to camera matrix (OpenCV format) |
| 168 | camera_param_i.camera_matrix_ = cv::Mat::eye(3, 3, CV_64F); |
| 169 | camera_param_i.camera_matrix_.at<double>(0, 0) = intrinsics[0]; |
| 170 | camera_param_i.camera_matrix_.at<double>(1, 1) = intrinsics[1]; |
| 171 | camera_param_i.camera_matrix_.at<double>(0, 2) = intrinsics[2]; |
| 172 | camera_param_i.camera_matrix_.at<double>(1, 2) = intrinsics[3]; |
| 173 | |
| 174 | // Parse extrinsics (rotation and translation) |
| 175 | std::vector<double> extrinsics; |
| 176 | // Encode calibration frame to body frame |
| 177 | std::vector<double> frame_change; |
| 178 | CHECK(nh_private_.getParam(camera_name + "extrinsics", extrinsics)); |
| 179 | CHECK(nh_private_.getParam("calibration_to_body_frame", frame_change)); |
| 180 | CHECK_EQ(extrinsics.size(), 16u); |
| 181 | CHECK_EQ(frame_change.size(), 16u); |
| 182 | // Place into matrix |
| 183 | // 4 4 is hardcoded here because currently only accept extrinsic input |
| 184 | // in homoegeneous format [R T ; 0 1] |
| 185 | cv::Mat E_calib = cv::Mat::zeros(4, 4, CV_64F); |
| 186 | cv::Mat calib2body = cv::Mat::zeros(4, 4, CV_64F); |
| 187 | for (int k = 0; k < 16; k++) { |
| 188 | int row = k / 4; // Integer division, truncation of fractional part. |
| 189 | int col = k % 4; |
| 190 | E_calib.at<double>(row, col) = extrinsics[k]; |
| 191 | calib2body.at<double>(row, col) = frame_change[k]; |
| 192 | } |