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/visualize_robot.h b/frc971/vision/visualize_robot.h
new file mode 100644
index 0000000..391a030
--- /dev/null
+++ b/frc971/vision/visualize_robot.h
@@ -0,0 +1,65 @@
+#ifndef FRC971_VISION_VISUALIZE_ROBOT_H_
+#define FRC971_VISION_VISUALIZE_ROBOT_H_
+
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+
+namespace frc971 {
+namespace vision {
+
+// Helper class to visualize the coordinate frames associated with
+// the robot Based on a virtual camera viewpoint, and camera model,
+// this class can be used to draw 3D coordinate frames in a virtual
+// camera view.
+//
+// Mostly useful just for doing all the projection calculations
+// Leverages Eigen for transforms and opencv for drawing axes
+
+class VisualizeRobot {
+ public:
+ // Set image on which to draw
+ void SetImage(cv::Mat image) { image_ = image; }
+
+ // Set the viewpoint of the camera relative to a global origin
+ void SetViewpoint(Eigen::Affine3d view_origin) {
+ H_world_viewpoint_ = view_origin;
+ }
+
+ // Set camera parameters (for projection into a virtual view)
+ void SetCameraParameters(cv::Mat camera_intrinsics) {
+ camera_mat_ = camera_intrinsics;
+ }
+
+ // Set distortion coefficients (defaults to 0)
+ void SetDistortionCoefficients(cv::Mat dist_coeffs) {
+ dist_coeffs_ = dist_coeffs;
+ }
+
+ // Helper function to project a point in 3D to the virtual image coordinates
+ cv::Point ProjectPoint(Eigen::Vector3d point3d);
+
+ // Draw a line connecting two 3D points
+ void DrawLine(Eigen::Vector3d start, Eigen::Vector3d end);
+
+ // Draw coordinate frame for a target frame relative to the world frame
+ // Axes are drawn (x,y,z) -> (red, green, blue)
+ void DrawFrameAxes(Eigen::Affine3d H_world_target, std::string label = "",
+ double axis_scale = 0.25);
+
+ // TODO<Jim>: Need to implement this, and maybe DrawRobotOutline
+ void DrawBoardOutline(Eigen::Affine3d H_world_board, std::string label = "");
+
+ Eigen::Affine3d H_world_viewpoint_; // Where to view the world from
+ cv::Mat image_; // Image to draw on
+ cv::Mat camera_mat_; // Virtual camera intrinsics (defines fov, center)
+ cv::Mat dist_coeffs_; // Distortion coefficients, if desired (only used in
+ // DrawFrameAxes
+};
+} // namespace vision
+} // namespace frc971
+
+#endif // FRC971_VISION_VISUALIZE_ROBOT_H_