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; }