| 698 | }; |
| 699 | |
| 700 | void OPTCalibration::optimize() |
| 701 | { |
| 702 | convertToWorldFrame(); |
| 703 | |
| 704 | ROS_INFO("Optimizing..."); |
| 705 | |
| 706 | Eigen::Matrix<cb::Scalar, Eigen::Dynamic, 6, Eigen::DontAlign | Eigen::RowMajor> cb_data(view_map_vec_.size(), 6); |
| 707 | Eigen::Matrix<cb::Scalar, Eigen::Dynamic, 6, Eigen::DontAlign | Eigen::RowMajor> sensor_data(node_vec_.size(), 6); |
| 708 | Eigen::Matrix<cb::Scalar, 3, 1, Eigen::DontAlign> floor_data; |
| 709 | |
| 710 | for (size_t i = 0; i < node_vec_.size(); ++i) |
| 711 | { |
| 712 | const TreeNode & sensor_node = *node_vec_[i]; |
| 713 | cb::AngleAxis rotation = cb::AngleAxis(sensor_node.sensor()->pose().linear()); |
| 714 | sensor_data.row(i).head<3>() = rotation.angle() * rotation.axis(); |
| 715 | sensor_data.row(i).tail<3>() = sensor_node.sensor()->pose().translation(); |
| 716 | } |
| 717 | |
| 718 | for (size_t i = 0; i < checkerboard_vec_.size(); ++i) |
| 719 | { |
| 720 | const cb::Checkerboard & checkerboard = *checkerboard_vec_[i]; |
| 721 | cb::AngleAxis rotation = cb::AngleAxis(checkerboard.pose().linear()); |
| 722 | cb_data.row(i).head<3>() = rotation.angle() * rotation.axis(); |
| 723 | cb_data.row(i).tail<3>() = checkerboard.pose().translation(); |
| 724 | |
| 725 | } |
| 726 | |
| 727 | // ROS_INFO("Before optimization:"); |
| 728 | // for (size_t i = 0; i < sensor_vec_.size(); ++i) |
| 729 | // ROS_INFO_STREAM("(" << node_vec_[i]->sensor()->parent()->frameId() << ") " |
| 730 | // << node_vec_[i]->sensor()->frameId() << ": " << sensor_data.row(i)); |
| 731 | |
| 732 | if (not floor_estimated_) |
| 733 | { |
| 734 | floor_estimated_ = estimateFloor(); |
| 735 | // if (floor_estimated_) |
| 736 | // ROS_INFO_STREAM("F " << floor_.normal().transpose() << " " << floor_.offset()); |
| 737 | } |
| 738 | |
| 739 | cb::Vector3 floor_origin; |
| 740 | cb::Vector3 floor_x; |
| 741 | cb::Transform floor_pose; |
| 742 | if (floor_estimated_) |
| 743 | { |
| 744 | floor_origin = -floor_.normal() * floor_.offset(); |
| 745 | floor_x = (floor_.projection(floor_origin + cb::Vector3::UnitX()) - floor_origin).normalized(); |
| 746 | |
| 747 | floor_pose.linear().col(2) = floor_.normal(); |
| 748 | floor_pose.linear().col(0) = floor_x; |
| 749 | floor_pose.linear().col(1) = floor_pose.linear().col(2).cross(floor_pose.linear().col(0)); |
| 750 | floor_pose.translation() = floor_origin; |
| 751 | |
| 752 | floor_data = floor_origin; |
| 753 | } |
| 754 | |
| 755 | for (size_t i = 0; i < checkerboard_vec_.size(); ++i) |
| 756 | { |
| 757 | if (floor_estimated_ and is_floor_vec_[i]) |