CameraManager::CameraManager()
: m_conf(PERSPECTIVE),
m_camSpeed(10.0f),
m_useCam(false),
m_active(0)
{
m_aspect = params::inst().windowSize.x / params::inst().windowSize.y;
m_fov = 45.0f;
m_ncp = 1.0f;
m_fcp = 10.0f * params::inst().scale;
m_rotHeight = -1.0f;
m_rotate = glm::vec3(20.0f, 0.0f, 0.0f);
m_translate = glm::vec3(0.0f, 0.0f, -1.5f * params::inst().scale);
m_zoomTrans = glm::vec3(0.0f);
m_mouseSensitivity = 0.1f;
m_camSpeed = 1.0f;
m_useCam = false;
m_active = 0;
unique_ptr<Camera> cam0(new Camera(glm::vec3(0.0f, 4.0f, 2.0f),
180.0f,
10.0f,
0.0f,
m_fov,
m_ncp,
m_fcp));
cam0->setSpeed(m_camSpeed);
cam0->update();
cam0->loadFrameDirectory("../data/Camera");
m_cameras.push_back(std::move(cam0));
}
void setupCasadiVars(const std::vector<Matrix<X_DIM> >& X, const std::vector<Matrix<U_DIM> >& U, double* XU_arr, double* Sigma0_arr, double* params_arr, double* cam0_arr, double* cam1_arr)
{
int index = 0;
for(int t = 0; t < T-1; ++t) {
for(int i=0; i < X_DIM; ++i) {
XU_arr[index++] = X[t][i];
}
for(int i=0; i < U_DIM; ++i) {
XU_arr[index++] = U[t][i];
}
}
for(int i=0; i < X_DIM; ++i) {
XU_arr[index++] = X[T-1][i];
}
Matrix<X_DIM,X_DIM> Sigma0 = SqrtSigma0*SqrtSigma0;
index = 0;
for(int i=0; i < X_DIM; ++i) {
for(int j=0; j < X_DIM; ++j) {
Sigma0_arr[index++] = Sigma0(i,j);
}
}
params_arr[0] = alpha_belief;
params_arr[1] = alpha_control;
params_arr[2] = alpha_final_belief;
cam0_arr[0] = cam0(0,0);
cam0_arr[1] = cam0(1,0);
cam0_arr[2] = cam0(2,0);
cam1_arr[0] = cam1(0,0);
cam1_arr[1] = cam1(1,0);
cam1_arr[2] = cam1(2,0);
}
/*!
*
*/
cv::Point3f CameraParameters::getCameraLocation(const cv::Mat &Rvec, const cv::Mat &Tvec)
{
cv::Mat m33(3,3,CV_32FC1);
cv::Rodrigues(Rvec, m33) ;
cv::Mat m44=cv::Mat::eye(4,4,CV_32FC1);
for (int i=0; i<3; i++)
for (int j=0; j<3; j++)
m44.at<float>(i,j)=m33.at<float>(i,j);
//now, add translation information
for (int i=0; i<3; i++)
m44.at<float>(i,3)=Tvec.at<float>(i,0);
//invert the matrix
cv::Mat cam0(4, 1, CV_32FC1, cv::Scalar(0.f));
cam0.at<float>(3,0) = 1.f;
cv::Mat pos = m44.inv() * cam0;
return cv::Point3f(pos.at<float>(0,0),pos.at<float>(1,0), pos.at<float>(2,0));
}
