Skip to content

Commit

Permalink
Merge pull request #6433 from cyberbotics/sync-master-868a862b7
Browse files Browse the repository at this point in the history 
Merge master into develop
  • Loading branch information
@omichel
omichel authored Nov 7, 2023
2 parents f6edee8 + 527cf22 commit b5a1826
Show file tree
Hide file tree
Showing 3 changed files with 162 additions and 62 deletions.
199 changes: 146 additions & 53 deletions resources/wren/shaders/merge_spherical.frag
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ precision highp float;

const float FLT_MAX = intBitsToFloat(0x7F800000);

const vec3 orientations[6] = vec3[6](vec3(1.0, 0.0, 0.0), vec3(-1.0, 0.0, 0.0), vec3(0.0, 1.0, 0.0), vec3(0.0, -1.0, 0.0),
const vec3 orientations[6] = vec3[6](vec3(1.0, 0.0, 0.0), vec3(0.0, -1.0, 0.0), vec3(-1.0, 0.0, 0.0), vec3(0.0, 1.0, 0.0),
vec3(0.0, 0.0, 1.0), vec3(0.0, 0.0, -1.0));

in vec2 texUv;
Expand All @@ -34,7 +34,121 @@ uniform float fovYCorrectionCoefficient;

uniform sampler2D inputTextures[6];

// Same as texture but pick the side to used for the interpolation to avoid discontinuities
vec4 textureSmooth(sampler2D tex, vec2 p, int bias) {
ivec2 texSize = textureSize(tex, 0);
ivec2 pixelCoord = ivec2(round((2 * texSize.x * p.x - 1) / 2), round((2 * texSize.y * p.y - 1) / 2));

float dx = ((2 * texSize.x * p.x - 1) / 2 - pixelCoord.x);
float dy = ((2 * texSize.y * p.y - 1) / 2 - pixelCoord.y);

// Get depth for each pixel in a 3 by 3 grid
float dCenter = texelFetch(tex, pixelCoord, bias).x;
float dUp = texelFetch(tex, pixelCoord + ivec2(0, 1), bias).x;
float dDown = texelFetch(tex, pixelCoord + ivec2(0, -1), bias).x;
float dLeft = texelFetch(tex, pixelCoord + ivec2(-1, 0), bias).x;
float dRight = texelFetch(tex, pixelCoord + ivec2(1, 0), bias).x;
float dUpLeft = texelFetch(tex, pixelCoord + ivec2(-1, 1), bias).x;
float dDownRight = texelFetch(tex, pixelCoord + ivec2(1, -1), bias).x;
float dDownLeft = texelFetch(tex, pixelCoord + ivec2(-1, -1), bias).x;
float dUpRight = texelFetch(tex, pixelCoord + ivec2(1, 1), bias).x;

int xSide = 1, ySide = 1;
if (texSize.x != 1 && texSize.y != 1) {
// Compute variance for each corner
float costUpRight =
(dUp - dCenter) * (dUp - dCenter) + (dUpRight - dCenter) * (dUpRight - dCenter) + (dRight - dCenter) * (dRight - dCenter);
float costDownRight = (dDown - dCenter) * (dDown - dCenter) + (dDownRight - dCenter) * (dDownRight - dCenter) +
(dRight - dCenter) * (dRight - dCenter);
float costUpLeft =
(dUp - dCenter) * (dUp - dCenter) + (dUpLeft - dCenter) * (dUpLeft - dCenter) + (dLeft - dCenter) * (dLeft - dCenter);
float costDownLeft = (dDown - dCenter) * (dDown - dCenter) + (dDownLeft - dCenter) * (dDownLeft - dCenter) +
(dLeft - dCenter) * (dLeft - dCenter);

// Deal with image borders
if (pixelCoord.x == 0) {
costUpLeft = FLT_MAX;
costDownLeft = FLT_MAX;
}
if (pixelCoord.x + 1 > texSize.x - 1) {
costUpRight = FLT_MAX;
costDownRight = FLT_MAX;
}
if (pixelCoord.y == 0) {
costDownLeft = FLT_MAX;
costDownRight = FLT_MAX;
}
if (pixelCoord.y + 1 > texSize.y - 1) {
costUpLeft = FLT_MAX;
costUpRight = FLT_MAX;
}
// Find the corner with lowest variance
float costMin = costUpRight;
if (costDownRight < costMin) {
xSide = 1;
ySide = -1;
costMin = costDownRight;
}
if (costDownLeft < costMin) {
xSide = -1;
ySide = -1;
costMin = costDownLeft;
}
if (costUpLeft < costMin) {
xSide = -1;
ySide = 1;
}

vec4 c00 = texelFetch(tex, pixelCoord, bias);
vec4 c01 = texelFetch(tex, pixelCoord + ivec2(xSide, 0), bias);
vec4 c10 = texelFetch(tex, pixelCoord + ivec2(0, ySide), bias);
vec4 c11 = texelFetch(tex, pixelCoord + ivec2(xSide, ySide), bias);

if (isinf(c00.x) || isinf(c01.x) || isinf(c10.x) || isinf(c11.x))
return c00;
else {
vec4 c0 = c00 + (c01 - c00) * dx * xSide;
vec4 c1 = c10 + (c11 - c10) * dx * xSide;

vec4 c = c0 + (c1 - c0) * dy * ySide;

return c;
}
} else if (texSize.x == 1 && texSize.y == 1) { // No interpolation
return texelFetch(tex, ivec2(pixelCoord.x, pixelCoord.y), bias);
} else if (texSize.y == 1) { // Linear interpolation
// Pick side with smallest variance (accounting for edges)
if ((pixelCoord.x == texSize.x - 1) || ((pixelCoord.x != 0) && (abs(dRight - dCenter) > abs(dLeft - dCenter))))
xSide = -1;
else
xSide = 1;

vec4 c0 = texelFetch(tex, pixelCoord, bias);
vec4 c1 = texelFetch(tex, pixelCoord + ivec2(xSide, 0), bias);
if (isinf(c0.x) || isinf(c1.x))
return c0;
else
return c0 + (c1 - c0) * dx;
} else if (texSize.x == 1) { // Linear interpolation
// Pick side with smallest variance (accounting for edges)
if ((pixelCoord.y == texSize.y - 1) || ((pixelCoord.y != 0) && (abs(dUp - dCenter) > abs(dDown - dCenter))))
ySide = -1;
else
ySide = 1;

vec4 c0 = texelFetch(tex, pixelCoord, bias);
vec4 c1 = texelFetch(tex, pixelCoord + ivec2(0, ySide), bias);
if (isinf(c0.x) || isinf(c1.x))
return c0;
else
return c0 + (c1 - c0) * dy;
}
}

void main() {
ivec2 texSize = textureSize(inputTextures[FRONT], 0);
ivec2 pixelCoord = ivec2(round((texUv.x - 1 / texSize.x / 2) * texSize.x), round((texUv.y - 1 / texSize.y / 2) * texSize.y));

vec3 coord3d;

if (cylindrical) {
Expand All @@ -61,46 +175,30 @@ void main() {
coord3d = vec3(sx, sy, sz);
}

// normalize the 3d coordinate
vec3 coord3dAbs = abs(coord3d);
int maxIndex = 0;
float signedValue = coord3d.x;
float absMax = coord3dAbs.x;
if (coord3dAbs.y > absMax) {
maxIndex = 1;
signedValue = coord3d.y;
absMax = coord3dAbs.y;
}
if (coord3dAbs.z > absMax) {
maxIndex = 2;
signedValue = coord3d.z;
absMax = coord3dAbs.z;
// determine on which face the 3d coordinate belongs
float maxDot = dot(coord3d, orientations[0]);
int face = FRONT;
for (int i = 1; i < 6; ++i) {
float current_dot = dot(vec3(coord3d.xy, coord3d.z / tan(min(fovY, pi_2) / 2)), orientations[i]);
if (maxDot < current_dot) {
maxDot = current_dot;
face = i;
}
}

// scale the 3d coordinate to be on the cube
float absMax = 0.0;
if (face == FRONT || face == BACK)
absMax = abs(coord3d.x);
else if (face == LEFT || face == RIGHT)
absMax = abs(coord3d.y);
else if (face == UP || face == DOWN)
absMax = abs(coord3d.z / tan(min(fovY, pi_2) / 2));

vec3 normalizedCoord3d = coord3d;
if (absMax > 0.0)
normalizedCoord3d /= absMax;

// determine on which face the 3d coordinate hits the cube
bool isNegative = (signedValue < 0.0);
int face = FRONT;
if (maxIndex == 0) {
if (isNegative)
face = BACK;
else
face = FRONT;
} else if (maxIndex == 1) {
if (isNegative)
face = RIGHT;
else
face = LEFT;
} else if (maxIndex == 2) {
if (isNegative)
face = DOWN;
else
face = UP;
}

// retrieve the x-y coordinate relatively to the current face
// according to the 3D coordinate
vec2 coord = vec2(0.0);
Expand All @@ -121,39 +219,34 @@ void main() {
coord.y = normalizedCoord3d.z;
}

if (fovX < pi_2)
coord.x *= pi_2 / fovX;
if (fovY < pi_2)
coord.y *= pi_2 / fovY * fovYCorrectionCoefficient;
if (face != UP && face != DOWN) {
if (fovX < pi_2)
coord.x /= tan(fovX / 2);
if (fovY < pi_2)
coord.y /= tan(fovY / 2);
}

vec2 faceCoord = vec2(0.5 * (1.0 - coord.x), 0.5 * (1.0 - coord.y));
ivec2 imageIndex = ivec2(round(faceCoord.x * (subCamerasResolutionX - 1)), round(faceCoord.y * (subCamerasResolutionY - 1)));

fragColor = vec4(0.0, 0.0, 0.0, 1.0);
if (face == FRONT)
fragColor = texelFetch(inputTextures[0], imageIndex, 0);
fragColor = textureSmooth(inputTextures[0], faceCoord, 0);
else if (face == RIGHT)
fragColor = texelFetch(inputTextures[1], imageIndex, 0);
fragColor = textureSmooth(inputTextures[1], faceCoord, 0);
else if (face == BACK)
fragColor = texelFetch(inputTextures[2], imageIndex, 0);
fragColor = textureSmooth(inputTextures[2], faceCoord, 0);
else if (face == LEFT)
fragColor = texelFetch(inputTextures[3], imageIndex, 0);
fragColor = textureSmooth(inputTextures[3], faceCoord, 0);
else if (face == UP)
fragColor = texelFetch(inputTextures[4], imageIndex, 0);
fragColor = textureSmooth(inputTextures[4], faceCoord, 0);
else if (face == DOWN)
fragColor = texelFetch(inputTextures[5], imageIndex, 0);
fragColor = textureSmooth(inputTextures[5], faceCoord, 0);

// rectify the spherical transform
if (rangeCamera) {
float depth = fragColor.x;
if (depth < maxRange) {
float cosine = 0.0f;
for (int i = 0; i < 6; ++i) {
float cosineTmp = dot(normalizedCoord3d, orientations[i]);
cosineTmp = cosineTmp / length(normalizedCoord3d);
if (cosineTmp > cosine)
cosine = cosineTmp;
}
float cosine = dot(coord3d, orientations[face]);
depth = depth / cosine;
}
if (depth < minRange)
Expand Down
4 changes: 2 additions & 2 deletions src/webots/nodes/WbLidar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -424,10 +424,10 @@ void WbLidar::updatePointCloud(int minWidth, int maxWidth) {
const double cosPhi0 = cos(phi0);
const double sinPhi0 = sin(phi0);

const double dtheta = mIsActuallyRotating ? (-2 * M_PI / (double)resolution) : (-actualFieldOfView() / (w - 1.0));
const double dtheta = mIsActuallyRotating ? (-2 * M_PI / (double)resolution) : (-actualFieldOfView() / w);
const double cosdTheta = cos(dtheta);
const double sindTheta = sin(dtheta);
const double theta0 = mIsActuallyRotating ? (minWidth * dtheta) - M_PI : (actualFieldOfView() / 2 + minWidth * dtheta);
const double theta0 = mIsActuallyRotating ? (minWidth * dtheta) : (actualFieldOfView() / 2 + minWidth * dtheta + dtheta / 2);
const double cosTheta0 = cos(theta0);
const double sinTheta0 = sin(theta0);

Expand Down
21 changes: 14 additions & 7 deletions src/webots/wren/WbWrenCamera.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -194,10 +194,9 @@ void WbWrenCamera::setFieldOfView(float fov) {
init();
}

if (fov > M_PI_2) { // maximum X field of view of the sub-camera is pi / 2
aspectRatio = aspectRatio * (M_PI_2 / fov);
fov = M_PI_2;
}
fov = fov > M_PI_2 ? M_PI_2 : fov; // maximum X field of view of the sub-camera is pi / 2
aspectRatio = tan((mSphericalFieldOfViewX > M_PI_2 ? M_PI_2 : mSphericalFieldOfViewX) / 2) /
tan((mSphericalFieldOfViewY > M_PI_2 ? M_PI_2 : mSphericalFieldOfViewY) / 2);

fieldOfViewY = computeFieldOfViewY(fov, aspectRatio);
if (fieldOfViewY > M_PI_2) { // maximum Y field of view of the sub-camera is pi / 2
Expand Down Expand Up @@ -766,7 +765,7 @@ void WbWrenCamera::setupSphericalSubCameras() {

if (verticalCameraNumber == 1) {
// this coefficient is set to work even in the worse case (just before enabling top and bottom cameras)
mSphericalFovYCorrectionCoefficient = 1.27;
mSphericalFovYCorrectionCoefficient = 1.4;
mSphericalFieldOfViewY *= mSphericalFovYCorrectionCoefficient;
} else
mSphericalFovYCorrectionCoefficient = 1.0;
Expand Down Expand Up @@ -883,17 +882,25 @@ void WbWrenCamera::setCamerasOrientations() {
}

void WbWrenCamera::setFovy(float fov) {
for (int i = 0; i < CAMERA_ORIENTATION_COUNT; ++i) {
for (int i = 0; i < CAMERA_ORIENTATION_COUNT - 2; ++i) { // Skip the UP/DOWN camera
if (mIsCameraActive[i])
wr_camera_set_fovy(mCamera[i], fov);
}
if (mIsCameraActive[CAMERA_ORIENTATION_UP])
wr_camera_set_fovy(mCamera[CAMERA_ORIENTATION_UP], M_PI - fov);
if (mIsCameraActive[CAMERA_ORIENTATION_DOWN])
wr_camera_set_fovy(mCamera[CAMERA_ORIENTATION_DOWN], M_PI - fov);
}

void WbWrenCamera::setAspectRatio(float aspectRatio) {
for (int i = 0; i < CAMERA_ORIENTATION_COUNT; ++i) {
for (int i = 0; i < CAMERA_ORIENTATION_COUNT - 2; ++i) { // Skip the UP/DOWN camera
if (mIsCameraActive[i])
wr_camera_set_aspect_ratio(mCamera[i], aspectRatio);
}
if (mIsCameraActive[CAMERA_ORIENTATION_UP])
wr_camera_set_aspect_ratio(mCamera[CAMERA_ORIENTATION_UP], 1.0);
if (mIsCameraActive[CAMERA_ORIENTATION_DOWN])
wr_camera_set_aspect_ratio(mCamera[CAMERA_ORIENTATION_DOWN], 1.0);
}

void WbWrenCamera::updatePostProcessingParameters(int index) {
Expand Down

0 comments on commit b5a1826

Please sign in to comment.