//--------------------------------------------------------------
void testApp::setup(){
// loading all the Paris data
// To create the input video block
for (int k = 1; k<= Nlast; k++){
ofImage auxImage;
auxImage.loadImage("ParisFrames/paris"+ofToString(k)+".jpg");
BlockDims.x = auxImage.width;
BlockDims.y = auxImage.height;
BlockDims.z = Nlast;
ofxCvColorImage AuxCVImage;
AuxCVImage.allocate(auxImage.width,auxImage.height);
AuxCVImage.setFromPixels(auxImage.getPixels(),auxImage.width,auxImage.height);
cv::Mat AuxMat(AuxCVImage.getCvImage());
TheFramesInput.push_back(AuxMat.clone());
}
//initial value
OutFramesN = BlockDims.z;
// vidGrabber.setVerbose(true);
// vidGrabber.initGrabber(Nx,Ny);
ofSetFrameRate(15);
inOp =50;
// the input grain list
UpdateScheduler();
//UpdateSchedulerUpDown(TheUf,1);
// GrainCamera = RotateAux(ofVec3f(0.0,0.0,-1.1* sqrt(BlockDims.x*BlockDims.x+BlockDims.y*BlockDims.y+
// BlockDims.z*BlockDims.z)/2.0), 85.0, 0.0,
// ofVec3f(BlockDims.x/2.0,BlockDims.y/2.0,BlockDims.z/2.0));
// GrainUpVector = ofVec3f(0.0,-1.0,0.0);
//side
// GrainCamera = ofVec3f(1.2*BlockDims.x,BlockDims.y/2.0,BlockDims.z/2.0);
//vertex
// GrainCamera = ofVec3f(BlockDims.x,0.0,0.0);
// corner
// GrainCamera = ofVec3f(BlockDims.x,BlockDims.y/2.0,0.0);
// corner side
// GrainCamera = ofVec3f(BlockDims.x,0.0,BlockDims.z/2);
// up
// GrainCamera = ofVec3f(BlockDims.x/2.0,0.0,BlockDims.z/2);
//GrainUpVector = ofVec3f(-1.0,0.0,0.0);
ScaleCorrectFactor =CalculateScalecorrect();
UpdateBool = true;
}
void CameraFrames::ProjectToCameraPlane(Eigen::MatrixXf cloud){
Eigen::MatrixXf P_L(3,4);
Eigen::MatrixXf P_R(3,4);
Eigen::MatrixXf Point2dimensions_right(3,cloud.cols());
Eigen::MatrixXf Point2dimensions_left(3,cloud.cols());
Eigen::MatrixXf u = Eigen::MatrixXf::Ones(1,cloud.cols());
Eigen::MatrixXf translation_l(4,1);
Eigen::MatrixXf translation_r(4,1);
Eigen::Matrix3f rotationMatrix;
Eigen::MatrixXf toLeftAxis(4,cloud.cols());
Eigen::MatrixXf AuxMat(3,cloud.cols());
cv::Point2f aux_l(0,0);
cv::Point2f aux_r(0,0);
modelpoints2dright.clear();
modelpoints2dleft.clear();
//The Projection matrix projects points in the camera frame.
//Construct the Projection Matrix of the Left Camera
P_L(0,0) = leftCamInfo->P[0]; P_L(1,0) = leftCamInfo->P[4]; P_L(2,0) = leftCamInfo->P[8];
P_L(0,1) = leftCamInfo->P[1]; P_L(1,1) = leftCamInfo->P[5]; P_L(2,1) = leftCamInfo->P[9];
P_L(0,2) = leftCamInfo->P[2]; P_L(1,2) = leftCamInfo->P[6]; P_L(2,2) = leftCamInfo->P[10];
P_L(0,3) = leftCamInfo->P[3]; P_L(1,3) = leftCamInfo->P[7]; P_L(2,3) = leftCamInfo->P[11];
//Construct the Projection Matrix of the Right Camera
P_R(0,0) = rightCamInfo->P[0]; P_R(1,0) = rightCamInfo->P[4]; P_R(2,0) = rightCamInfo->P[8];
P_R(0,1) = rightCamInfo->P[1]; P_R(1,1) = rightCamInfo->P[5]; P_R(2,1) = rightCamInfo->P[9];
P_R(0,2) = rightCamInfo->P[2]; P_R(1,2) = rightCamInfo->P[6]; P_R(2,2) = rightCamInfo->P[10];
P_R(0,3) = 0; P_R(1,3) = rightCamInfo->P[7]; P_R(2,3) = rightCamInfo->P[11];
/*Project the 3D point onto the camera planes [u v w] = ProjMat * (3DPoints+Translation).
Translation is neccesary to put the 3D points on the corresponding camera frame.
Baseline = 0.04 and global frame in the midle of the baseline. */
rotationMatrix = Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY())
* Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ())
* Eigen::AngleAxisf(-90, Eigen::Vector3f::UnitX());
translation_l << 0, 0, 0, 0;
translation_r << 0.05, 0, 0, 0;
AuxMat = rotationMatrix*cloud;
toLeftAxis.row(0) = -AuxMat.row(0); //The Leap is left handed
toLeftAxis.row(1) = AuxMat.row(1);
toLeftAxis.row(2) = AuxMat.row(2);
toLeftAxis.row(3).setOnes();
Point2dimensions_left = P_L*(toLeftAxis+(translation_l*u));
Point2dimensions_right = P_R*(toLeftAxis+(translation_r*u));
//Divide by the Homogeneous Coordinates to get the 2D Points
Point2dimensions_left.row(0) = Point2dimensions_left.row(0).cwiseQuotient(Point2dimensions_left.row(2));
Point2dimensions_left.row(1) = Point2dimensions_left.row(1).cwiseQuotient(Point2dimensions_left.row(2));
Point2dimensions_right.row(0) = Point2dimensions_right.row(0).cwiseQuotient(Point2dimensions_right.row(2));
Point2dimensions_right.row(1) = Point2dimensions_right.row(1).cwiseQuotient(Point2dimensions_right.row(2));
for(int i = 0; i < cloud.cols(); i++){
aux_l.x = Point2dimensions_left(0,i);
aux_l.y = Point2dimensions_left(1,i);
aux_r.x = Point2dimensions_right(0,i);
aux_r.y = Point2dimensions_right(1,i);
if(aux_l.x < 280 && aux_l.x >=0 && aux_r.x < 280 && aux_r.x >= 0 && aux_l.y < 220 && aux_l.y >= 0 && aux_r.y < 220 && aux_r.y >=0 ){
modelpoints2dleft.push_back(aux_l);
modelpoints2dright.push_back(aux_r);
}
}
/* cv::Point center(floor(aux_l.x), floor(aux_l.y));
cv::circle(LeftFrame,center,0.1,cv::Scalar(255,255,255),-1);
cv::Point center2(floor(aux_l.x), floor(aux_l.y));
cv::circle(RightFrame,center2,0.1,cv::Scalar(255,255,255),-1);
cv::imshow("pointsleft", LeftFrame);
cv::imshow("pointsright", RightFrame);
cv::waitKey(1);*/
}
