| 407 | } |
| 408 | |
| 409 | bool OPTCalibrationNode::save() |
| 410 | { |
| 411 | // Save tfs between sensors and world coordinate system (last checherboard) to file |
| 412 | std::string file_name = ros::package::getPath("opt_calibration") + "/conf/camera_poses.yaml"; |
| 413 | std::ofstream file; |
| 414 | file.open(file_name.c_str()); |
| 415 | |
| 416 | if (file.is_open()) |
| 417 | { |
| 418 | ros::Time time = ros::Time::now(); |
| 419 | file << "# Auto generated file." << std::endl; |
| 420 | file << "calibration_id: " << time.sec << std::endl << std::endl; |
| 421 | |
| 422 | Pose new_world_pose = Pose::Identity(); |
| 423 | |
| 424 | if (world_computation_ == LAST_CHECKERBOARD) |
| 425 | { |
| 426 | new_world_pose = calibration_->getLastCheckerboardPose().inverse(); |
| 427 | for (size_t i = 0; i < sensor_vec_.size(); ++i) |
| 428 | { |
| 429 | const Sensor::Ptr & sensor = sensor_vec_[i]; |
| 430 | Pose pose = new_world_pose * sensor->pose(); |
| 431 | if(pose.translation().z() < 0) |
| 432 | { |
| 433 | ROS_INFO_STREAM("[" << sensor->frameId() << "] z < 0. Flipping /world orientation."); |
| 434 | AngleAxis rotation(M_PI, Vector3(1.0, 1.0, 0.0).normalized()); |
| 435 | new_world_pose = rotation * calibration_->getLastCheckerboardPose().inverse(); |
| 436 | break; |
| 437 | } |
| 438 | } |
| 439 | } |
| 440 | else if (world_computation_ == UPDATE) |
| 441 | { |
| 442 | new_world_pose = fixed_sensor_pose_ * fixed_sensor_->pose().inverse(); |
| 443 | } |
| 444 | |
| 445 | // Write TF transforms between cameras and world frame |
| 446 | file << "# Poses w.r.t. the \"world\" reference frame" << std::endl; |
| 447 | file << "poses:" << std::endl; |
| 448 | for (size_t i = 0; i < sensor_vec_.size(); ++i) |
| 449 | { |
| 450 | const Sensor::Ptr & sensor = sensor_vec_[i]; |
| 451 | |
| 452 | Pose pose = new_world_pose * sensor->pose(); |
| 453 | |
| 454 | file << " " << sensor->frameId().substr(1) << ":" << std::endl; |
| 455 | |
| 456 | file << " translation:" << std::endl |
| 457 | << " x: " << pose.translation().x() << std::endl |
| 458 | << " y: " << pose.translation().y() << std::endl |
| 459 | << " z: " << pose.translation().z() << std::endl; |
| 460 | |
| 461 | Quaternion rotation(pose.rotation()); |
| 462 | file << " rotation:" << std::endl |
| 463 | << " x: " << rotation.x() << std::endl |
| 464 | << " y: " << rotation.y() << std::endl |
| 465 | << " z: " << rotation.z() << std::endl |
| 466 | << " w: " << rotation.w() << std::endl; |