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