Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <pcl/common/projection_matrix.h>
00039
00041 void
00042 pcl::getCameraMatrixFromProjectionMatrix (
00043 const Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix,
00044 Eigen::Matrix3f& camera_matrix)
00045 {
00046 Eigen::Matrix3f KR = projection_matrix.topLeftCorner <3, 3> ();
00047
00048 Eigen::Matrix3f KR_KRT = KR * KR.transpose ();
00049
00050 Eigen::Matrix3f cam = KR_KRT / KR_KRT.coeff (8);
00051
00052 memset (&(camera_matrix.coeffRef (0)), 0, sizeof (Eigen::Matrix3f::Scalar) * 9);
00053 camera_matrix.coeffRef (8) = 1.0;
00054
00055 if (camera_matrix.Flags & Eigen::RowMajorBit)
00056 {
00057 camera_matrix.coeffRef (2) = cam.coeff (2);
00058 camera_matrix.coeffRef (5) = cam.coeff (5);
00059 camera_matrix.coeffRef (4) = static_cast<float> (sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5)));
00060 camera_matrix.coeffRef (1) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
00061 camera_matrix.coeffRef (0) = static_cast<float> (sqrt (cam.coeff (0) - camera_matrix.coeff (1) * camera_matrix.coeff (1) - cam.coeff (2) * cam.coeff (2)));
00062 }
00063 else
00064 {
00065 camera_matrix.coeffRef (6) = cam.coeff (2);
00066 camera_matrix.coeffRef (7) = cam.coeff (5);
00067 camera_matrix.coeffRef (4) = static_cast<float> (sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5)));
00068 camera_matrix.coeffRef (3) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
00069 camera_matrix.coeffRef (0) = static_cast<float> (sqrt (cam.coeff (0) - camera_matrix.coeff (3) * camera_matrix.coeff (3) - cam.coeff (2) * cam.coeff (2)));
00070 }
00071 }
00072