Skip to content
Snippets Groups Projects
Commit 5679dac8 authored by Fabian Reister's avatar Fabian Reister
Browse files

Merge branch 'fix/image-provider-dynamic-simulation' into 'master' 

fix: ImageProviderDynamicSimulation fixing camera frame and min z clip

See merge request !52
parents ffedc880 fdd55ca6
No related branches found
No related tags found
1 merge request!52fix: ImageProviderDynamicSimulation fixing camera frame and min z clip
Pipeline #6109 failed
Stage: build
Stage: test
Hide whitespace changes
Inline Side-by-side
source/ArmarXSimulation/components/ImageProviderDynamicSimulation/ImageProviderDynamicSimulation.cpp
+ 98
− 33
View file @ 5679dac8
......@@ -26,6 +26,8 @@
#include <Ice/Properties.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/system/ArmarXDataPath.h>
// VisionXCore
......@@ -39,9 +41,11 @@
#include "ImageProviderDynamicSimulation.h"
#include <Calibration/Calibration.h>
#include <Inventor/SoInteraction.h>
#include <Inventor/nodes/SoDirectionalLight.h>
#include <Inventor/nodes/SoUnits.h>
#include <Math/Math3d.h>
namespace armarx
......@@ -51,38 +55,17 @@ namespace armarx
// init SoDB / Coin3D
//SoDB::init();
ARMARX_INFO << "1";
VirtualRobot::init(this->getName());
// needed for SoSelection
SoInteraction::init();
// image format
setImageFormat(visionx::ImageDimension(getProperty<visionx::ImageDimension>("ImageSize").getValue()), visionx::eRgb);
setImageSyncMode(visionx::eFpsSynchronization);
frameRate = getProperty<float>("FrameRate").getValue();
const visionx::ImageDimension dimensions = getProperty<visionx::ImageDimension>("ImageSize").getValue();
renderImg_bpp = 3;
renderImg_width = dimensions.width;
renderImg_height = dimensions.height;
setNumberImages(2);
images = new CByteImage*[getNumberImages()];
for (int i = 0; i < getNumberImages(); i++)
{
images[i] = new CByteImage(dimensions.width, dimensions.height, CByteImage::eRGB24);
}
resizedImages = new CByteImage*[getNumberImages()];
for (int i = 0 ; i < getNumberImages() ; i++)
{
resizedImages[i] = visionx::tools::createByteImage(getImageFormat(), getImageFormat().type);
}
std::string calibrationFileName = getProperty<std::string>("CalibrationFile").getValue();
ARMARX_VERBOSE << "Camera calibration file: " << calibrationFileName;
......@@ -96,24 +79,58 @@ namespace armarx
}
setlocale(LC_NUMERIC, "C");
CStereoCalibration ivtStereoCalibration;
if (!ivtStereoCalibration.LoadCameraParameters(fullCalibrationFileName.c_str(), true))
{
ARMARX_ERROR << "Error loading camera calibration file: " << fullCalibrationFileName;
}
stereoCalibration = visionx::tools::convert(ivtStereoCalibration);
renderImg_width = stereoCalibration->calibrationLeft.cameraParam.width;
renderImg_height = stereoCalibration->calibrationLeft.cameraParam.height;
ARMARX_CHECK_EQUAL(stereoCalibration->calibrationLeft.cameraParam.height, renderImg_height);
ARMARX_CHECK_EQUAL(stereoCalibration->calibrationRight.cameraParam.height, renderImg_height);
ARMARX_CHECK_EQUAL(stereoCalibration->calibrationLeft.cameraParam.width, renderImg_width);
ARMARX_CHECK_EQUAL(stereoCalibration->calibrationRight.cameraParam.width, renderImg_width);
}
else{
const visionx::ImageDimension dimensions = getProperty<visionx::ImageDimension>("ImageSize").getValue();
renderImg_width = dimensions.width;
renderImg_height = dimensions.height;
}
setImageFormat(visionx::ImageDimension{renderImg_width, renderImg_height}, visionx::eRgb);
renderImg_bpp = 3;
setNumberImages(2);
images = new CByteImage*[getNumberImages()];
for (int i = 0; i < getNumberImages(); i++)
{
images[i] = new CByteImage(renderImg_width, renderImg_height, CByteImage::eRGB24);
}
resizedImages = new CByteImage*[getNumberImages()];
for (int i = 0 ; i < getNumberImages() ; i++)
{
resizedImages[i] = visionx::tools::createByteImage(getImageFormat(), getImageFormat().type);
}
stereoCalibration = visionx::tools::convert(ivtStereoCalibration);
fovLeft = 2.0 * std::atan(renderImg_height / (2.0 * stereoCalibration.calibrationLeft.cameraParam.focalLength[1]));
fovRight = 2.0 * std::atan(renderImg_height / (2.0 * stereoCalibration.calibrationRight.cameraParam.focalLength[1]));
ARMARX_LOG << "fov left: " << (fovLeft / M_PI * 180.0) << " fov right: " << (fovRight / M_PI * 180.0);
std::stringstream svName;
svName << getName() << "_PhysicsWorldVisualization";
simVisu = Component::create<ArmarXPhysicsWorldVisualization>(getIceProperties(), svName.str());
getArmarXManager()->addObject(simVisu);
}
......@@ -156,6 +173,7 @@ namespace armarx
robotName = getProperty<std::string>("RobotName").getValue();
leftNodeName = getProperty<std::string>("RobotNodeLeftCamera").getValue();
rightNodeName = getProperty<std::string>("RobotNodeRightCamera").getValue();
focalLength = getProperty<float>("focalLength").getValue();
// ensure that all data is loaded
if (simVisu)
......@@ -242,12 +260,20 @@ namespace armarx
}
if(not stereoCalibration.has_value())
{
initStereoCalib();
}
float fovLeft = 2.0 * std::atan(renderImg_height / (2.0 * stereoCalibration->calibrationLeft.cameraParam.focalLength[1]));
float fovRight = 2.0 * std::atan(renderImg_height / (2.0 * stereoCalibration->calibrationRight.cameraParam.focalLength[1]));
// ARMARX_LOG << "fov left: " << (fovLeft / M_PI * 180.0) << " fov right: " << (fovRight / M_PI * 180.0);
// render Left Camera
#if 1
renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(rendererLeft.get(), cameraNodeL, simVisu->getVisualization(), &renderBuffer, 10.0f, 100000.0f, fovLeft);
renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(rendererLeft.get(), cameraNodeL, simVisu->getVisualization(), &renderBuffer, 100.0f, 100000.0f, fovLeft);
#else
//////////////// optional: we render by our own
SoPerspectiveCamera* cam = new SoPerspectiveCamera();
......@@ -298,7 +324,7 @@ namespace armarx
// render Right Camera
renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(rendererRight.get(), cameraNodeR, simVisu->getVisualization(), &renderBuffer, 10.0f, 100000.0f, fovRight);
renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(rendererRight.get(), cameraNodeR, simVisu->getVisualization(), &renderBuffer, 100.0f, 100000.0f, fovRight);
if (renderOK && renderBuffer != NULL)
{
......@@ -363,10 +389,51 @@ namespace armarx
}
void ImageProviderDynamicSimulation::initStereoCalib()
{
CCalibration leftCalibration;
leftCalibration.SetCameraParameters(focalLength,focalLength, renderImg_width / 2, renderImg_height / 2, 0,0,0,0, Math3d::unit_mat, Math3d::zero_vec, renderImg_width, renderImg_height);
ARMARX_CHECK_NOT_NULL(simVisu);
while (!simVisu->getRobot(robotName))
{
usleep(100000);
simVisu->synchronizeVisualizationData();
ARMARX_INFO << deactivateSpam(3) << "Waiting for visu";
}
ARMARX_CHECK(simVisu->synchronizeVisualizationData());
auto robot = simVisu->getRobot(robotName);
ARMARX_CHECK_NOT_NULL(robot);
cameraNodeL = robot->getRobotNode(leftNodeName);
cameraNodeR = robot->getRobotNode(rightNodeName);
ARMARX_CHECK_NOT_NULL(cameraNodeL);
ARMARX_CHECK_NOT_NULL(cameraNodeR);
const Eigen::Vector3f left_P_right = cameraNodeR->getPositionInFrame(cameraNodeL);
ARMARX_IMPORTANT << VAROUT(left_P_right);
CCalibration rightCalibration;
Vec3d transRight{-left_P_right.x(), 0, 0};
rightCalibration.SetCameraParameters(focalLength,focalLength, renderImg_width / 2, renderImg_height / 2, 0,0,0,0, Math3d::unit_mat, transRight, renderImg_width, renderImg_height);
CStereoCalibration ivtStereoCalibration;
ivtStereoCalibration.SetSingleCalibrations(leftCalibration, rightCalibration);
stereoCalibration = visionx::tools::convert(ivtStereoCalibration);
}
visionx::StereoCalibration ImageProviderDynamicSimulation::getStereoCalibration(const Ice::Current& c)
{
return stereoCalibration;
if(not stereoCalibration.has_value())
{
initStereoCalib();
}
return stereoCalibration.value();
}
bool ImageProviderDynamicSimulation::getImagesAreUndistorted(const Ice::Current& c)
......@@ -380,5 +447,3 @@ namespace armarx
}
}
source/ArmarXSimulation/components/ImageProviderDynamicSimulation/ImageProviderDynamicSimulation.h
+ 10
− 6
View file @ 5679dac8
......@@ -142,9 +142,13 @@ namespace armarx
*/
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
{
return armarx::PropertyDefinitionsPtr(
auto props = armarx::PropertyDefinitionsPtr(
new ImageSequenceProviderDynamicSimulationPropertyDefinitions(
getConfigIdentifier()));
props->defineOptionalProperty<float>("focalLength", 600, "The focal length.");
return props;
}
CByteImage** images;
CByteImage** resizedImages;
......@@ -157,7 +161,6 @@ namespace armarx
int renderImg_bpp;
int renderImg_width;
int renderImg_height;
float fovLeft, fovRight;
//RemoteRobotPtr remoteRobot;
......@@ -178,15 +181,16 @@ namespace armarx
VirtualRobot::RobotNodePtr cameraNodeL;
VirtualRobot::RobotNodePtr cameraNodeR;
CStereoCalibration ivtStereoCalibration;
visionx::StereoCalibration stereoCalibration;
float focalLength;
std::optional<visionx::StereoCalibration> stereoCalibration;
bool setupCameraRendering(const std::string& robotName, const std::string& cameraSensorNameLeft, const std::string& cameraSensorNameRight);
bool updateCameraRendering();
void initStereoCalib();
private:
};
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment