CameraInfo kinectPoseFromEigen(std::pair<Eigen::Matrix3d,Eigen::Vector3d> pos,float fx, float fy, float cx, float cy){
CameraInfo result;
cv::Mat intrinsic = cv::Mat::eye(3,3,cv::DataType<double>::type);
//Kinect Intrinsic Parameters
intrinsic.at<double>(0,0) = fx;
intrinsic.at<double>(1,1) = fy;
intrinsic.at<double>(0,2) = cx;
intrinsic.at<double>(1,2) = cy;
result.setIntrinsic(intrinsic);
Eigen::Matrix3d rotation = pos.first;
cv::Mat rotation2 = cv::Mat::eye(3,3,cv::DataType<double>::type);
for(int i=0;i<3;i++) for(int j=0;j<3;j++) rotation2.at<double>(i,j) = rotation(i,j);
result.setRotation(rotation2);
Eigen::Vector3d translation = pos.second;
cv::Mat translation2 = cv::Mat::zeros(3,1,cv::DataType<double>::type);
for(int i=0;i<3;i++) translation2.at<double>(i,0) = translation(i);
result.setTranslation(translation2);
return result;
}
CameraInfo OnlineFusionObject::fromSE3(pi::SE3d pose,float fx,float fy,float cx,float cy)
{
CameraInfo result;
cv::Mat intrinsic = cv::Mat::eye(3,3,cv::DataType<double>::type);
//Kinect Intrinsic Parameters
intrinsic.at<double>(0,0) = fx;
intrinsic.at<double>(1,1) = fy;
intrinsic.at<double>(0,2) = cx;
intrinsic.at<double>(1,2) = cy;
result.setIntrinsic(intrinsic);
cv::Mat rotation2 = cv::Mat::eye(3,3,cv::DataType<double>::type);
// for(int i=0;i<3;i++) for(int j=0;j<3;j++) rotation2.at<double>(i,j) = rotation(i,j);
pose.get_rotation().getMatrix((double*)rotation2.data);
result.setRotation(rotation2);
pi::Point3d trans=pose.get_translation();
cv::Mat translation2 = cv::Mat::zeros(3,1,cv::DataType<double>::type);
translation2.at<double>(0,0) = trans.x;
translation2.at<double>(1,0) = trans.y;
translation2.at<double>(2,0) = trans.z;
result.setTranslation(translation2);
return result;
}
