cv::Point3d GraphGridMapper::to3D(cv::Point3d &p, const Eigen::Isometry3d &camera_transform, const image_geometry::PinholeCameraModel &camera_model)
{
int width = camera_model.cameraInfo().width;
int height = camera_model.cameraInfo().height;
int u = round(p.x);
if(u < 0) {
u = 0;
} else if (u >= width) {
u = width -1;
}
int v = round(p.y);
if(v < 0) {
v = 0;
} else if (v >= height) {
v = height - 1;
}
cv::Point3d p3D(-1.0,-1.0,std::numeric_limits<double>::infinity());
if (p.z != 0 && !isnan(p.z))
{
p3D.x = (u - camera_model.cx()) * p.z / camera_model.fx();
p3D.y = (v - camera_model.cy()) * p.z / camera_model.fy();
p3D.z = p.z;
Eigen::Vector3d vec(p3D.x, p3D.y, p3D.z);
vec = camera_transform * vec.homogeneous();
p3D.x = vec(0);
p3D.y = vec(1);
p3D.z = vec(2);
}
return p3D;
}
cv::Mat projectWithEigen()
{
// Transform meshes into camera frame
// For each frame in vector
for (int frame = 0; frame < mMeshFrameIDs.size(); frame++)
{
// Lookup current transform
Eigen::Isometry3 transform;
transform = transforms.at(mMeshFrameIDs[frame]);
// Get copy of mesh for each frame
ap::Mesh* sourceMesh;
ap::Mesh* transformedMesh;
//std::cerr << "Getting frame " << frame << " : " << mMeshFrameIDs[frame] << std::endl;
MeshMap::iterator scene_i = scenes.find(mMeshFrameIDs[frame]);
if (scenes.end() == scene_i) { continue; }
sourceMesh = scene_i->second;
MeshMap::iterator scene_t = transformedScenes.find(mMeshFrameIDs[frame]);
if (transformedScenes.end() == scene_t) { continue; }
transformedMesh = scene_t->second;
// Transform mesh into camera frame
for (int i = 0; i < sourceMesh->vertices.size(); i++)
{
Eigen::Vector3 newVertex = transform * sourceMesh->vertices[i];
//std::cerr << mesh->vertices[i].transpose() << "\t->\t" << newVertex.transpose() << std::endl;
transformedMesh->vertices[i] = newVertex;
}
}
// For each pixel in camera image
cv::Mat robotImage(mCameraModel.cameraInfo().height, mCameraModel.cameraInfo().width, CV_32F);
float* pixelPtr = (float*)robotImage.data;
float maxDepth = 0;
for (int v = 0; v < robotImage.rows; v++)
{
for (int u = 0; u < robotImage.cols; u++)
{
// Create a ray through the pixel
int pixelIdx = u + (v * robotImage.cols);
//std::cerr << "Pixel (" << u << "," << v << ")" << std::endl;
cv::Point2d pixel = cv::Point2d(u, v);
cv::Point3d cvRay = mCameraModel.projectPixelTo3dRay(pixel);
// Convert cvRay to ap::Ray
ap::Ray ray;
ray.point = Eigen::Vector3::Zero();
ray.vector.x() = cvRay.x; ray.vector.y() = cvRay.y; ray.vector.z() = cvRay.z;
ray.vector.normalize();
//std::cerr << ray.vector.transpose() << std::endl;
// For each frame in vector
for (int frame = 0; frame < mMeshFrameIDs.size(); frame++)
{
MeshMap::iterator scene_i = transformedScenes.find(mMeshFrameIDs[frame]);
if (transformedScenes.end() == scene_i)
{
continue;
}
ap::Mesh* mesh = scene_i->second;
// For each triangle in mesh
for (int i = 0; i < mesh->faces.size(); i++)
{
// Check for intersection. If finite, set distance
ap::Triangle triangle(mesh->vertices[mesh->faces[i].vertices[0]],
mesh->vertices[mesh->faces[i].vertices[1]],
mesh->vertices[mesh->faces[i].vertices[2]]);
Eigen::Vector3 intersection = ap::intersectRayTriangle(ray, triangle);
if (std::isfinite(intersection.x()))
{
float d = intersection.norm();
float val = pixelPtr[pixelIdx];
if (val == 0 || val > d)
{
pixelPtr[pixelIdx] = d;
}
if (d > maxDepth)
{
maxDepth = d;
}
}
}
}
}
}
// Return the matrix
if (maxDepth == 0) { maxDepth = 1;}
return robotImage/maxDepth;
}