/* * ===================================================================================== * * Filename: camera.cpp * * Description: Class for camera * * Version: 1.0 * Created: 03/19/2017 09:39:58 PM * Revision: none * Compiler: gcc * * Author: Martin Miller (MHM), miller7@illinois.edu * Organization: Aerospace Robotics and Controls Lab (ARC) * * ===================================================================================== */ #include "camera.h" #include /* *-------------------------------------------------------------------------------------- * Class: Camera * Method: Camera * Description: constructor *-------------------------------------------------------------------------------------- */ Camera::Camera (const char *fin) { YAML::Node config = YAML::LoadFile(fin); _K(0,0) = BINNING*config["camera_matrix"]["data"][0].as(); _K(0,1) = BINNING*config["camera_matrix"]["data"][1].as(); _K(0,2) = BINNING*config["camera_matrix"]["data"][2].as(); _K(1,0) = BINNING*config["camera_matrix"]["data"][3].as(); _K(1,1) = BINNING*config["camera_matrix"]["data"][4].as(); _K(1,2) = BINNING*config["camera_matrix"]["data"][5].as(); _K(2,0) = BINNING*config["camera_matrix"]["data"][6].as(); _K(2,1) = BINNING*config["camera_matrix"]["data"][7].as(); _K(2,2) = config["camera_matrix"]["data"][8].as(); _d[0] = config["distortion_coefficients"]["data"][0].as(); _d[1] = config["distortion_coefficients"]["data"][1].as(); _d[2] = config["distortion_coefficients"]["data"][2].as(); _d[3] = config["distortion_coefficients"]["data"][3].as(); _T(0,0) = config["T_cam_imu"][0][0].as(); _T(0,1) = config["T_cam_imu"][0][1].as(); _T(0,2) = config["T_cam_imu"][0][2].as(); _T(0,3) = config["T_cam_imu"][0][3].as(); _T(1,0) = config["T_cam_imu"][1][0].as(); _T(1,1) = config["T_cam_imu"][1][1].as(); _T(1,2) = config["T_cam_imu"][1][2].as(); _T(1,3) = config["T_cam_imu"][1][3].as(); _T(2,0) = config["T_cam_imu"][2][0].as(); _T(2,1) = config["T_cam_imu"][2][1].as(); _T(2,2) = config["T_cam_imu"][2][2].as(); _T(2,3) = config["T_cam_imu"][2][3].as(); _T(3,0) = config["T_cam_imu"][3][0].as(); _T(3,1) = config["T_cam_imu"][3][1].as(); _T(3,2) = config["T_cam_imu"][3][2].as(); _T(3,3) = config["T_cam_imu"][3][3].as(); } /* ----- end of method Camera::Camera (constructor) ----- */ /* *-------------------------------------------------------------------------------------- * Class: Camera * Method: Camera :: Rc2b * Description: Returns the rotation matrix that transforms a point from the * camera frame to the body frame. *-------------------------------------------------------------------------------------- */ Matrix Camera::Rc2b ( ) { return _T.block<3,3>(0,0).transpose(); } /* ----- end of method Camera::Rc2b ----- */ /* *-------------------------------------------------------------------------------------- * Class: Camera * Method: Camera :: Rc2b4 * Description: Returns the 4x4 Rc2b matrix *-------------------------------------------------------------------------------------- */ Matrix Camera::Rc2b4 ( ) { Matrix R; R = Matrix::Identity(); R.block<3,3>(0,0) = Rc2b(); return R ; } /* ----- end of method Camera::Rc2b4 ----- */ /* *-------------------------------------------------------------------------------------- * Class: Camera * Method: Camera :: K4 * Description: Returns the 4x4 camera matrix. *-------------------------------------------------------------------------------------- */ Matrix Camera::K4 ( ) { Matrix K4; K4 = Matrix::Identity(); K4.block<3,3>(0,0) = K(); return K4 ; } /* ----- end of method Camera::K4 ----- */ Matrix Camera::K ( ) { return _K ; } /* ----- end of method Camera::K ----- */ Vector4d Camera::d ( ) { return _d; } /* ----- end of method Camera::d ----- */ Vector3d Camera::img2body ( Vector3d &xi ) { Vector3d xc, xb; xc = K().inverse()*xi; // Normalize xc /= xc.norm(); xb = Rc2b()*xc; return xb; } /* ----- end of method Camera::img2body ----- */