Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merge master into develop #6433

Merged
merged 2 commits into from
Nov 7, 2023
Merged

Merge master into develop #6433

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
868a862
Fix missing points in lidar scan (#6417)
TonyWelte Nov 6, 2023
527cf22
Merge branch 'develop' into sync-master-868a862b7
omichel Nov 7, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
199 changes: 146 additions & 53 deletions resources/wren/shaders/merge_spherical.frag
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading