Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 1 | #include "frc971/vision/target_mapper.h" |
| 2 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 3 | #include "absl/strings/str_format.h" |
Philipp Schrader | 790cb54 | 2023-07-05 21:06:52 -0700 | [diff] [blame] | 4 | |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 5 | #include "frc971/control_loops/control_loop.h" |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 6 | #include "frc971/vision/ceres/pose_graph_3d_error_term.h" |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 7 | #include "frc971/vision/geometry.h" |
| 8 | |
| 9 | DEFINE_uint64(max_num_iterations, 100, |
| 10 | "Maximum number of iterations for the ceres solver"); |
milind-u | d62f80a | 2023-03-04 16:37:09 -0800 | [diff] [blame] | 11 | DEFINE_double(distortion_noise_scalar, 1.0, |
| 12 | "Scale the target pose distortion factor by this when computing " |
| 13 | "the noise."); |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 14 | DEFINE_int32( |
milind-u | 6ff399f | 2023-03-24 18:33:38 -0700 | [diff] [blame] | 15 | frozen_target_id, 1, |
| 16 | "Freeze the pose of this target so the map can have one fixed point."); |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 17 | DEFINE_int32(min_target_id, 1, "Minimum target id to solve for"); |
| 18 | DEFINE_int32(max_target_id, 8, "Maximum target id to solve for"); |
| 19 | DEFINE_bool(visualize_solver, false, "If true, visualize the solving process."); |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 20 | // This does seem like a strict threshold for a constaint to be considered an |
| 21 | // outlier, but most inliers were very close together and this is what worked |
| 22 | // best for map solving. |
| 23 | DEFINE_double(outlier_std_devs, 1.0, |
| 24 | "Number of standard deviations above average error needed for a " |
| 25 | "constraint to be considered an outlier and get removed."); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 26 | |
| 27 | namespace frc971::vision { |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 28 | Eigen::Affine3d PoseUtils::Pose3dToAffine3d( |
| 29 | const ceres::examples::Pose3d &pose3d) { |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 30 | Eigen::Affine3d H_world_pose = |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 31 | Eigen::Translation3d(pose3d.p(0), pose3d.p(1), pose3d.p(2)) * pose3d.q; |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 32 | return H_world_pose; |
| 33 | } |
| 34 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 35 | ceres::examples::Pose3d PoseUtils::Affine3dToPose3d(const Eigen::Affine3d &H) { |
| 36 | return ceres::examples::Pose3d{.p = H.translation(), |
| 37 | .q = Eigen::Quaterniond(H.rotation())}; |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 38 | } |
| 39 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 40 | ceres::examples::Pose3d PoseUtils::ComputeRelativePose( |
| 41 | const ceres::examples::Pose3d &pose_1, |
| 42 | const ceres::examples::Pose3d &pose_2) { |
| 43 | Eigen::Affine3d H_world_1 = Pose3dToAffine3d(pose_1); |
| 44 | Eigen::Affine3d H_world_2 = Pose3dToAffine3d(pose_2); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 45 | |
| 46 | // Get the location of 2 in the 1 frame |
| 47 | Eigen::Affine3d H_1_2 = H_world_1.inverse() * H_world_2; |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 48 | return Affine3dToPose3d(H_1_2); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 49 | } |
| 50 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 51 | ceres::examples::Pose3d PoseUtils::ComputeOffsetPose( |
| 52 | const ceres::examples::Pose3d &pose_1, |
| 53 | const ceres::examples::Pose3d &pose_2_relative) { |
| 54 | auto H_world_1 = Pose3dToAffine3d(pose_1); |
| 55 | auto H_1_2 = Pose3dToAffine3d(pose_2_relative); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 56 | auto H_world_2 = H_world_1 * H_1_2; |
| 57 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 58 | return Affine3dToPose3d(H_world_2); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 59 | } |
| 60 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 61 | Eigen::Quaterniond PoseUtils::EulerAnglesToQuaternion( |
| 62 | const Eigen::Vector3d &rpy) { |
| 63 | Eigen::AngleAxisd roll_angle(rpy.x(), Eigen::Vector3d::UnitX()); |
| 64 | Eigen::AngleAxisd pitch_angle(rpy.y(), Eigen::Vector3d::UnitY()); |
| 65 | Eigen::AngleAxisd yaw_angle(rpy.z(), Eigen::Vector3d::UnitZ()); |
| 66 | |
| 67 | return yaw_angle * pitch_angle * roll_angle; |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 68 | } |
| 69 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 70 | Eigen::Vector3d PoseUtils::QuaternionToEulerAngles( |
| 71 | const Eigen::Quaterniond &q) { |
| 72 | return RotationMatrixToEulerAngles(q.toRotationMatrix()); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 73 | } |
| 74 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 75 | Eigen::Vector3d PoseUtils::RotationMatrixToEulerAngles( |
| 76 | const Eigen::Matrix3d &R) { |
| 77 | double roll = aos::math::NormalizeAngle(std::atan2(R(2, 1), R(2, 2))); |
| 78 | double pitch = aos::math::NormalizeAngle(-std::asin(R(2, 0))); |
| 79 | double yaw = aos::math::NormalizeAngle(std::atan2(R(1, 0), R(0, 0))); |
| 80 | |
| 81 | return Eigen::Vector3d(roll, pitch, yaw); |
| 82 | } |
| 83 | |
milind-u | 3f5f83c | 2023-01-29 15:23:51 -0800 | [diff] [blame] | 84 | flatbuffers::Offset<TargetPoseFbs> PoseUtils::TargetPoseToFbs( |
| 85 | const TargetMapper::TargetPose &target_pose, |
| 86 | flatbuffers::FlatBufferBuilder *fbb) { |
| 87 | const auto position_offset = |
| 88 | CreatePosition(*fbb, target_pose.pose.p(0), target_pose.pose.p(1), |
| 89 | target_pose.pose.p(2)); |
| 90 | const auto orientation_offset = |
| 91 | CreateQuaternion(*fbb, target_pose.pose.q.w(), target_pose.pose.q.x(), |
| 92 | target_pose.pose.q.y(), target_pose.pose.q.z()); |
| 93 | return CreateTargetPoseFbs(*fbb, target_pose.id, position_offset, |
| 94 | orientation_offset); |
| 95 | } |
| 96 | |
| 97 | TargetMapper::TargetPose PoseUtils::TargetPoseFromFbs( |
| 98 | const TargetPoseFbs &target_pose_fbs) { |
| 99 | return {.id = static_cast<TargetMapper::TargetId>(target_pose_fbs.id()), |
| 100 | .pose = ceres::examples::Pose3d{ |
| 101 | Eigen::Vector3d(target_pose_fbs.position()->x(), |
| 102 | target_pose_fbs.position()->y(), |
| 103 | target_pose_fbs.position()->z()), |
| 104 | Eigen::Quaterniond(target_pose_fbs.orientation()->w(), |
| 105 | target_pose_fbs.orientation()->x(), |
| 106 | target_pose_fbs.orientation()->y(), |
| 107 | target_pose_fbs.orientation()->z())}}; |
| 108 | } |
| 109 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 110 | ceres::examples::VectorOfConstraints DataAdapter::MatchTargetDetections( |
Milind Upadhyay | ec49391 | 2022-12-18 21:33:15 -0800 | [diff] [blame] | 111 | const std::vector<DataAdapter::TimestampedDetection> |
| 112 | ×tamped_target_detections, |
| 113 | aos::distributed_clock::duration max_dt) { |
| 114 | CHECK_GE(timestamped_target_detections.size(), 2ul) |
| 115 | << "Must have at least 2 detections"; |
| 116 | |
| 117 | // Match consecutive detections |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 118 | ceres::examples::VectorOfConstraints target_constraints; |
milind-u | d62f80a | 2023-03-04 16:37:09 -0800 | [diff] [blame] | 119 | for (auto detection = timestamped_target_detections.begin() + 1; |
| 120 | detection < timestamped_target_detections.end(); detection++) { |
| 121 | auto last_detection = detection - 1; |
Milind Upadhyay | ec49391 | 2022-12-18 21:33:15 -0800 | [diff] [blame] | 122 | |
| 123 | // Skip two consecutive detections of the same target, because the solver |
| 124 | // doesn't allow this |
milind-u | d62f80a | 2023-03-04 16:37:09 -0800 | [diff] [blame] | 125 | if (detection->id == last_detection->id) { |
Milind Upadhyay | ec49391 | 2022-12-18 21:33:15 -0800 | [diff] [blame] | 126 | continue; |
| 127 | } |
| 128 | |
| 129 | // Don't take into account constraints too far apart in time, because the |
| 130 | // recording device could have moved too much |
milind-u | d62f80a | 2023-03-04 16:37:09 -0800 | [diff] [blame] | 131 | if ((detection->time - last_detection->time) > max_dt) { |
Milind Upadhyay | ec49391 | 2022-12-18 21:33:15 -0800 | [diff] [blame] | 132 | continue; |
| 133 | } |
| 134 | |
milind-u | d62f80a | 2023-03-04 16:37:09 -0800 | [diff] [blame] | 135 | auto confidence = ComputeConfidence(*last_detection, *detection); |
Milind Upadhyay | ec49391 | 2022-12-18 21:33:15 -0800 | [diff] [blame] | 136 | target_constraints.emplace_back( |
milind-u | d62f80a | 2023-03-04 16:37:09 -0800 | [diff] [blame] | 137 | ComputeTargetConstraint(*last_detection, *detection, confidence)); |
Milind Upadhyay | ec49391 | 2022-12-18 21:33:15 -0800 | [diff] [blame] | 138 | } |
| 139 | |
| 140 | return target_constraints; |
| 141 | } |
| 142 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 143 | TargetMapper::ConfidenceMatrix DataAdapter::ComputeConfidence( |
milind-u | d62f80a | 2023-03-04 16:37:09 -0800 | [diff] [blame] | 144 | const TimestampedDetection &detection_start, |
| 145 | const TimestampedDetection &detection_end) { |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 146 | constexpr size_t kX = 0; |
| 147 | constexpr size_t kY = 1; |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 148 | constexpr size_t kZ = 2; |
| 149 | constexpr size_t kOrientation1 = 3; |
| 150 | constexpr size_t kOrientation2 = 4; |
| 151 | constexpr size_t kOrientation3 = 5; |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 152 | |
| 153 | // Uncertainty matrix between start and end |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 154 | TargetMapper::ConfidenceMatrix P = TargetMapper::ConfidenceMatrix::Zero(); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 155 | |
| 156 | { |
| 157 | // Noise for odometry-based robot position measurements |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 158 | TargetMapper::ConfidenceMatrix Q_odometry = |
| 159 | TargetMapper::ConfidenceMatrix::Zero(); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 160 | Q_odometry(kX, kX) = std::pow(0.045, 2); |
| 161 | Q_odometry(kY, kY) = std::pow(0.045, 2); |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 162 | Q_odometry(kZ, kZ) = std::pow(0.045, 2); |
| 163 | Q_odometry(kOrientation1, kOrientation1) = std::pow(0.01, 2); |
| 164 | Q_odometry(kOrientation2, kOrientation2) = std::pow(0.01, 2); |
| 165 | Q_odometry(kOrientation3, kOrientation3) = std::pow(0.01, 2); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 166 | |
| 167 | // Add uncertainty for robot position measurements from start to end |
milind-u | d62f80a | 2023-03-04 16:37:09 -0800 | [diff] [blame] | 168 | int iterations = (detection_end.time - detection_start.time) / |
| 169 | frc971::controls::kLoopFrequency; |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 170 | P += static_cast<double>(iterations) * Q_odometry; |
| 171 | } |
| 172 | |
| 173 | { |
Milind Upadhyay | ebf93ee | 2023-01-05 14:12:58 -0800 | [diff] [blame] | 174 | // Noise for vision-based target localizations. Multiplying this matrix by |
milind-u | 6ff399f | 2023-03-24 18:33:38 -0700 | [diff] [blame] | 175 | // the distance from camera to target squared results in the uncertainty |
| 176 | // in that measurement |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 177 | TargetMapper::ConfidenceMatrix Q_vision = |
| 178 | TargetMapper::ConfidenceMatrix::Zero(); |
Milind Upadhyay | ebf93ee | 2023-01-05 14:12:58 -0800 | [diff] [blame] | 179 | Q_vision(kX, kX) = std::pow(0.045, 2); |
| 180 | Q_vision(kY, kY) = std::pow(0.045, 2); |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 181 | Q_vision(kZ, kZ) = std::pow(0.045, 2); |
| 182 | Q_vision(kOrientation1, kOrientation1) = std::pow(0.02, 2); |
| 183 | Q_vision(kOrientation2, kOrientation2) = std::pow(0.02, 2); |
| 184 | Q_vision(kOrientation3, kOrientation3) = std::pow(0.02, 2); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 185 | |
| 186 | // Add uncertainty for the 2 vision measurements (1 at start and 1 at end) |
milind-u | fbc5c81 | 2023-04-06 21:24:29 -0700 | [diff] [blame] | 187 | P += Q_vision * std::pow(detection_start.distance_from_camera * |
| 188 | (1.0 + FLAGS_distortion_noise_scalar * |
| 189 | detection_start.distortion_factor), |
| 190 | 2); |
| 191 | P += Q_vision * std::pow(detection_end.distance_from_camera * |
| 192 | (1.0 + FLAGS_distortion_noise_scalar * |
| 193 | detection_end.distortion_factor), |
| 194 | 2); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 195 | } |
| 196 | |
| 197 | return P.inverse(); |
| 198 | } |
| 199 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 200 | ceres::examples::Constraint3d DataAdapter::ComputeTargetConstraint( |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 201 | const TimestampedDetection &target_detection_start, |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 202 | const TimestampedDetection &target_detection_end, |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 203 | const TargetMapper::ConfidenceMatrix &confidence) { |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 204 | // Compute the relative pose (constraint) between the two targets |
| 205 | Eigen::Affine3d H_targetstart_targetend = |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 206 | target_detection_start.H_robot_target.inverse() * |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 207 | target_detection_end.H_robot_target; |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 208 | ceres::examples::Pose3d target_constraint = |
| 209 | PoseUtils::Affine3dToPose3d(H_targetstart_targetend); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 210 | |
milind-u | f3ab8ba | 2023-02-04 17:56:16 -0800 | [diff] [blame] | 211 | const auto constraint_3d = |
| 212 | ceres::examples::Constraint3d{target_detection_start.id, |
| 213 | target_detection_end.id, |
| 214 | {target_constraint.p, target_constraint.q}, |
| 215 | confidence}; |
| 216 | |
| 217 | VLOG(2) << "Computed constraint: " << constraint_3d; |
| 218 | return constraint_3d; |
Milind Upadhyay | ec49391 | 2022-12-18 21:33:15 -0800 | [diff] [blame] | 219 | } |
| 220 | |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 221 | TargetMapper::TargetMapper( |
Milind Upadhyay | cd677a3 | 2022-12-04 13:06:43 -0800 | [diff] [blame] | 222 | std::string_view target_poses_path, |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 223 | const ceres::examples::VectorOfConstraints &target_constraints) |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 224 | : target_constraints_(target_constraints), |
| 225 | T_frozen_actual_(Eigen::Vector3d::Zero()), |
| 226 | R_frozen_actual_(Eigen::Quaterniond::Identity()), |
| 227 | vis_robot_(cv::Size(1280, 1000)) { |
Milind Upadhyay | cd677a3 | 2022-12-04 13:06:43 -0800 | [diff] [blame] | 228 | aos::FlatbufferDetachedBuffer<TargetMap> target_map = |
| 229 | aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path); |
| 230 | for (const auto *target_pose_fbs : *target_map.message().target_poses()) { |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 231 | ideal_target_poses_[target_pose_fbs->id()] = |
milind-u | 3f5f83c | 2023-01-29 15:23:51 -0800 | [diff] [blame] | 232 | PoseUtils::TargetPoseFromFbs(*target_pose_fbs).pose; |
Milind Upadhyay | cd677a3 | 2022-12-04 13:06:43 -0800 | [diff] [blame] | 233 | } |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 234 | target_poses_ = ideal_target_poses_; |
milind-u | 526d567 | 2023-04-17 20:09:10 -0700 | [diff] [blame] | 235 | CountConstraints(); |
Milind Upadhyay | cd677a3 | 2022-12-04 13:06:43 -0800 | [diff] [blame] | 236 | } |
| 237 | |
| 238 | TargetMapper::TargetMapper( |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 239 | const ceres::examples::MapOfPoses &target_poses, |
| 240 | const ceres::examples::VectorOfConstraints &target_constraints) |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 241 | : ideal_target_poses_(target_poses), |
| 242 | target_poses_(ideal_target_poses_), |
| 243 | target_constraints_(target_constraints), |
| 244 | T_frozen_actual_(Eigen::Vector3d::Zero()), |
| 245 | R_frozen_actual_(Eigen::Quaterniond::Identity()), |
milind-u | 526d567 | 2023-04-17 20:09:10 -0700 | [diff] [blame] | 246 | vis_robot_(cv::Size(1280, 1000)) { |
| 247 | CountConstraints(); |
| 248 | } |
| 249 | |
| 250 | namespace { |
| 251 | std::pair<TargetMapper::TargetId, TargetMapper::TargetId> MakeIdPair( |
| 252 | const ceres::examples::Constraint3d &constraint) { |
| 253 | auto min_id = std::min(constraint.id_begin, constraint.id_end); |
| 254 | auto max_id = std::max(constraint.id_begin, constraint.id_end); |
| 255 | return std::make_pair(min_id, max_id); |
| 256 | } |
| 257 | } // namespace |
| 258 | |
| 259 | void TargetMapper::CountConstraints() { |
| 260 | for (const auto &constraint : target_constraints_) { |
| 261 | auto id_pair = MakeIdPair(constraint); |
| 262 | if (constraint_counts_.count(id_pair) == 0) { |
| 263 | constraint_counts_[id_pair] = 0; |
| 264 | } |
| 265 | constraint_counts_[id_pair]++; |
| 266 | } |
| 267 | } |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 268 | |
| 269 | std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById( |
| 270 | std::vector<TargetMapper::TargetPose> target_poses, TargetId target_id) { |
| 271 | for (auto target_pose : target_poses) { |
| 272 | if (target_pose.id == target_id) { |
| 273 | return target_pose; |
| 274 | } |
| 275 | } |
| 276 | |
| 277 | return std::nullopt; |
| 278 | } |
| 279 | |
Jim Ostrowski | 49be823 | 2023-03-23 01:00:14 -0700 | [diff] [blame] | 280 | std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById( |
milind-u | 2ab4db1 | 2023-03-25 21:59:23 -0700 | [diff] [blame] | 281 | TargetId target_id) const { |
Jim Ostrowski | 49be823 | 2023-03-23 01:00:14 -0700 | [diff] [blame] | 282 | if (target_poses_.count(target_id) > 0) { |
milind-u | 2ab4db1 | 2023-03-25 21:59:23 -0700 | [diff] [blame] | 283 | return TargetMapper::TargetPose{target_id, target_poses_.at(target_id)}; |
Jim Ostrowski | 49be823 | 2023-03-23 01:00:14 -0700 | [diff] [blame] | 284 | } |
| 285 | |
| 286 | return std::nullopt; |
| 287 | } |
| 288 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 289 | // Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc |
| 290 | // Constructs the nonlinear least squares optimization problem from the pose |
| 291 | // graph constraints. |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 292 | void TargetMapper::BuildTargetPoseOptimizationProblem( |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 293 | const ceres::examples::VectorOfConstraints &constraints, |
| 294 | ceres::examples::MapOfPoses *poses, ceres::Problem *problem) { |
| 295 | CHECK(poses != nullptr); |
| 296 | CHECK(problem != nullptr); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 297 | if (constraints.empty()) { |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 298 | LOG(INFO) << "No constraints, no problem to optimize."; |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 299 | return; |
| 300 | } |
| 301 | |
milind-u | 13ff1a5 | 2023-01-22 17:10:49 -0800 | [diff] [blame] | 302 | ceres::LossFunction *loss_function = new ceres::HuberLoss(2.0); |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 303 | ceres::LocalParameterization *quaternion_local_parameterization = |
| 304 | new ceres::EigenQuaternionParameterization; |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 305 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 306 | for (ceres::examples::VectorOfConstraints::const_iterator constraints_iter = |
| 307 | constraints.begin(); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 308 | constraints_iter != constraints.end(); ++constraints_iter) { |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 309 | const ceres::examples::Constraint3d &constraint = *constraints_iter; |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 310 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 311 | ceres::examples::MapOfPoses::iterator pose_begin_iter = |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 312 | poses->find(constraint.id_begin); |
| 313 | CHECK(pose_begin_iter != poses->end()) |
| 314 | << "Pose with ID: " << constraint.id_begin << " not found."; |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 315 | ceres::examples::MapOfPoses::iterator pose_end_iter = |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 316 | poses->find(constraint.id_end); |
| 317 | CHECK(pose_end_iter != poses->end()) |
| 318 | << "Pose with ID: " << constraint.id_end << " not found."; |
| 319 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 320 | const Eigen::Matrix<double, 6, 6> sqrt_information = |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 321 | constraint.information.llt().matrixL(); |
milind-u | 526d567 | 2023-04-17 20:09:10 -0700 | [diff] [blame] | 322 | |
| 323 | auto id_pair = MakeIdPair(constraint); |
| 324 | CHECK_GT(constraint_counts_.count(id_pair), 0ul) |
| 325 | << "Should have counted constraints for " << id_pair.first << "->" |
| 326 | << id_pair.second; |
| 327 | |
| 328 | // Normalize constraint cost by occurances |
| 329 | size_t constraint_count = constraint_counts_[id_pair]; |
| 330 | // Scale all costs so the total cost comes out to more reasonable numbers |
| 331 | constexpr double kGlobalWeight = 1000.0; |
| 332 | double constraint_weight = |
| 333 | kGlobalWeight / static_cast<double>(constraint_count); |
| 334 | |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 335 | // Ceres will take ownership of the pointer. |
| 336 | ceres::CostFunction *cost_function = |
milind-u | 526d567 | 2023-04-17 20:09:10 -0700 | [diff] [blame] | 337 | ceres::examples::PoseGraph3dErrorTerm::Create( |
| 338 | constraint.t_be, sqrt_information, constraint_weight); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 339 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 340 | problem->AddResidualBlock(cost_function, loss_function, |
| 341 | pose_begin_iter->second.p.data(), |
| 342 | pose_begin_iter->second.q.coeffs().data(), |
| 343 | pose_end_iter->second.p.data(), |
| 344 | pose_end_iter->second.q.coeffs().data()); |
| 345 | |
| 346 | problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(), |
| 347 | quaternion_local_parameterization); |
| 348 | problem->SetParameterization(pose_end_iter->second.q.coeffs().data(), |
| 349 | quaternion_local_parameterization); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 350 | } |
| 351 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 352 | // The pose graph optimization problem has six DOFs that are not fully |
milind-u | 3f5f83c | 2023-01-29 15:23:51 -0800 | [diff] [blame] | 353 | // constrained. This is typically referred to as gauge freedom. You can |
| 354 | // apply a rigid body transformation to all the nodes and the optimization |
| 355 | // problem will still have the exact same cost. The Levenberg-Marquardt |
| 356 | // algorithm has internal damping which mitigates this issue, but it is |
| 357 | // better to properly constrain the gauge freedom. This can be done by |
| 358 | // setting one of the poses as constant so the optimizer cannot change it. |
milind-u | 6ff399f | 2023-03-24 18:33:38 -0700 | [diff] [blame] | 359 | CHECK_NE(poses->count(FLAGS_frozen_target_id), 0ul) |
| 360 | << "Got no poses for frozen target id " << FLAGS_frozen_target_id; |
| 361 | ceres::examples::MapOfPoses::iterator pose_start_iter = |
| 362 | poses->find(FLAGS_frozen_target_id); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 363 | CHECK(pose_start_iter != poses->end()) << "There are no poses."; |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 364 | problem->SetParameterBlockConstant(pose_start_iter->second.p.data()); |
| 365 | problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data()); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 366 | } |
| 367 | |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 368 | std::unique_ptr<ceres::CostFunction> |
| 369 | TargetMapper::BuildMapFittingOptimizationProblem(ceres::Problem *problem) { |
Philipp Schrader | a671252 | 2023-07-05 20:25:11 -0700 | [diff] [blame^] | 370 | // Set up robot visualization. |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 371 | vis_robot_.ClearImage(); |
| 372 | constexpr int kImageWidth = 1280; |
| 373 | constexpr double kFocalLength = 500.0; |
| 374 | vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength); |
| 375 | |
| 376 | const size_t num_targets = FLAGS_max_target_id - FLAGS_min_target_id; |
| 377 | // Translation and rotation error for each target |
| 378 | const size_t num_residuals = num_targets * 6; |
| 379 | // Set up the only cost function (also known as residual). This uses |
| 380 | // auto-differentiation to obtain the derivative (jacobian). |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 381 | std::unique_ptr<ceres::CostFunction> cost_function = std::make_unique< |
| 382 | ceres::AutoDiffCostFunction<TargetMapper, ceres::DYNAMIC, 3, 4>>( |
| 383 | this, num_residuals, ceres::DO_NOT_TAKE_OWNERSHIP); |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 384 | |
| 385 | ceres::LossFunction *loss_function = new ceres::HuberLoss(2.0); |
| 386 | ceres::LocalParameterization *quaternion_local_parameterization = |
| 387 | new ceres::EigenQuaternionParameterization; |
| 388 | |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 389 | problem->AddResidualBlock(cost_function.get(), loss_function, |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 390 | T_frozen_actual_.vector().data(), |
| 391 | R_frozen_actual_.coeffs().data()); |
| 392 | problem->SetParameterization(R_frozen_actual_.coeffs().data(), |
| 393 | quaternion_local_parameterization); |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 394 | return cost_function; |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 395 | } |
| 396 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 397 | // Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 398 | bool TargetMapper::SolveOptimizationProblem(ceres::Problem *problem) { |
| 399 | CHECK_NOTNULL(problem); |
| 400 | |
| 401 | ceres::Solver::Options options; |
| 402 | options.max_num_iterations = FLAGS_max_num_iterations; |
| 403 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 404 | options.minimizer_progress_to_stdout = false; |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 405 | |
| 406 | ceres::Solver::Summary summary; |
| 407 | ceres::Solve(options, problem, &summary); |
| 408 | |
Milind Upadhyay | cd677a3 | 2022-12-04 13:06:43 -0800 | [diff] [blame] | 409 | LOG(INFO) << summary.FullReport() << '\n'; |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 410 | |
| 411 | return summary.IsSolutionUsable(); |
| 412 | } |
| 413 | |
Milind Upadhyay | 05652cb | 2022-12-07 20:51:51 -0800 | [diff] [blame] | 414 | void TargetMapper::Solve(std::string_view field_name, |
| 415 | std::optional<std::string_view> output_dir) { |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 416 | ceres::Problem target_pose_problem_1; |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 417 | BuildTargetPoseOptimizationProblem(target_constraints_, &target_poses_, |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 418 | &target_pose_problem_1); |
| 419 | CHECK(SolveOptimizationProblem(&target_pose_problem_1)) |
| 420 | << "The target pose solve 1 was not successful, exiting."; |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 421 | |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 422 | RemoveOutlierConstraints(); |
| 423 | |
| 424 | // Solve again once we've thrown out bad constraints |
| 425 | ceres::Problem target_pose_problem_2; |
| 426 | BuildTargetPoseOptimizationProblem(target_constraints_, &target_poses_, |
| 427 | &target_pose_problem_2); |
| 428 | CHECK(SolveOptimizationProblem(&target_pose_problem_2)) |
| 429 | << "The target pose solve 2 was not successful, exiting."; |
| 430 | |
| 431 | ceres::Problem map_fitting_problem( |
| 432 | {.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP}); |
| 433 | std::unique_ptr<ceres::CostFunction> map_fitting_cost_function = |
| 434 | BuildMapFittingOptimizationProblem(&map_fitting_problem); |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 435 | CHECK(SolveOptimizationProblem(&map_fitting_problem)) |
| 436 | << "The map fitting solve was not successful, exiting."; |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 437 | map_fitting_cost_function.release(); |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 438 | |
| 439 | Eigen::Affine3d H_frozen_actual = T_frozen_actual_ * R_frozen_actual_; |
| 440 | LOG(INFO) << "H_frozen_actual: " |
| 441 | << PoseUtils::Affine3dToPose3d(H_frozen_actual); |
| 442 | |
| 443 | auto H_world_frozen = |
| 444 | PoseUtils::Pose3dToAffine3d(target_poses_[FLAGS_frozen_target_id]); |
| 445 | auto H_world_frozenactual = H_world_frozen * H_frozen_actual; |
| 446 | |
| 447 | // Offset the solved poses to become the actual ones |
| 448 | for (auto &[id, pose] : target_poses_) { |
| 449 | // Don't offset targets we didn't solve for |
| 450 | if (id < FLAGS_min_target_id || id > FLAGS_max_target_id) { |
| 451 | continue; |
| 452 | } |
| 453 | |
| 454 | // Take the delta between the frozen target and the solved target, and put |
| 455 | // that on top of the actual pose of the frozen target |
| 456 | auto H_world_solved = PoseUtils::Pose3dToAffine3d(pose); |
| 457 | auto H_frozen_solved = H_world_frozen.inverse() * H_world_solved; |
| 458 | auto H_world_actual = H_world_frozenactual * H_frozen_solved; |
| 459 | pose = PoseUtils::Affine3dToPose3d(H_world_actual); |
| 460 | } |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 461 | |
Milind Upadhyay | 05652cb | 2022-12-07 20:51:51 -0800 | [diff] [blame] | 462 | auto map_json = MapToJson(field_name); |
Milind Upadhyay | cd677a3 | 2022-12-04 13:06:43 -0800 | [diff] [blame] | 463 | VLOG(1) << "Solved target poses: " << map_json; |
Milind Upadhyay | 05652cb | 2022-12-07 20:51:51 -0800 | [diff] [blame] | 464 | |
| 465 | if (output_dir.has_value()) { |
| 466 | std::string output_path = |
| 467 | absl::StrCat(output_dir.value(), "/", field_name, ".json"); |
| 468 | LOG(INFO) << "Writing map to file: " << output_path; |
| 469 | aos::util::WriteStringToFileOrDie(output_path, map_json); |
Milind Upadhyay | cd677a3 | 2022-12-04 13:06:43 -0800 | [diff] [blame] | 470 | } |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 471 | |
| 472 | for (TargetId id_start = FLAGS_min_target_id; id_start < FLAGS_max_target_id; |
| 473 | id_start++) { |
| 474 | for (TargetId id_end = id_start + 1; id_end <= FLAGS_max_target_id; |
| 475 | id_end++) { |
| 476 | auto H_start_end = |
| 477 | PoseUtils::Pose3dToAffine3d(target_poses_.at(id_start)).inverse() * |
| 478 | PoseUtils::Pose3dToAffine3d(target_poses_.at(id_end)); |
| 479 | auto constraint = PoseUtils::Affine3dToPose3d(H_start_end); |
| 480 | LOG(INFO) << id_start << "->" << id_end << ": " << constraint.p.norm() |
| 481 | << " meters"; |
| 482 | } |
| 483 | } |
Milind Upadhyay | cd677a3 | 2022-12-04 13:06:43 -0800 | [diff] [blame] | 484 | } |
| 485 | |
Milind Upadhyay | 05652cb | 2022-12-07 20:51:51 -0800 | [diff] [blame] | 486 | std::string TargetMapper::MapToJson(std::string_view field_name) const { |
Milind Upadhyay | cd677a3 | 2022-12-04 13:06:43 -0800 | [diff] [blame] | 487 | flatbuffers::FlatBufferBuilder fbb; |
| 488 | |
| 489 | // Convert poses to flatbuffers |
| 490 | std::vector<flatbuffers::Offset<TargetPoseFbs>> target_poses_fbs; |
| 491 | for (const auto &[id, pose] : target_poses_) { |
milind-u | 3f5f83c | 2023-01-29 15:23:51 -0800 | [diff] [blame] | 492 | target_poses_fbs.emplace_back( |
| 493 | PoseUtils::TargetPoseToFbs(TargetPose{.id = id, .pose = pose}, &fbb)); |
Milind Upadhyay | cd677a3 | 2022-12-04 13:06:43 -0800 | [diff] [blame] | 494 | } |
| 495 | |
Milind Upadhyay | 05652cb | 2022-12-07 20:51:51 -0800 | [diff] [blame] | 496 | const auto field_name_offset = fbb.CreateString(field_name); |
| 497 | flatbuffers::Offset<TargetMap> target_map_offset = CreateTargetMap( |
| 498 | fbb, fbb.CreateVector(target_poses_fbs), field_name_offset); |
Milind Upadhyay | cd677a3 | 2022-12-04 13:06:43 -0800 | [diff] [blame] | 499 | |
| 500 | return aos::FlatbufferToJson( |
| 501 | flatbuffers::GetMutableTemporaryPointer(fbb, target_map_offset), |
| 502 | {.multi_line = true}); |
Milind Upadhyay | 7c20522 | 2022-11-16 18:20:58 -0800 | [diff] [blame] | 503 | } |
| 504 | |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 505 | namespace { |
| 506 | |
| 507 | // Hacks to extract a double from a scalar, which is either a ceres jet or a |
| 508 | // double. Only used for debugging and displaying. |
| 509 | template <typename S> |
| 510 | double ScalarToDouble(S s) { |
| 511 | const double *ptr = reinterpret_cast<double *>(&s); |
| 512 | return *ptr; |
| 513 | } |
| 514 | |
| 515 | template <typename S> |
| 516 | Eigen::Affine3d ScalarAffineToDouble(Eigen::Transform<S, 3, Eigen::Affine> H) { |
| 517 | Eigen::Affine3d H_double; |
| 518 | for (size_t i = 0; i < H.rows(); i++) { |
| 519 | for (size_t j = 0; j < H.cols(); j++) { |
| 520 | H_double(i, j) = ScalarToDouble(H(i, j)); |
| 521 | } |
| 522 | } |
| 523 | return H_double; |
| 524 | } |
| 525 | |
| 526 | } // namespace |
| 527 | |
| 528 | template <typename S> |
| 529 | bool TargetMapper::operator()(const S *const translation, |
| 530 | const S *const rotation, S *residual) const { |
| 531 | using Affine3s = Eigen::Transform<S, 3, Eigen::Affine>; |
| 532 | Eigen::Quaternion<S> R_frozen_actual(rotation[3], rotation[1], rotation[2], |
| 533 | rotation[0]); |
| 534 | Eigen::Translation<S, 3> T_frozen_actual(translation[0], translation[1], |
| 535 | translation[2]); |
| 536 | // Actual target pose in the frame of the fixed pose. |
| 537 | Affine3s H_frozen_actual = T_frozen_actual * R_frozen_actual; |
| 538 | VLOG(2) << "H_frozen_actual: " |
| 539 | << PoseUtils::Affine3dToPose3d(ScalarAffineToDouble(H_frozen_actual)); |
| 540 | |
| 541 | Affine3s H_world_frozen = |
| 542 | PoseUtils::Pose3dToAffine3d(target_poses_.at(FLAGS_frozen_target_id)) |
| 543 | .cast<S>(); |
| 544 | Affine3s H_world_frozenactual = H_world_frozen * H_frozen_actual; |
| 545 | |
| 546 | size_t residual_index = 0; |
| 547 | if (FLAGS_visualize_solver) { |
| 548 | vis_robot_.ClearImage(); |
| 549 | } |
| 550 | |
| 551 | for (const auto &[id, solved_pose] : target_poses_) { |
| 552 | if (id < FLAGS_min_target_id || id > FLAGS_max_target_id) { |
| 553 | continue; |
| 554 | } |
| 555 | |
| 556 | Affine3s H_world_ideal = |
| 557 | PoseUtils::Pose3dToAffine3d(ideal_target_poses_.at(id)).cast<S>(); |
| 558 | Affine3s H_world_solved = |
| 559 | PoseUtils::Pose3dToAffine3d(solved_pose).cast<S>(); |
| 560 | // Take the delta between the frozen target and the solved target, and put |
| 561 | // that on top of the actual pose of the frozen target |
| 562 | auto H_frozen_solved = H_world_frozen.inverse() * H_world_solved; |
| 563 | auto H_world_actual = H_world_frozenactual * H_frozen_solved; |
| 564 | VLOG(2) << id << ": " << H_world_actual.translation(); |
| 565 | Affine3s H_ideal_actual = H_world_ideal.inverse() * H_world_actual; |
| 566 | auto T_ideal_actual = H_ideal_actual.translation(); |
| 567 | VLOG(2) << "T_ideal_actual: " << T_ideal_actual; |
| 568 | VLOG(2); |
| 569 | auto R_ideal_actual = Eigen::AngleAxis<S>(H_ideal_actual.rotation()); |
| 570 | |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 571 | // Weight translation errors higher than rotation. |
| 572 | // 1 m in position error = 0.01 radian (or ~0.573 degrees) |
| 573 | constexpr double kTranslationScalar = 1000.0; |
| 574 | constexpr double kRotationScalar = 100.0; |
milind-u | 8f4e43e | 2023-04-09 17:11:19 -0700 | [diff] [blame] | 575 | |
| 576 | // Penalize based on how much our actual poses matches the ideal |
| 577 | // ones. We've already solved for the relative poses, now figure out |
| 578 | // where all of them fit in the world. |
| 579 | residual[residual_index++] = kTranslationScalar * T_ideal_actual(0); |
| 580 | residual[residual_index++] = kTranslationScalar * T_ideal_actual(1); |
| 581 | residual[residual_index++] = kTranslationScalar * T_ideal_actual(2); |
| 582 | residual[residual_index++] = |
| 583 | kRotationScalar * R_ideal_actual.angle() * R_ideal_actual.axis().x(); |
| 584 | residual[residual_index++] = |
| 585 | kRotationScalar * R_ideal_actual.angle() * R_ideal_actual.axis().y(); |
| 586 | residual[residual_index++] = |
| 587 | kRotationScalar * R_ideal_actual.angle() * R_ideal_actual.axis().z(); |
| 588 | |
| 589 | if (FLAGS_visualize_solver) { |
| 590 | vis_robot_.DrawFrameAxes(ScalarAffineToDouble(H_world_actual), |
| 591 | std::to_string(id), cv::Scalar(0, 255, 0)); |
| 592 | vis_robot_.DrawFrameAxes(ScalarAffineToDouble(H_world_ideal), |
| 593 | std::to_string(id), cv::Scalar(255, 255, 255)); |
| 594 | } |
| 595 | } |
| 596 | if (FLAGS_visualize_solver) { |
| 597 | cv::imshow("Target maps", vis_robot_.image_); |
| 598 | cv::waitKey(0); |
| 599 | } |
| 600 | |
| 601 | // Ceres can't handle residual values of exactly zero |
| 602 | for (size_t i = 0; i < residual_index; i++) { |
| 603 | if (residual[i] == S(0)) { |
| 604 | residual[i] = S(1e-9); |
| 605 | } |
| 606 | } |
| 607 | |
| 608 | return true; |
| 609 | } |
| 610 | |
milind-u | 401de98 | 2023-04-14 17:32:03 -0700 | [diff] [blame] | 611 | TargetMapper::PoseError TargetMapper::ComputeError( |
| 612 | const ceres::examples::Constraint3d &constraint) const { |
| 613 | // Compute the difference between the map-based transform of the end target |
| 614 | // in the start target frame, to the one from this constraint |
| 615 | auto H_start_end_map = |
| 616 | PoseUtils::Pose3dToAffine3d(target_poses_.at(constraint.id_begin)) |
| 617 | .inverse() * |
| 618 | PoseUtils::Pose3dToAffine3d(target_poses_.at(constraint.id_end)); |
| 619 | auto H_start_end_constraint = PoseUtils::Pose3dToAffine3d(constraint.t_be); |
| 620 | ceres::examples::Pose3d delta_pose = PoseUtils::Affine3dToPose3d( |
| 621 | H_start_end_map.inverse() * H_start_end_constraint); |
| 622 | double distance = delta_pose.p.norm(); |
| 623 | Eigen::AngleAxisd err_angle(delta_pose.q); |
| 624 | double angle = std::abs(err_angle.angle()); |
| 625 | return {.angle = angle, .distance = distance}; |
| 626 | } |
| 627 | |
| 628 | TargetMapper::Stats TargetMapper::ComputeStats() const { |
| 629 | Stats stats{.avg_err = {.angle = 0.0, .distance = 0.0}, |
| 630 | .std_dev = {.angle = 0.0, .distance = 0.0}, |
| 631 | .max_err = {.angle = 0.0, .distance = 0.0}}; |
| 632 | |
| 633 | for (const auto &constraint : target_constraints_) { |
| 634 | PoseError err = ComputeError(constraint); |
| 635 | |
| 636 | // Update our statistics |
| 637 | stats.avg_err.distance += err.distance; |
| 638 | if (err.distance > stats.max_err.distance) { |
| 639 | stats.max_err.distance = err.distance; |
| 640 | } |
| 641 | |
| 642 | stats.avg_err.angle += err.angle; |
| 643 | if (err.angle > stats.max_err.angle) { |
| 644 | stats.max_err.angle = err.angle; |
| 645 | } |
| 646 | } |
| 647 | |
| 648 | stats.avg_err.distance /= static_cast<double>(target_constraints_.size()); |
| 649 | stats.avg_err.angle /= static_cast<double>(target_constraints_.size()); |
| 650 | |
| 651 | for (const auto &constraint : target_constraints_) { |
| 652 | PoseError err = ComputeError(constraint); |
| 653 | |
| 654 | // Update our statistics |
| 655 | stats.std_dev.distance += |
| 656 | std::pow(err.distance - stats.avg_err.distance, 2); |
| 657 | |
| 658 | stats.std_dev.angle += std::pow(err.angle - stats.avg_err.angle, 2); |
| 659 | } |
| 660 | |
| 661 | stats.std_dev.distance = std::sqrt( |
| 662 | stats.std_dev.distance / static_cast<double>(target_constraints_.size())); |
| 663 | stats.std_dev.angle = std::sqrt( |
| 664 | stats.std_dev.angle / static_cast<double>(target_constraints_.size())); |
| 665 | |
| 666 | return stats; |
| 667 | } |
| 668 | |
| 669 | void TargetMapper::RemoveOutlierConstraints() { |
| 670 | stats_with_outliers_ = ComputeStats(); |
| 671 | size_t original_size = target_constraints_.size(); |
| 672 | target_constraints_.erase( |
| 673 | std::remove_if( |
| 674 | target_constraints_.begin(), target_constraints_.end(), |
| 675 | [&](const auto &constraint) { |
| 676 | PoseError err = ComputeError(constraint); |
| 677 | // Remove constraints with errors significantly above |
| 678 | // the average |
| 679 | if (err.distance > stats_with_outliers_.avg_err.distance + |
| 680 | FLAGS_outlier_std_devs * |
| 681 | stats_with_outliers_.std_dev.distance) { |
| 682 | return true; |
| 683 | } |
| 684 | if (err.angle > stats_with_outliers_.avg_err.angle + |
| 685 | FLAGS_outlier_std_devs * |
| 686 | stats_with_outliers_.std_dev.angle) { |
| 687 | return true; |
| 688 | } |
| 689 | return false; |
| 690 | }), |
| 691 | target_constraints_.end()); |
| 692 | |
| 693 | LOG(INFO) << "Removed " << (original_size - target_constraints_.size()) |
| 694 | << " outlier constraints out of " << original_size << " total"; |
| 695 | } |
| 696 | |
| 697 | void TargetMapper::DumpStats(std::string_view path) const { |
| 698 | LOG(INFO) << "Dumping mapping stats to " << path; |
| 699 | Stats stats = ComputeStats(); |
| 700 | std::ofstream fout(path.data()); |
| 701 | fout << "Stats after outlier rejection: " << std::endl; |
| 702 | fout << "Average error - angle: " << stats.avg_err.angle |
| 703 | << ", distance: " << stats.avg_err.distance << std::endl |
| 704 | << std::endl; |
| 705 | fout << "Standard deviation - angle: " << stats.std_dev.angle |
| 706 | << ", distance: " << stats.std_dev.distance << std::endl |
| 707 | << std::endl; |
| 708 | fout << "Max error - angle: " << stats.max_err.angle |
| 709 | << ", distance: " << stats.max_err.distance << std::endl; |
| 710 | |
| 711 | fout << std::endl << "Stats before outlier rejection:" << std::endl; |
| 712 | fout << "Average error - angle: " << stats_with_outliers_.avg_err.angle |
| 713 | << ", distance: " << stats_with_outliers_.avg_err.distance << std::endl |
| 714 | << std::endl; |
| 715 | fout << "Standard deviation - angle: " << stats_with_outliers_.std_dev.angle |
| 716 | << ", distance: " << stats_with_outliers_.std_dev.distance << std::endl |
| 717 | << std::endl; |
| 718 | fout << "Max error - angle: " << stats_with_outliers_.max_err.angle |
| 719 | << ", distance: " << stats_with_outliers_.max_err.distance << std::endl; |
| 720 | |
| 721 | fout.flush(); |
| 722 | fout.close(); |
| 723 | } |
| 724 | |
| 725 | void TargetMapper::DumpConstraints(std::string_view path) const { |
| 726 | LOG(INFO) << "Dumping target constraints to " << path; |
| 727 | std::ofstream fout(path.data()); |
| 728 | for (const auto &constraint : target_constraints_) { |
| 729 | fout << constraint << std::endl; |
| 730 | } |
| 731 | fout.flush(); |
| 732 | fout.close(); |
| 733 | } |
| 734 | |
milind-u | fbc5c81 | 2023-04-06 21:24:29 -0700 | [diff] [blame] | 735 | } // namespace frc971::vision |
| 736 | |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 737 | std::ostream &operator<<(std::ostream &os, ceres::examples::Pose3d pose) { |
milind-u | fbc5c81 | 2023-04-06 21:24:29 -0700 | [diff] [blame] | 738 | auto rpy = frc971::vision::PoseUtils::QuaternionToEulerAngles(pose.q); |
Milind Upadhyay | c5beba1 | 2022-12-17 17:41:20 -0800 | [diff] [blame] | 739 | os << absl::StrFormat( |
| 740 | "{x: %.3f, y: %.3f, z: %.3f, roll: %.3f, pitch: " |
| 741 | "%.3f, yaw: %.3f}", |
| 742 | pose.p(0), pose.p(1), pose.p(2), rpy(0), rpy(1), rpy(2)); |
| 743 | return os; |
| 744 | } |
| 745 | |
| 746 | std::ostream &operator<<(std::ostream &os, |
| 747 | ceres::examples::Constraint3d constraint) { |
| 748 | os << absl::StrFormat("{id_begin: %d, id_end: %d, pose: ", |
| 749 | constraint.id_begin, constraint.id_end) |
| 750 | << constraint.t_be << "}"; |
| 751 | return os; |
| 752 | } |