// Reconstruct 3D Points
void Scanner3D::Reconstruct()
{
MatrixXd RC_H = cvmat2matrix(rotMatrixH);
MatrixXd RC_V = cvmat2matrix(rotMatrixV);
VectorXd TC_H = cvmat2vect(transVectorH);
VectorXd TC_V = cvmat2vect(transVectorV);
// Estimate parameters of reference planes (using least-squares)
MatrixXd X(3,4);
int colNum = X.cols();
int rowNum = X.rows();
X << 0, dX, dX, 0,
0 ,0, dY, dY,
0 ,0 ,0 ,0;
Vector4d Xrow1 = X.row(0);
Vector4d Xrow2 = X.row(1);
Vector4d Xrow3 = X.row(2);
Vector4d hPlane;
hPlane = fitPlane(Xrow1,Xrow2,Xrow3);
VectorXd diffT_vh;
diffT_vh = TC_V - TC_H;
MatrixXd TempX(rowNum,colNum);
for(int i = 0; i<colNum; i++)
TempX.col(i) = diffT_vh;
X = RC_H.transpose()*((RC_V * X) + TempX );
Xrow1 = X.row(0);
Xrow2 = X.row(1);
Xrow3 = X.row(2);
Vector4d vPlane;
vPlane = fitPlane(Xrow1,Xrow2,Xrow3);
vPlane = (-1)*vPlane;
// Calculate camera center (in "horizontal" reference coordinate system)
Vector3d C = ((-1)*RC_H.transpose()) * TC_H;
// Determine implicit representation for the shadow planes
MatrixXd shadowPlaneEnter(recFrames.rows(),4); //"entering" shadow plane
MatrixXd shadowPlaneLeave(recFrames.rows(),4); //"leaving" shadow plane
for(int i = 0; i<recFrames.size(); i++)
{
Vector3d n1_v,n2_v,p1_v,p2_v;
VectorXd vLineEnterRow = vLineEnter.row(i);
VectorXd hLineEnterRow = hLineEnter.row(i);
//Determine true position of the "vertical" shadow boundary (entering)
n1_v = RC_H.transpose() * Pixel2Ray(intersectLines(vLineEnterRow,middleLine));
n2_v = RC_H.transpose() * Pixel2Ray(intersectLines(vLineEnterRow,upperLine));
p1_v = intersectLineWithPlane(C,n1_v,vPlane);
p2_v = intersectLineWithPlane(C,n2_v,vPlane);
Vector3d n1_h,n2_h,p1_h,p2_h;
//Determine true position of the "horizontal" shadow boundary (entering)
n1_h = RC_H.transpose() * Pixel2Ray(intersectLines(hLineEnterRow,middleLine));
n2_h = RC_H.transpose() * Pixel2Ray(intersectLines(hLineEnterRow,lowerLine));
p1_h = intersectLineWithPlane(C,n1_h,hPlane);
p2_h = intersectLineWithPlane(C,n2_h,hPlane);
Vector3d q_v,v_v,q_h,v_h;
// Compute the "entering" shadow plane parameters
q_v = p1_v;
v_v = (p2_v-p1_v).normalized();
q_h = p1_h;
v_h = (p2_h-p1_h).normalized();
shadowPlaneEnter.block(i,0,1,3) = v_v.cross(v_h).transpose();
shadowPlaneEnter.block(i,0,1,3) = shadowPlaneEnter.block(i,0,1,3).normalized();
VectorXd shadowEnterBlock = shadowPlaneEnter.block(i,0,1,3).transpose();
double product = shadowEnterBlock.adjoint()*(q_v+q_h);
shadowPlaneEnter(i,3) = 0.5 * product;
// Determine true position of the "vertical" shadow boundary (leaving)
n1_v = RC_H.transpose() * Pixel2Ray(intersectLines(vLineEnterRow,middleLine));
n2_v = RC_H.transpose() * Pixel2Ray(intersectLines(vLineEnterRow,upperLine));
p1_v = intersectLineWithPlane(C,n1_v,vPlane);
p2_v = intersectLineWithPlane(C,n2_v,vPlane);
// Determine true position of the "horizontal" shadow boundary (leaving)
n1_h = RC_H.transpose() * Pixel2Ray(intersectLines(hLineEnterRow,middleLine));
n2_h = RC_H.transpose() * Pixel2Ray(intersectLines(hLineEnterRow,lowerLine));
p1_h = intersectLineWithPlane(C,n1_h,hPlane);
p2_h = intersectLineWithPlane(C,n2_h,hPlane);
q_v = p1_v;
v_v = (p2_v-p1_v).normalized();
q_h = p1_h;
v_h = (p2_h-p1_h).normalized();
// Compute the "entering" shadow plane parameters
shadowPlaneLeave.block(i,0,1,3) = v_v.cross(v_h).transpose();
shadowPlaneLeave.block(i,0,1,3) = shadowPlaneLeave.block(i,0,1,3).normalized();
VectorXd shadowLeaveBlock = shadowPlaneLeave.block(i,0,1,3).transpose();
product = shadowLeaveBlock.adjoint()*(q_v+q_h);
shadowPlaneLeave(i,3) = 0.5 * product;
}
//Reconstruct 3D points using intersection with shadow plane(s)
IplImage * frameImg = getImage(1);
RgbImage frame(frameImg);
vector<int> rowIdx;
vector<int> colIdx;
for(int i=0; i<shadowValues.cols(); i++)
{
for(int j=0; j<shadowValues.rows(); j++)
{
if( !isNan(shadowEnter(j,i)) && !isNan(shadowLeave(j,i)) )
{
rowIdx.push_back(j);
colIdx.push_back(i);
}
}
}
vector<int> rowIdxx;
vector<int> colIdxx;
for(int i=0; i<rowIdx.size(); i += dSample)
{
rowIdxx.push_back(rowIdx.at(i));
colIdxx.push_back(colIdx.at(i));
}
int npts = rowIdxx.size();
cout << "npts: "<< endl;
cout << npts << endl << endl;
MatrixXd verticesTemp(npts,3);
MatrixXd colorsTemp = 0.65* MatrixXd::Ones(npts, 3);
for(int i=0; i<npts; i++)
{
//Obtain the camera ray for this pixel
Vector3d n = RC_H.transpose() * Pixel2Ray(Vector2d(colIdxx.at(i),rowIdxx.at(i)));
Vector4d wEnter;
Vector3d pEnter;
Vector4d wLeave;
Vector3d pLeave;
//Interpolate "entering" shadow plane parameters (using shadow time)
if( !isNan(shadowEnter(rowIdxx.at(i),colIdxx.at(i))) )
{
double t = shadowEnter(rowIdxx.at(i),colIdxx.at(i));
double t1 = floor(t);
double t2 = t1 + 1;
if(t2 <= recFrames.size())
{
double alpha = (t-t1)/(t2-t1);
wEnter = (1-alpha)*shadowPlaneEnter.row(t1).transpose() + alpha*shadowPlaneEnter.row(t2).transpose();
pEnter = intersectLineWithPlane(C,n,wEnter);
verticesTemp.row(i) = pEnter.transpose();
}
}
//Interpolate "leaving" shadow plane parameters (using shadow time)
if( !isNan(shadowLeave(rowIdxx.at(i),colIdxx.at(i))) )
{
double t = shadowLeave(rowIdxx.at(i),colIdxx.at(i));
double t1 = floor(t);
double t2 = t1 + 1;
if(t2 <= recFrames.size())
{
double alpha = (t-t1)/(t2-t1);
wLeave = (1-alpha)*shadowPlaneEnter.row(t1).transpose() + alpha*shadowPlaneEnter.row(t2).transpose();
pLeave = intersectLineWithPlane(C,n,wLeave);
verticesTemp.row(i) = pLeave.transpose();
}
}
// Average "entering" and "leaving" estimates (if both are available)
if( !isNan(shadowLeave(rowIdxx.at(i),colIdxx.at(i))) && !isNan(shadowEnter(rowIdxx.at(i),colIdxx.at(i))) )
{
if( (pEnter-pLeave).norm() <= distReject )
verticesTemp.row(i) = 0.5*(pEnter + pLeave).transpose();
else //If points do not agree, set coordinate to infinity. This will ensure that it is clipped by the bounding volume.
{
VectorXd ones(3);
ones << 1,1,1;
verticesTemp.row(i) = INF*ones.transpose();
}
}
Vector3d colorLine;
// Assign color per vertex (using source image)
colorLine << (double)frame[rowIdxx.at(i)][colIdxx.at(i)].r/255.0, (double)frame[rowIdxx.at(i)][colIdxx.at(i)].g/255.0, (double)frame[rowIdxx.at(i)][colIdxx.at(i)].b/255.0;
if(verticesTemp.row(i)(0)<10000 && verticesTemp.row(i)(1)<10000 && verticesTemp.row(i)(2)<10000)
{
Vector3d vertex = verticesTemp.row(i).transpose();
Vector3d colorOfVertex = colorLine;
verticesTemp2.push_back(vertex);
colorTemp2.push_back(colorOfVertex);
}
}
}
