Solver with turret fixes and visualization
Converges generally well, but still has sensitivity to some parameters
In addition to turret and visualization, significant changes include:
-- Added quaternion parameterization for pivot_to_imu
-- Filtering out of bad gyro readings
-- Scaling of rotation error (residual)
Visualization tool is really just a set of helper routines to plot axes
in 3D space in a projected 2D virtual image
Change-Id: I03a6939b6b12df4b8116c30c617a1595781fe635
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index fde5394..1398af5 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -277,8 +277,16 @@
cv::aruco::drawAxis(rgb_image, camera_matrix_, dist_coeffs_, rvec,
tvec, 0.1);
}
+ std::stringstream ss;
+ ss << "tvec = " << tvec << "; |t| = " << tvec_eigen.norm();
+ cv::putText(rgb_image, ss.str(), cv::Point(10, 25),
+ cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(255, 255, 255));
+ ss.str("");
+ ss << "rvec = " << rvec;
+ cv::putText(rgb_image, ss.str(), cv::Point(10, 50),
+ cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(255, 255, 255));
} else {
- LOG(INFO) << "Age: " << age_double << ", invalid pose";
+ VLOG(2) << "Age: " << age_double << ", invalid pose";
}
} else {
VLOG(2) << "Age: " << age_double << ", not enough charuco IDs, got "