i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Export Added RoiRatios parameter to texturing (#171)

Browse Source
This commit is contained in:
matlabbe
2017-04-20 11:51:23 -04:00
parent c068c3c33a
commit a274f3cf01
10 changed files with 279 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

85
corelib/src/pcl18/surface/impl/texture_mapping.hpp
View File

@@ -1389,44 +1389,61 @@ pcl::TextureMapping<PointInT>::getTriangleCircumcscribedCircleCentroid ( const p
template<typename PointInT> inline bool
pcl::TextureMapping<PointInT>::getPointUVCoordinates(const PointInT &pt, const Camera &cam, pcl::PointXY &UV_coordinates)
{
if (pt.z > 0 && (max_distance_<=0.0f || pt.z<max_distance_))
{
// compute image center and dimension
double sizeX = cam.width;
double sizeY = cam.height;
double cx, cy;
if (cam.center_w > 0)
cx = cam.center_w;
else
cx = sizeX / 2.0;
if (cam.center_h > 0)
cy = cam.center_h;
else
cy = sizeY / 2.0;
if (pt.z > 0 && (max_distance_<=0.0f || pt.z<max_distance_))
{
// compute image center and dimension
double sizeX = cam.width;
double sizeY = cam.height;
double cx, cy;
if (cam.center_w > 0)
cx = cam.center_w;
else
cx = sizeX / 2.0;
if (cam.center_h > 0)
cy = cam.center_h;
else
cy = sizeY / 2.0;
double focal_x, focal_y;
if (cam.focal_length_w > 0)
focal_x = cam.focal_length_w;
else
focal_x = cam.focal_length;
if (cam.focal_length_h > 0)
focal_y = cam.focal_length_h;
else
focal_y = cam.focal_length;
double focal_x, focal_y;
if (cam.focal_length_w > 0)
focal_x = cam.focal_length_w;
else
focal_x = cam.focal_length;
if (cam.focal_length_h > 0)
focal_y = cam.focal_length_h;
else
focal_y = cam.focal_length;
// project point on camera's image plane
UV_coordinates.x = static_cast<float> ((focal_x * (pt.x / pt.z) + cx) / sizeX); //horizontal
UV_coordinates.y = 1.0f - static_cast<float> ((focal_y * (pt.y / pt.z) + cy) / sizeY); //vertical
// project point on camera's image plane
UV_coordinates.x = static_cast<float> ((focal_x * (pt.x / pt.z) + cx) / sizeX); //horizontal
UV_coordinates.y = static_cast<float> ((focal_y * (pt.y / pt.z) + cy) / sizeY); //vertical
// point is visible!
if (UV_coordinates.x >= 0.0 && UV_coordinates.x <= 1.0 && UV_coordinates.y >= 0.0 && UV_coordinates.y <= 1.0)
return (true); // point was visible by the camera
}
if(cam.roi.size() == 4)
{
if( UV_coordinates.x < cam.roi[0]/sizeX ||
UV_coordinates.y < cam.roi[1]/sizeY ||
UV_coordinates.x > (cam.roi[0]+cam.roi[2])/sizeX ||
UV_coordinates.y > (cam.roi[1]+cam.roi[3])/sizeY)
{
// point is NOT in region of interest of the camera
UV_coordinates.x = -1.0f;
UV_coordinates.y = -1.0f;
return false;
}
}
// point is NOT visible by the camera
UV_coordinates.x = -1.0f;
UV_coordinates.y = -1.0f;
return (false); // point was not visible by the camera
// original code of PCL inverted y
UV_coordinates.y = 1.0f - UV_coordinates.y;
// point is visible!
if (UV_coordinates.x >= 0.0 && UV_coordinates.x <= 1.0 && UV_coordinates.y >= 0.0 && UV_coordinates.y <= 1.0)
return (true); // point was visible by the camera
}
// point is NOT visible by the camera
UV_coordinates.x = -1.0f;
UV_coordinates.y = -1.0f;
return (false); // point was not visible by the camera
}
///////////////////////////////////////////////////////////////////////////////////////////////

1
corelib/src/pcl18/surface/texture_mapping.h
View File

@@ -77,6 +77,7 @@ namespace pcl
double height;
double width;
std::string texture_file;
std::vector<double> roi; // [x, y, width, height]
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

14
corelib/src/util2d.cpp
View File

@@ -1147,28 +1147,30 @@ cv::Rect computeRoi(const cv::Size & imageSize, const std::vector<float> & roiRa
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
//left roi
if(roiRatios[0] > 0 && roiRatios[0] < 1 - roiRatios[1])
if(roiRatios[0] > 0 && roiRatios[0] < 1.0f - roiRatios[1])
{
roi.x = width * roiRatios[0];
}
//right roi
if(roiRatios[1] > 0 && roiRatios[1] < 1 - roiRatios[0])
if(roiRatios[1] > 0 && roiRatios[1] < 1.0f - roiRatios[0])
{
roi.width -= width * roiRatios[1] + width * roiRatios[0];
roi.width -= width * roiRatios[1];
}
roi.width -= roi.x;
//top roi
if(roiRatios[2] > 0 && roiRatios[2] < 1 - roiRatios[3])
if(roiRatios[2] > 0 && roiRatios[2] < 1.0f - roiRatios[3])
{
roi.y = height * roiRatios[2];
}
//bottom roi
if(roiRatios[3] > 0 && roiRatios[3] < 1 - roiRatios[2])
if(roiRatios[3] > 0 && roiRatios[3] < 1.0f - roiRatios[2])
{
roi.height -= height * roiRatios[3] + height * roiRatios[2];
roi.height -= height * roiRatios[3];
}
roi.height -= roi.y;
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
return roi;

8
corelib/src/util3d.cpp
View File

@@ -992,10 +992,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
cv::Mat depth(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows));
CameraModel model = sensorData.cameraModels()[i];
if( roiRatios.size() == 4 &&
(roiRatios[0] > 0.0f ||
roiRatios[1] > 0.0f ||
roiRatios[2] > 0.0f ||
roiRatios[3] > 0.0f))
((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
(roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
(roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
(roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
{
cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
cv::Rect roiRgb = util2d::computeRoi(rgb, roiRatios);

16
corelib/src/util3d_surface.cpp
View File

@@ -573,9 +573,11 @@ pcl::PolygonMesh::Ptr createMesh(
pcl::texture_mapping::CameraVector createTextureCameras(
const std::map<int, Transform> & poses,
const std::map<int, CameraModel> & cameraModels)
const std::map<int, CameraModel> & cameraModels,
const std::vector<float> & roiRatios)
{
UASSERT(poses.size() == cameraModels.size());
UASSERT(roiRatios.empty() || roiRatios.size() == 4);
pcl::texture_mapping::CameraVector cameras(poses.size());
std::map<int, Transform>::const_iterator poseIter=poses.begin();
std::map<int, CameraModel>::const_iterator modelIter=cameraModels.begin();
@@ -602,6 +604,14 @@ pcl::texture_mapping::CameraVector createTextureCameras(
cam.height=modelIter->second.imageHeight();
cam.width=modelIter->second.imageWidth();
cam.texture_file = uFormat("%d", poseIter->first);
if(!roiRatios.empty())
{
cam.roi.resize(4);
cam.roi[0] = cam.width * roiRatios[0]; // left -> x
cam.roi[1] = cam.height * roiRatios[2]; // top -> y
cam.roi[2] = cam.width * (1.0 - roiRatios[1]) - cam.roi[0]; // right -> width
cam.roi[3] = cam.height * (1.0 - roiRatios[3]) - cam.roi[1]; // bottom -> height
}
cameras[oi++] = cam;
}
return cameras;
@@ -613,6 +623,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
const std::map<int, CameraModel> & cameraModels,
float maxDistance,
int minClusterSize,
const std::vector<float> & roiRatios,
const ProgressState * state,
std::vector<std::map<int, pcl::PointXY> > * vertexToPixels)
{
@@ -629,7 +640,8 @@ pcl::TextureMesh::Ptr createTextureMesh(
// create cameras
pcl::texture_mapping::CameraVector cameras = createTextureCameras(
poses,
cameraModels);
cameraModels,
roiRatios);
// Create materials for each texture (and one extra for occluded faces)
textureMesh->tex_materials.resize (cameras.size () + 1);