| 31 | } |
| 32 | |
| 33 | bool sloam::TwoStepOptimizePose(const SE3& poseEstimate, const bool optimTrees, const bool optimGround, |
| 34 | const std::vector<ObjectMatch<Cylinder>> &allTMatch, |
| 35 | const std::vector<ObjectMatch<Plane>> &allGMatch, |
| 36 | SE3 &tf) |
| 37 | { |
| 38 | double treeOut[3]; |
| 39 | double groundOut[3]; |
| 40 | OptimizeXYYaw(poseEstimate, optimTrees, allTMatch, treeOut); |
| 41 | OptimizeZRollPitch(poseEstimate, optimGround, allGMatch, groundOut); |
| 42 | |
| 43 | // roll, pitch, yaw |
| 44 | double q[4]; |
| 45 | double angleAxis[3] = {groundOut[1], groundOut[2], treeOut[2]}; |
| 46 | ceres::AngleAxisToQuaternion(angleAxis, q); |
| 47 | Quat quat(q[0], q[1], q[2], q[3]); |
| 48 | tf.setQuaternion(quat); |
| 49 | tf.translation()[0] = treeOut[0]; |
| 50 | tf.translation()[1] = treeOut[1]; |
| 51 | tf.translation()[2] = groundOut[0]; |
| 52 | return true; |
| 53 | } |
| 54 | |
| 55 | void sloam::OptimizeXYYaw(const SE3 &poseEstimate, const bool optimize, const std::vector<ObjectMatch<Cylinder>> &allTMatch, double* out) |
| 56 | { |
nothing calls this directly
no outgoing calls
no test coverage detected