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