27 #include <Ice/Properties.h>
44 #include <Calibration/Calibration.h>
45 #include <Inventor/SoInteraction.h>
46 #include <Inventor/nodes/SoDirectionalLight.h>
47 #include <Inventor/nodes/SoUnits.h>
48 #include <Math/Math3d.h>
60 VirtualRobot::init(this->
getName());
63 SoInteraction::init();
67 frameRate = getProperty<float>(
"FrameRate").getValue();
69 std::string calibrationFileName = getProperty<std::string>(
"CalibrationFile").getValue();
70 ARMARX_VERBOSE <<
"Camera calibration file: " << calibrationFileName;
72 if (!calibrationFileName.empty())
74 std::string fullCalibrationFileName;
78 ARMARX_ERROR <<
"Could not find camera calibration file in ArmarXDataPath: " << calibrationFileName;
81 setlocale(LC_NUMERIC,
"C");
83 CStereoCalibration ivtStereoCalibration;
85 if (!ivtStereoCalibration.LoadCameraParameters(fullCalibrationFileName.c_str(),
true))
87 ARMARX_ERROR <<
"Error loading camera calibration file: " << fullCalibrationFileName;
100 const visionx::ImageDimension dimensions = getProperty<visionx::ImageDimension>(
"ImageSize").getValue();
130 std::stringstream svName;
131 svName <<
getName() <<
"_PhysicsWorldVisualization";
173 robotName = getProperty<std::string>(
"RobotName").getValue();
174 leftNodeName = getProperty<std::string>(
"RobotNodeLeftCamera").getValue();
175 rightNodeName = getProperty<std::string>(
"RobotNodeRightCamera").getValue();
176 focalLength = getProperty<float>(
"focalLength").getValue();
181 simVisu->synchronizeVisualizationData();
186 simVisu->synchronizeVisualizationData();
197 auto l =
simVisu->getScopedLock();
212 static void copyRenderBufferToByteImage(CByteImage* image, std::uint8_t* renderBuffer)
214 int height = image->height;
215 int width = image->width;
216 std::uint8_t* pixelsRow = image->pixels;
217 std::uint8_t* renderBufferEndOfRow = renderBuffer + 3 * (width - 1);
218 for (
int y = 0; y < height; y++)
220 for (
int x = 0; x < width; x++)
222 pixelsRow[x * 3 + 0] = renderBufferEndOfRow[-x * 3 + 0];
223 pixelsRow[x * 3 + 1] = renderBufferEndOfRow[-x * 3 + 1];
224 pixelsRow[x * 3 + 2] = renderBufferEndOfRow[-x * 3 + 2];
226 pixelsRow += width * 3;
227 renderBufferEndOfRow += width * 3;
234 auto l =
simVisu->getScopedLock();
243 unsigned char* renderBuffer = NULL;
244 bool renderOK =
false;
276 renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(
rendererLeft.get(),
cameraNodeL,
simVisu->getVisualization(), &renderBuffer, 100.0f, 100000.0f, fovLeft);
279 SoPerspectiveCamera* cam =
new SoPerspectiveCamera();
283 Eigen::Vector3f camPos = MathTools::getTranslation(camPose);
285 cam->position.setValue(camPos[0]*sc, camPos[1]*sc, camPos[2]*sc);
287 SbRotation
align(SbVec3f(1, 0, 0), (
float)(
M_PI));
288 SbRotation align2(SbVec3f(0, 0, 1), (
float)(-
M_PI / 2.0));
289 SbRotation trans(CoinVisualizationFactory::getSbMatrix(camPose));
290 cam->orientation.setValue(align2 *
align * trans);
293 cam->nearDistance.setValue(0.01f);
294 cam->farDistance.setValue(10000.0f);
297 SoSeparator* root =
new SoSeparator();
299 SoDirectionalLight* light =
new SoDirectionalLight;
300 root->addChild(light);
308 root->addChild(
simVisu->getVisualization());
310 renderOK =
rendererLeft.get()->render(root) == TRUE ? true :
false;
320 if (renderOK && renderBuffer != NULL)
322 copyRenderBufferToByteImage(
images[0], renderBuffer);
327 renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(
rendererRight.get(),
cameraNodeR,
simVisu->getVisualization(), &renderBuffer, 100.0f, 100000.0f, fovRight);
329 if (renderOK && renderBuffer != NULL)
331 copyRenderBufferToByteImage(
images[1], renderBuffer);
344 bool succeeded =
true;
348 simVisu->synchronizeVisualizationData();
367 ARMARX_WARNING <<
"Shared memory provider is null (possibly shutting down)";
376 memcpy(ppImageBuffers[i],
396 CCalibration leftCalibration;
397 leftCalibration.SetCameraParameters(
focalLength,
focalLength,
renderImg_width / 2,
renderImg_height / 2, 0,0,0,0, Math3d::unit_mat, Math3d::zero_vec,
renderImg_width,
renderImg_height);
403 simVisu->synchronizeVisualizationData();
420 CCalibration rightCalibration;
421 Vec3d transRight{-left_P_right.x(), 0, 0};
422 rightCalibration.SetCameraParameters(
focalLength,
focalLength,
renderImg_width / 2,
renderImg_height / 2, 0,0,0,0, Math3d::unit_mat, transRight,
renderImg_width,
renderImg_height);
424 CStereoCalibration ivtStereoCalibration;
425 ivtStereoCalibration.SetSingleCalibrations(leftCalibration, rightCalibration);
446 return getProperty<std::string>(
"ReferenceFrame").getValue();