/**
* Uses 2D measurements and projection matrices of two views and returns the corresponding 3D point
* TODO: move it to utility.cpp
*/
Point3D return3DpointFrom2Views(Point2D p2Dv1, Point2D p2Dv2, CcamView V1, CcamView V2)//CvMat* projMat1, CvMat* projMat2);
{
Point3D toRet;
///toRet.x = 0; toRet.y = 0;toRet.z = 0;
Point3D p3Dv1,p3Dv2;
p3Dv1 = backProject(p2Dv1, V1.invProjMatrix);
p3Dv2 = backProject(p2Dv2, V2.invProjMatrix);
double mua, mub;
Point3D pa, pb;
LineLineIntersect(p3Dv1,V1.opticalCenter,p3Dv2,V2.opticalCenter,&pa,&pb,&mua,&mub);
toRet = meanOfPoints(pa,pb);
return toRet;
}
/** Returns the BackProjectionError_struct when two points in different views are intersected
* (here "intersection" means the center of line with minimum distance between 3D lines joining optical center and 3D projection os these points in their respective views).
* \n Here is a step by step working of this function:
* -# Construct the 2 lines (in 3D) pasing through optical center (of their respective views) and intersecting the ViewPlane at given 2D points.
* -# Find the equation of shortest line (in 3D) joining these two lines. This would be a single point, if the lines actually intersect.
* -# Compute the center of this shortest line, and assume it to be "true" 3D point for stereo pair: "pt2D_view1" and "pt2D_view2".
* -# Now to see the validity of "true" 3D point, backProject this 3D point to each of views again, and call these 2D points as : new_pt_view1 , new_pt_view2
* -# Compute backPojection error as Manhattan distance (|x2-x1| + |y2-y1|) between original 2D points (passed as input) and backProjection of "true" 3D points. And also the
length of shortest line joining two backprojected 3D lines.
* -# Return the BackProjection_struct: Manhattan distance for View1, Manhattan Distance for View2 and Length of shortest line joining two 3D lines as the error.
(If the input 2D points, weere actually the same point, the error should be very less for all values in the structure).
* \param : pt2D_view1 2D Point (input) in View 1
* \param : pt2D_view2 2D Point (input) in View 2
* \param : *v1 Pointer to View 1, so that we can access Camera Parameters (a simple technicality).
* \param : *v2 Pointer to View 2, so that we can access Camera Parameters (a simple technicality).
* \return : BackProjectionError_struct. See the description above for details.
\sa LineLineIntersect(), backProject(), get2Dfrom3D()
\relates View
*/
BackProjectionError_struct computeBackProjectionError(Point2D pt2D_view1, Point2D pt2D_view2,View *v1,View *v2)
{
BackProjectionError_struct toReturn;
//////////////////////////////////////////////////////////////////////////////////////////////////
// Case for dummy points - added for G-correspondence
int howManyInvalid = ( (isInvalidPoint(pt2D_view1))?1:0 ) + ( (isInvalidPoint(pt2D_view2))?1:0 );
if(howManyInvalid == 2) // Both the points are dummy point, then error between them = -Infinity
{
toReturn.errV1 = -1.0*INFINITY_ALEPH0;
toReturn.errV2 = -1.0*INFINITY_ALEPH0;
toReturn.shortestLineLength = -1.0*INFINITY_ALEPH0;
return toReturn;
}
if(howManyInvalid == 1) // Exactly one of them is dummy (invalid) point, then error = +Infinity
{
toReturn.errV1 = INFINITY_ALEPH0;
toReturn.errV2 = INFINITY_ALEPH0;
toReturn.shortestLineLength = INFINITY_ALEPH0;
return toReturn;
}
// If execution reach here => None of the point is "invalid" (dummy), then proceed as usual
///////////////////////////////////////////////////////////////////////////////////
Point3D pt3D_view1 = backProject(pt2D_view1,v1->camParams.invProjMatrix); // Get an arbitrary 3D point lying on line joining optical center and intersecting View 1 plane at pt2D_view1
Point3D pt3D_view2 = backProject(pt2D_view2,v2->camParams.invProjMatrix); // Get an arbitrary 3D point lying on line joining optical center and intersecting View 2 plane at pt2D_view2
double mua,mub;
Point3D Pa,Pb;
LineLineIntersect(v1->camParams.opticalCenter,pt3D_view1,v2->camParams.opticalCenter,pt3D_view2,&Pa,&Pb,&mua,&mub); // Find the shortest distance line (Pa-Pb) among two 3D lines
Point3D center; // This is supposedly the 3D point computed from stereo pair pt2D_view1 and pt2D_view2
center = meanOfPoints(Pa,Pb); // Compute center of Pa,Pb
Point2D new_pt_view1 = get2Dfrom3D(center,v1->camParams.projMatrix); // BackProject "center" to view 1
Point2D new_pt_view2 = get2Dfrom3D(center,v2->camParams.projMatrix); // BackProject "center" to view 2
// Compute the Manhattan distances between original and backProjected 2D points
toReturn.errV1 = fabs(pt2D_view1.x-new_pt_view1.x) + fabs(pt2D_view1.y-new_pt_view1.y);
toReturn.errV2 = fabs(pt2D_view2.x-new_pt_view2.x) + fabs(pt2D_view2.y-new_pt_view2.y);
//Compute the length of shortest line joining the backprojected 3D lines.
toReturn.shortestLineLength = euclidDistance(Pa,Pb);
return toReturn;
}
bool PinholeCamera<DISTORTION_T>::backProjectHomogeneous(
const Eigen::Vector2d & imagePoint, Eigen::Vector4d * direction) const
{
Eigen::Vector3d ray;
bool success = backProject(imagePoint, &ray);
direction->template head<3>() = ray;
(*direction)[4] = 1.0; // arbitrary
return success;
}
bool PinholeCamera<DISTORTION_T>::backProjectHomogeneous(
const Eigen::Vector2d & imagePoint, Eigen::Vector4d * direction,
Eigen::Matrix<double, 4, 2> * pointJacobian) const
{
Eigen::Vector3d ray;
Eigen::Matrix<double, 3, 2> pointJacobian3;
bool success = backProject(imagePoint, &ray, &pointJacobian3);
direction->template head<3>() = ray;
(*direction)[4] = 1.0; // arbitrary
pointJacobian->template bottomRightCorner<1,2>() = Eigen::Vector2d::Zero();
pointJacobian->template topLeftCorner<3, 2>() = pointJacobian3;
return success;
}
bool PinholeCamera<DISTORTION_T>::backProjectHomogeneousBatch(
const Eigen::Matrix2Xd & imagePoints, Eigen::Matrix4Xd * directions,
std::vector<bool> * success) const
{
const int numPoints = imagePoints.cols();
directions->row(3) = Eigen::VectorXd::Ones(numPoints);
for (int i = 0; i < numPoints; ++i) {
Eigen::Vector2d imagePoint = imagePoints.col(i);
Eigen::Vector3d point;
bool suc = backProject(imagePoint, &point);
if(success)
success->push_back(suc);
directions->template block<3, 1>(0, i) = point;
}
return true;
}
void ImageSegmentation::computePose(std::vector<Segment> &segments, const cv::Mat &cameraMatrix,
const cv::Mat &distCoefficients, const cv::Mat &planeNormal,
const double planeDistance)
{
if(segments.empty())
{
return;
}
std::vector<cv::Point2d> pointsImg, pointsUndist;
pointsImg.resize(segments.size() * 3);
for(size_t i = 0, j = 0; i < segments.size(); ++i)
{
Segment &seg = segments[i];
pointsImg[j++] = seg.center;
pointsImg[j++] = seg.center + seg.axisX;
pointsImg[j++] = seg.center + seg.axisY;
}
cv::undistortPoints(pointsImg, pointsUndist, cameraMatrix, distCoefficients);
for(size_t i = 0, j = 0; i < segments.size(); ++i)
{
Segment &seg = segments[i];
cv::Mat axisX3D, axisY3D;
backProject(planeNormal, planeDistance, pointsUndist[j++], seg.translation);
backProject(planeNormal, planeDistance, pointsUndist[j++], axisX3D);
backProject(planeNormal, planeDistance, pointsUndist[j++], axisY3D);
axisX3D = axisX3D - seg.translation;
axisY3D = axisY3D - seg.translation;
seg.lengthX = sqrt(axisX3D.dot(axisX3D));
seg.lengthY = sqrt(axisY3D.dot(axisY3D));
axisX3D = axisX3D / seg.lengthX;
double &aX = axisX3D.at<double>(0);
double &aY = axisX3D.at<double>(1);
double &aZ = axisX3D.at<double>(2);
//plane normal from PCL points away from camera....for consistency flip this. so that all estimated poses have an up pointing Z
const double &nX = -planeNormal.at<double>(0);
const double &nY = -planeNormal.at<double>(1);
const double &nZ = -planeNormal.at<double>(2);
if(abs(nX) > abs(nY) && abs(nX) > abs(nZ))
{
aX = (-(nY * aY) - (nZ * aZ)) / nX;
}
else if(abs(nY) > abs(nZ))
{
aY = (-(nX * aX) - (nZ * aZ)) / nY;
}
else
{
aZ = (-(nX * aX) - (nY * aY)) / nZ;
}
axisY3D = -axisX3D.cross(-planeNormal);
seg.rotation = cv::Mat(3, 3, CV_64F);
seg.rotation.at<double>(0, 0) = axisX3D.at<double>(0);
seg.rotation.at<double>(1, 0) = axisX3D.at<double>(1);
seg.rotation.at<double>(2, 0) = axisX3D.at<double>(2);
seg.rotation.at<double>(0, 1) = axisY3D.at<double>(0);
seg.rotation.at<double>(1, 1) = axisY3D.at<double>(1);
seg.rotation.at<double>(2, 1) = axisY3D.at<double>(2);
seg.rotation.at<double>(0, 2) = -planeNormal.at<double>(0);
seg.rotation.at<double>(1, 2) = -planeNormal.at<double>(1);
seg.rotation.at<double>(2, 2) = -planeNormal.at<double>(2);
}
}
/*
This is the primary host code.
*/
int reconstruct(FILE *dataFile){
int j, i;
float min, max;
FILE *fileOut;
float *orig_sinogram;
float *filt_sinogram;
float *output;
float deltaTheta;
size_t size_sinogram = imgwidth*numangles*sizeof(float);
size_t size_result = imgwidth*imgheight*sizeof(float);
logIt(DEBUG, "reconstruct(FILE *dataFile) started.");
output = (float *)malloc(size_result);
// Reserve host memory for sinogram versions
orig_sinogram = (float *)malloc(size_sinogram);
filt_sinogram = (float *)malloc(size_sinogram);
if (dataFile == NULL){
logIt(ERR, "Data file: data was not found.");
return 0;
}
//Read in sinogram from text file. This is probably a slow point!
for(i=0; i < numangles*imgwidth; i++){
fscanf(dataFile,"%f",&orig_sinogram[i]);
}
fclose(dataFile);
//Create filter and save to device memory. The filter is never viewed on the host.
createFilter();
// Filtere the lines of the sinogram using the generated filter.
filterSinogram(orig_sinogram, filt_sinogram);
deltaTheta = PI/cfg.numberOfProjectionAngles; //delta theta in radians
// Back Project and reconstruct full image pixel by pixel.
backProject(deltaTheta, output, filt_sinogram);
logIt(DEBUG, "Backprojection finished.");
fileOut = fopen(outputPath, "wb");
fprintf(fileOut, "P2\n%d %d\n255\n", imgwidth, imgheight);
min = output[0];
max = output[0];
for(j = 0; j<imgheight*imgwidth; j++){
if(output[j]<min){
min = output[j];
}
if(output[j]>max){
max = output[j];
}
}
//Export image data.
for(j=0;j<imgwidth;j++){
for(i = 0; i< imgheight; i++){
//printf("%e ",output_host[j*IMG_WIDTH + i]);
fprintf(fileOut,"%d ",(int)((output[j*imgwidth + i]-min)/(max-min)*255.0));
//fwrite((const void *) &(output_host[j*IMG_WIDTH + i]), 1,3,fileOut);
}
}
//Free all remaining host memory.
free(orig_sinogram);
free(filt_sinogram);
free(output);
fclose(fileOut);
logIt(DEBUG, "reconstruct(FILE *dataFile) finished.");
return 0;
}
