| 257 | } |
| 258 | |
| 259 | int main(int argc, char** argv) { |
| 260 | ros::init(argc, argv, "cloud_banchmark"); |
| 261 | ros::NodeHandle nh("~"); |
| 262 | |
| 263 | nh.getParam("cam_width", width); |
| 264 | nh.getParam("cam_height", height); |
| 265 | nh.getParam("cam_fx", fx); |
| 266 | nh.getParam("cam_fy", fy); |
| 267 | nh.getParam("cam_cx", cx); |
| 268 | nh.getParam("cam_cy", cy); |
| 269 | |
| 270 | depthrender.set_para(fx, fy, cx, cy, width, height); |
| 271 | |
| 272 | cv_K = (cv::Mat_<float>(3, 3) << fx, 0.0f, cx, 0.0f, fy, cy, 0.0f, 0.0f, 1.0f); |
| 273 | if (nh.hasParam("cam_k1") && nh.hasParam("cam_k2") && nh.hasParam("cam_r1") && nh.hasParam("cam_r2")) { |
| 274 | float k1, k2, r1, r2; |
| 275 | nh.getParam("cam_k1", k1); |
| 276 | nh.getParam("cam_k2", k2); |
| 277 | nh.getParam("cam_r1", r1); |
| 278 | nh.getParam("cam_r2", r2); |
| 279 | cv_D = (cv::Mat_<float>(1, 4) << k1, k2, r1, r2); |
| 280 | cv::initUndistortRectifyMap(cv_K, cv_D, cv::Mat_<double>::eye(3, 3), cv_K, cv::Size(width, height), |
| 281 | CV_16SC2, undist_map1, undist_map2); |
| 282 | is_distorted = true; |
| 283 | } |
| 284 | if (is_distorted) |
| 285 | printf("need to rectify.\n"); |
| 286 | else |
| 287 | printf("do not need to rectify.\n"); |
| 288 | |
| 289 | vicon2body << 0.33638, -0.01749, 0.94156, 0.06901, -0.02078, -0.99972, -0.01114, -0.02781, 0.94150, |
| 290 | -0.01582, -0.33665, -0.12395, 0.0, 0.0, 0.0, 1.0; |
| 291 | cam02body << 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, 0.999557249008, |
| 292 | 0.0149672133247, 0.025715529948, -0.064676986768, -0.0257744366974, 0.00375618835797, |
| 293 | 0.999660727178, 0.00981073058949, 0.0, 0.0, 0.0, 1.0; |
| 294 | cam2world = Matrix4d::Identity(); |
| 295 | |
| 296 | string cloud_path; |
| 297 | nh.getParam("cloud_path", cloud_path); |
| 298 | printf("cloud file %s\n", cloud_path.c_str()); |
| 299 | std::fstream data_file; |
| 300 | data_file.open(cloud_path.c_str(), ios::in); |
| 301 | vector<float> cloud_data; |
| 302 | double x, y, z, i, r, g, b; |
| 303 | while (data_file >> x >> y >> z >> i >> r >> g >> b) { |
| 304 | cloud_data.push_back(x); |
| 305 | cloud_data.push_back(y); |
| 306 | cloud_data.push_back(z); |
| 307 | } |
| 308 | data_file.close(); |
| 309 | printf("has points %d.\n", cloud_data.size() / 3); |
| 310 | |
| 311 | string groundtruth_path = string("/home/denny/Downloads/wkx_bag/data.txt"); |
| 312 | std::fstream gt_file; |
| 313 | gt_file.open(groundtruth_path.c_str(), ios::in); |
| 314 | read_pose(gt_file); |
| 315 | gt_file.close(); |
| 316 | |