static int BeginPage(PSEngine *psEngine) { int currentPage= psEngine->currentPage; psEngine->e.marked= 1; /* A change in color table can take place only at the beginning of a page. ChangePalette strobes the palette in the Engine base class part into the PSEngine palette. */ psEngine->nColors= 0; /* reset to mono mode */ ChangePalette((Engine *)psEngine); if (psEngine->nchars && PutLine(psEngine)) return 1; if (PutLine(psEngine)) return 1; sprintf(line, "%%%%Page: %d %d", currentPage, currentPage); if (Append(psEngine, line) || PutLine(psEngine)) return 1; if (Append(psEngine, "%%PageBoundingBox: (atend)") || PutLine(psEngine)) return 1; if (Append(psEngine, "GistPrimitives begin /PG save def GI") || PutLine(psEngine)) return 1; /* Set transform viewport to reflect current page orientation */ if (psEngine->landscape != psEngine->e.landscape) { SetPSTransform(&psEngine->e.transform, psEngine->e.landscape); psEngine->landscape= psEngine->e.landscape; } if (psEngine->landscape) { if (Append(psEngine, "LAND") || PutLine(psEngine)) return 1; } if (psEngine->e.colorMode && psEngine->e.palette && psEngine->e.nColors>0) { int i, nColors= psEngine->e.nColors; GpColorCell *palette= psEngine->e.palette; long color; sprintf(line, "%d CT", nColors); if (Append(psEngine, line) || PutLine(psEngine)) return 1; for (i=0 ; i<nColors ; i++) { color= P_R(palette[i])<<16 | P_G(palette[i])<<8 | P_B(palette[i]); sprintf(line, "%06lx", color); if (Append(psEngine, line)) return 1; } if (psEngine->nchars && PutLine(psEngine)) return 1; psEngine->colorMode= 1; /* color table has been written */ psEngine->nColors= nColors; } else { psEngine->colorMode= 0; /* NO color table exists on this page */ /* But, if there is a palette, still want to use grays */ if (psEngine->e.palette && psEngine->e.nColors>0) psEngine->nColors= psEngine->e.nColors; } if (Append(psEngine, "%%EndPageSetup") || PutLine(psEngine)) return 1; return 0; }
static int SetupColor(PSEngine *psEngine, unsigned long color) { int nColors= psEngine->nColors; if (!psEngine->e.marked && BeginPage(psEngine)) return 1; if (color==psEngine->curColor) return 0; if (color<240UL) { /* "CI index C"-- "CI" omitted if current color is indexed */ unsigned long c; GpColorCell *palette= psEngine->e.palette; if (nColors>0) { if (color>=(unsigned long)nColors) color= nColors-1; if (psEngine->colorMode) c= color; /* this page has a color table */ else c= (P_R(palette[color])+ P_G(palette[color])+P_B(palette[color]))/3; /* mono page */ } else { if (color>255) color= 255; c= color; /* No palette ==> no color table on page */ } if (psEngine->curColor>=240UL) { if (Append(psEngine, "CI")) return 1; } sprintf(line, "%ld C", c); if (Append(psEngine, line)) return 1; } else if (color<256UL) { /* standard color command FG, RED, etc. */ if (color<P_GRAYA) color= P_FG; if (Append(psEngine, colorCommands[255UL-color])) return 1; } else { sprintf(line, "16#%lx C", color); if (Append(psEngine, line)) return 1; } psEngine->curColor= color; return 0; }
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);*/ }
static int DrawCells(Engine *engine, GpReal px, GpReal py, GpReal qx, GpReal qy, long width, long height, long nColumns, const GpColor *colors) { PSEngine *psEngine= (PSEngine *)engine; GpXYMap *map= &psEngine->e.map; int nColors= psEngine->nColors; GpColorCell *palette; int ix, iy, idx, idy, depth; long i, j, off; int markEnd= 0; long nLines; char *now= psEngine->line; int nc, ncmax, color, colorMode; if (!psEngine->e.marked && BeginPage(psEngine)) return 1; if (CheckClip(psEngine)) return 1; /* Transform corner coordinates, clipping and adjusting width, height, nColumns, and colors as necessary. */ width = GpClipCells(&map->x, &px, &qx, gistT.window.xmin, gistT.window.xmax, width, &off); colors += gistA.rgb? 3*off : off; height = GpClipCells(&map->y, &py, &qy, gistT.window.ymin, gistT.window.ymax, height, &off); colors += gistA.rgb? 3*nColumns*off : nColumns*off; if (width<=0 || height<=0) return 0; ix= (int)px; iy= (int)py; idx= (int)(qx-px); idy= (int)(qy-py); /* Set bounding box for image if necessary */ if (!psEngine->curClip) { GpBox *wind= &engine->transform.window; GpReal xmin, xmax, ymin, ymax; int xll, yll, xur, yur; if (wind->xmax>wind->xmin) { xmin= wind->xmin; xmax= wind->xmax; } else { xmin= wind->xmax; xmax= wind->xmin; } if (wind->ymax>wind->ymin) { ymin= wind->ymin; ymax= wind->ymax; } else { ymin= wind->ymax; ymax= wind->ymin; } if (px<qx) { if (px>xmin) xmin= px; if (qx<xmax) xmax= qx; } else { if (qx>xmin) xmin= qx; if (px<xmax) xmax= px; } if (py<qy) { if (py>ymin) ymin= py; if (qy<ymax) ymax= qy; } else { if (qy>ymin) ymin= qy; if (py<ymax) ymax= py; } xll= (int)xmin; xur= (int)xmax; yll= (int)ymin; yur= (int)ymax; if (xll<psEngine->pageBB.xll) psEngine->pageBB.xll= xll; if (xur>psEngine->pageBB.xur) psEngine->pageBB.xur= xur; if (yll<psEngine->pageBB.yll) psEngine->pageBB.yll= yll; if (yur>psEngine->pageBB.yur) psEngine->pageBB.yur= yur; } /* Use 4 bit depth if the color table is small, otherwise use 8 bit depth. */ if (nColors>0) { /* Image value will be either */ colorMode= psEngine->colorMode; if (colorMode) { palette= 0; /* palette already written */ if (nColors>16) depth= 8; else depth= 4; } else { palette= psEngine->e.palette; /* must lookup gray level now */ depth= 8; } } else { /* Must assume image varies over maximum possible range */ colorMode= 1; /* That is, use index without trying palette lookup */ depth= 8; palette= 0; } if (gistA.rgb) { /* Write the 6 arguments to the J procedure */ sprintf(line, "%d %d %d %d %d %d", (int)width, (int)height, idx, idy, ix, iy); } else { /* Write the 7 arguments to the I procedure */ sprintf(line, "%d %d %d %d %d %d %d", (int)width, (int)height, depth, idx, idy, ix, iy); } if (Append(psEngine, line)) return 1; if (gistA.rgb) { nLines= 6*width*height; ncmax = 72; /* 12 cells (72 chars) per line */ } else { nLines= width*height; depth= (depth==8); if (depth) nLines*= 2; else if (nLines&1L) nLines++; ncmax = 76; /* Will put 76 or 38 cells per line */ } nLines= 1+(nLines-1)/ncmax; if (nLines>10) { if (psEngine->nchars && PutLine(psEngine)) return 1; sprintf(line, "%%%%BeginData: %ld ASCII Lines", nLines+1); if (Append(psEngine, line) || PutLine(psEngine)) return 1; markEnd= 1; } if (Append(psEngine, gistA.rgb?"J":"I") || PutLine(psEngine)) return 1; i= j= 0; while (nLines--) { for (nc=0 ; nc<ncmax ; ) { if (i>=width) { height--; if (height<=0) break; i= 0; j+= nColumns; } if (gistA.rgb) { const GpColor *ccell = &colors[3*(i+j)]; now[nc++] = hexChar[ccell[0]>>4]; now[nc++] = hexChar[ccell[0]&0xf]; now[nc++] = hexChar[ccell[1]>>4]; now[nc++] = hexChar[ccell[1]&0xf]; now[nc++] = hexChar[ccell[2]>>4]; now[nc++] = hexChar[ccell[2]&0xf]; } else { color= colors[i+j]; if (color>=nColors && nColors>0) color= nColors-1; if (!colorMode) color= (P_R(palette[color])+ P_G(palette[color])+P_B(palette[color]))/3; if (depth) { /* 2 hex chars per cell */ now[nc++]= hexChar[color>>4]; now[nc++]= hexChar[color&0xf]; } else { /* 1 hex char per cell */ now[nc++]= hexChar[color]; } } i++; }