diff --git a/source/ArmarXSimulation/components/DepthImageProviderDynamicSimulation/DepthImageProviderDynamicSimulation.cpp b/source/ArmarXSimulation/components/DepthImageProviderDynamicSimulation/DepthImageProviderDynamicSimulation.cpp
index 3f5c8b5668bd0631453bf08dff3227b20d162db1..1c379bb9b53f6bacf5ac4d8d67fba7d091259e21 100644
--- a/source/ArmarXSimulation/components/DepthImageProviderDynamicSimulation/DepthImageProviderDynamicSimulation.cpp
+++ b/source/ArmarXSimulation/components/DepthImageProviderDynamicSimulation/DepthImageProviderDynamicSimulation.cpp
@@ -29,13 +29,107 @@
#include <pcl/common/transforms.h>
#include <pcl/compression/organized_pointcloud_conversion.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <VisionX/tools/ImageUtil.h>
+#include <VisionX/tools/TypeMapping.h>
+
#include <Inventor/SoInteraction.h>
#include <Inventor/actions/SoGLRenderAction.h>
namespace armarx
{
+
+ DepthImageProviderDynamicSimulationPropertyDefinitions::
+ DepthImageProviderDynamicSimulationPropertyDefinitions(std::string prefix) :
+ visionx::CapturingPointCloudProviderPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<bool>(
+ "FloatImageMode", false, "Whether to provide a CFloatImage or the standard CByteImage");
+ defineOptionalProperty<float>("Noise",
+ 0.0f,
+ "Noise of the point cloud position results as standard "
+ "deviation of the normal distribution (in mm)")
+ .setMin(0);
+ defineOptionalProperty<float>("DistanceZNear",
+ 20.0f,
+ "Distance of the near clipping plain. (If set too small "
+ "the agent's model's inside may be visible")
+ .setMin(1e-8);
+ defineOptionalProperty<float>(
+ "DistanceZFar",
+ 5000.0f,
+ "Distance of the far clipping plain. (DistanceZFar-DistanceZNear should be "
+ "minimal, DistanceZFar > DistanceZNear)")
+ .setMin(1e-8);
+ defineOptionalProperty<float>("FOV", M_PI / 4, "Vertical FOV in rad.");
+ defineOptionalProperty<float>(
+ "BaseLine", 0.075, "The value returned from getBaseline(). It has no other effect.");
+ defineOptionalProperty<float>("NanValue",
+ std::nan(""),
+ "Value of points that are farther away then "
+ "DistanceZFar. Most cameras return here NaN.");
+
+ defineOptionalProperty<visionx::ImageDimension>(
+ "ImageSize",
+ visionx::ImageDimension(640, 480),
+ "Target resolution of the images. Captured images will be converted to this size.")
+ .setCaseInsensitive(true)
+ .map("200x200", visionx::ImageDimension(200, 200))
+ .map("320x240", visionx::ImageDimension(320, 240))
+ .map("640x480", visionx::ImageDimension(640, 480))
+ .map("800x600", visionx::ImageDimension(800, 600))
+ .map("768x576", visionx::ImageDimension(768, 576))
+ .map("1024x768", visionx::ImageDimension(1024, 768))
+ .map("1280x960", visionx::ImageDimension(1280, 960))
+ .map("1600x1200", visionx::ImageDimension(1600, 1200))
+ .map("none", visionx::ImageDimension(0, 0));
+ defineOptionalProperty<std::string>("RobotName", "Armar3", "The robot");
+ defineOptionalProperty<std::string>(
+ "RobotNodeCamera", "DepthCameraSim", "The coordinate system of the used camera");
+
+ //used to draw the cloud to the simulator
+ defineOptionalProperty<bool>(
+ "DrawPointCloud",
+ false,
+ "Whether the point cloud is drawn to the given DebugDrawerTopic");
+ defineOptionalProperty<std::string>("DrawPointCloud_DebugDrawerTopic",
+ "DebugDrawerUpdates",
+ "Name of the DebugDrawerTopic");
+ defineOptionalProperty<unsigned int>(
+ "DrawPointCloud_DrawDelay",
+ 1000,
+ "The time between updates of the drawn point cloud (in ms)");
+ defineOptionalProperty<float>("DrawPointCloud_PointSize", 4, "The size of a point.");
+ defineOptionalProperty<std::size_t>(
+ "DrawPointCloud_PointSkip",
+ 3,
+ "Only draw every n'th point in x and y direction (n=DrawPointCloud_PointSkip). "
+ "Increase this whenever the ice buffer size is to small to transmitt the cloud "
+ "size. (>0)");
+
+ defineOptionalProperty<bool>(
+ "DrawPointCloud_ClipPoints",
+ true,
+ "Whether to clip the point cloud drawn to the given DebugDrawerTopic");
+
+ defineOptionalProperty<float>(
+ "DrawPointCloud_ClipXHi", 25000, "Skip points with x higher than this limit.");
+ defineOptionalProperty<float>(
+ "DrawPointCloud_ClipYHi", 25000, "Skip points with y higher than this limit.");
+ defineOptionalProperty<float>(
+ "DrawPointCloud_ClipZHi", 25000, "Skip points with z higher than this limit.");
+
+ defineOptionalProperty<float>(
+ "DrawPointCloud_ClipXLo", -25000, "Skip points with x lower than this limit.");
+ defineOptionalProperty<float>(
+ "DrawPointCloud_ClipYLo", -25000, "Skip points with y lower than this limit.");
+ defineOptionalProperty<float>(
+ "DrawPointCloud_ClipZLo", -25000, "Skip points with z lower than this limit.");
+ }
+
void
DepthImageProviderDynamicSimulation::onInitComponent()
{
@@ -146,6 +240,91 @@ namespace armarx
ARMARX_INFO << "done DepthImageProviderDynamicSimulation::onStartCapture";
}
+ void
+ DepthImageProviderDynamicSimulation::onStopCapture()
+ {
+ }
+
+ DepthImageProviderDynamicSimulation::~DepthImageProviderDynamicSimulation()
+ {
+ }
+
+ std::string
+ DepthImageProviderDynamicSimulation::getDefaultName() const
+ {
+ return "DynamicSimulationDepthImageProvider";
+ }
+
+ bool
+ DepthImageProviderDynamicSimulation::hasSharedMemorySupport(const Ice::Current& c)
+ {
+ return true;
+ }
+
+ visionx::StereoCalibration
+ DepthImageProviderDynamicSimulation::getStereoCalibration(const Ice::Current& c)
+ {
+ visionx::StereoCalibration stereoCalibration;
+
+
+ float r = static_cast<float>(width) / static_cast<float>(height);
+
+ visionx::CameraParameters RGBCameraIntrinsics;
+ RGBCameraIntrinsics.distortion = {0, 0, 0, 0};
+ RGBCameraIntrinsics.focalLength = {
+ static_cast<float>(width) / (2 * std::tan((vertFov * r) / 2)),
+ static_cast<float>(height) / (2 * std::tan(vertFov / 2))};
+ RGBCameraIntrinsics.height = height;
+ RGBCameraIntrinsics.principalPoint = {width / 2.0f, height / 2.0f};
+ RGBCameraIntrinsics.rotation =
+ visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
+ RGBCameraIntrinsics.translation =
+ visionx::tools::convertEigenVecToVisionX(Eigen::Vector3f::Zero());
+ RGBCameraIntrinsics.width = width;
+
+ visionx::CameraParameters DepthCameraIntrinsics;
+ DepthCameraIntrinsics.distortion = {0, 0, 0, 0};
+ DepthCameraIntrinsics.focalLength = {
+ static_cast<float>(width) / (2 * std::tan((vertFov * r) / 2)),
+ static_cast<float>(height) / (2 * std::tan(vertFov / 2))};
+ DepthCameraIntrinsics.height = height;
+ DepthCameraIntrinsics.principalPoint = {width / 2.0f, height / 2.0f};
+ DepthCameraIntrinsics.rotation =
+ visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
+ DepthCameraIntrinsics.translation = {0.075, 0, 0};
+ DepthCameraIntrinsics.width = width;
+
+
+ stereoCalibration.calibrationLeft = visionx::tools::createDefaultMonocularCalibration();
+ stereoCalibration.calibrationRight = visionx::tools::createDefaultMonocularCalibration();
+ stereoCalibration.calibrationLeft.cameraParam = RGBCameraIntrinsics;
+ stereoCalibration.calibrationRight.cameraParam = DepthCameraIntrinsics;
+ stereoCalibration.rectificationHomographyLeft =
+ visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
+ stereoCalibration.rectificationHomographyRight =
+ visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
+
+
+ return stereoCalibration;
+ }
+
+ bool
+ DepthImageProviderDynamicSimulation::getImagesAreUndistorted(const Ice::Current& c)
+ {
+ return true;
+ }
+
+ std::string
+ DepthImageProviderDynamicSimulation::getReferenceFrame(const Ice::Current& c)
+ {
+ return getProperty<std::string>("RobotNodeCamera");
+ }
+
+ void
+ DepthImageProviderDynamicSimulation::onInitCapturingPointCloudProvider()
+ {
+ }
+
void
DepthImageProviderDynamicSimulation::onExitCapturingPointCloudProvider()
{
@@ -239,6 +418,17 @@ namespace armarx
CapturingPointCloudProvider::onDisconnectComponent();
}
+ void
+ DepthImageProviderDynamicSimulation::onExitImageProvider()
+ {
+ }
+
+ PropertyDefinitionsPtr
+ DepthImageProviderDynamicSimulation::createPropertyDefinitions()
+ {
+ return new DepthImageProviderDynamicSimulationPropertyDefinitions(getConfigIdentifier());
+ }
+
bool
DepthImageProviderDynamicSimulation::doCapture()
{
@@ -535,4 +725,5 @@ namespace armarx
}
}
}
+
} // namespace armarx
diff --git a/source/ArmarXSimulation/components/DepthImageProviderDynamicSimulation/DepthImageProviderDynamicSimulation.h b/source/ArmarXSimulation/components/DepthImageProviderDynamicSimulation/DepthImageProviderDynamicSimulation.h
index 3dea71c88c7998fd00ddd37e3270ed48a38e3b7b..275a13ab5143719a8227b9f5b37659d6e18820a8 100644
--- a/source/ArmarXSimulation/components/DepthImageProviderDynamicSimulation/DepthImageProviderDynamicSimulation.h
+++ b/source/ArmarXSimulation/components/DepthImageProviderDynamicSimulation/DepthImageProviderDynamicSimulation.h
@@ -30,19 +30,18 @@
#include <random>
#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <ArmarXCore/core/Component.h>
#include <VisionX/components/pointcloud_core/CapturingPointCloudProvider.h>
#include <VisionX/core/ImageProvider.h>
#include <VisionX/interface/components/RGBDImageProvider.h>
-#include <VisionX/tools/ImageUtil.h>
-#include <VisionX/tools/TypeMapping.h>
#include <ArmarXSimulation/components/ArmarXPhysicsWorldVisualization/ArmarXPhysicsWorldVisualization.h>
-#include <MemoryX/core/entity/ProbabilityMeasures.h>
+#include <Inventor/SoOffscreenRenderer.h>
+
+class SoOffscreenRenderer;
namespace armarx
{
@@ -50,96 +49,7 @@ namespace armarx
public visionx::CapturingPointCloudProviderPropertyDefinitions
{
public:
- DepthImageProviderDynamicSimulationPropertyDefinitions(std::string prefix) :
- visionx::CapturingPointCloudProviderPropertyDefinitions(prefix)
- {
- defineOptionalProperty<bool>(
- "FloatImageMode",
- false,
- "Whether to provide a CFloatImage or the standard CByteImage");
- defineOptionalProperty<float>("Noise",
- 0.0f,
- "Noise of the point cloud position results as standard "
- "deviation of the normal distribution (in mm)")
- .setMin(0);
- defineOptionalProperty<float>("DistanceZNear",
- 20.0f,
- "Distance of the near clipping plain. (If set too small "
- "the agent's model's inside may be visible")
- .setMin(1e-8);
- defineOptionalProperty<float>(
- "DistanceZFar",
- 5000.0f,
- "Distance of the far clipping plain. (DistanceZFar-DistanceZNear should be "
- "minimal, DistanceZFar > DistanceZNear)")
- .setMin(1e-8);
- defineOptionalProperty<float>("FOV", M_PI / 4, "Vertical FOV in rad.");
- defineOptionalProperty<float>(
- "BaseLine",
- 0.075,
- "The value returned from getBaseline(). It has no other effect.");
- defineOptionalProperty<float>("NanValue",
- std::nan(""),
- "Value of points that are farther away then "
- "DistanceZFar. Most cameras return here NaN.");
-
- defineOptionalProperty<visionx::ImageDimension>(
- "ImageSize",
- visionx::ImageDimension(640, 480),
- "Target resolution of the images. Captured images will be converted to this size.")
- .setCaseInsensitive(true)
- .map("200x200", visionx::ImageDimension(200, 200))
- .map("320x240", visionx::ImageDimension(320, 240))
- .map("640x480", visionx::ImageDimension(640, 480))
- .map("800x600", visionx::ImageDimension(800, 600))
- .map("768x576", visionx::ImageDimension(768, 576))
- .map("1024x768", visionx::ImageDimension(1024, 768))
- .map("1280x960", visionx::ImageDimension(1280, 960))
- .map("1600x1200", visionx::ImageDimension(1600, 1200))
- .map("none", visionx::ImageDimension(0, 0));
- defineOptionalProperty<std::string>("RobotName", "Armar3", "The robot");
- defineOptionalProperty<std::string>(
- "RobotNodeCamera", "DepthCameraSim", "The coordinate system of the used camera");
-
- //used to draw the cloud to the simulator
- defineOptionalProperty<bool>(
- "DrawPointCloud",
- false,
- "Whether the point cloud is drawn to the given DebugDrawerTopic");
- defineOptionalProperty<std::string>("DrawPointCloud_DebugDrawerTopic",
- "DebugDrawerUpdates",
- "Name of the DebugDrawerTopic");
- defineOptionalProperty<unsigned int>(
- "DrawPointCloud_DrawDelay",
- 1000,
- "The time between updates of the drawn point cloud (in ms)");
- defineOptionalProperty<float>("DrawPointCloud_PointSize", 4, "The size of a point.");
- defineOptionalProperty<std::size_t>(
- "DrawPointCloud_PointSkip",
- 3,
- "Only draw every n'th point in x and y direction (n=DrawPointCloud_PointSkip). "
- "Increase this whenever the ice buffer size is to small to transmitt the cloud "
- "size. (>0)");
-
- defineOptionalProperty<bool>(
- "DrawPointCloud_ClipPoints",
- true,
- "Whether to clip the point cloud drawn to the given DebugDrawerTopic");
-
- defineOptionalProperty<float>(
- "DrawPointCloud_ClipXHi", 25000, "Skip points with x higher than this limit.");
- defineOptionalProperty<float>(
- "DrawPointCloud_ClipYHi", 25000, "Skip points with y higher than this limit.");
- defineOptionalProperty<float>(
- "DrawPointCloud_ClipZHi", 25000, "Skip points with z higher than this limit.");
-
- defineOptionalProperty<float>(
- "DrawPointCloud_ClipXLo", -25000, "Skip points with x lower than this limit.");
- defineOptionalProperty<float>(
- "DrawPointCloud_ClipYLo", -25000, "Skip points with y lower than this limit.");
- defineOptionalProperty<float>(
- "DrawPointCloud_ClipZLo", -25000, "Skip points with z lower than this limit.");
- }
+ DepthImageProviderDynamicSimulationPropertyDefinitions(std::string prefix);
};
class DepthImageProviderDynamicSimulation :
@@ -149,139 +59,33 @@ namespace armarx
{
public:
DepthImageProviderDynamicSimulation() = default;
+ ~DepthImageProviderDynamicSimulation() override;
- ~DepthImageProviderDynamicSimulation() override
- {
- }
-
- std::string
- getDefaultName() const override
- {
- return "DynamicSimulationDepthImageProvider";
- }
+ std::string getDefaultName() const override;
- bool
- hasSharedMemorySupport(const Ice::Current& c = Ice::emptyCurrent) override
- {
- return true;
- }
+ bool hasSharedMemorySupport(const Ice::Current& c = Ice::emptyCurrent) override;
visionx::StereoCalibration
- getStereoCalibration(const Ice::Current& c = Ice::emptyCurrent) override
- {
- visionx::StereoCalibration stereoCalibration;
-
-
- float r = static_cast<float>(width) / static_cast<float>(height);
-
- visionx::CameraParameters RGBCameraIntrinsics;
- RGBCameraIntrinsics.distortion = {0, 0, 0, 0};
- RGBCameraIntrinsics.focalLength = {
- static_cast<float>(width) / (2 * std::tan((vertFov * r) / 2)),
- static_cast<float>(height) / (2 * std::tan(vertFov / 2))};
- RGBCameraIntrinsics.height = height;
- RGBCameraIntrinsics.principalPoint = {width / 2.0f, height / 2.0f};
- RGBCameraIntrinsics.rotation =
- visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
- RGBCameraIntrinsics.translation =
- visionx::tools::convertEigenVecToVisionX(Eigen::Vector3f::Zero());
- RGBCameraIntrinsics.width = width;
-
- visionx::CameraParameters DepthCameraIntrinsics;
- DepthCameraIntrinsics.distortion = {0, 0, 0, 0};
- DepthCameraIntrinsics.focalLength = {
- static_cast<float>(width) / (2 * std::tan((vertFov * r) / 2)),
- static_cast<float>(height) / (2 * std::tan(vertFov / 2))};
- DepthCameraIntrinsics.height = height;
- DepthCameraIntrinsics.principalPoint = {width / 2.0f, height / 2.0f};
- DepthCameraIntrinsics.rotation =
- visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
- DepthCameraIntrinsics.translation = {0.075, 0, 0};
- DepthCameraIntrinsics.width = width;
-
-
- stereoCalibration.calibrationLeft = visionx::tools::createDefaultMonocularCalibration();
- stereoCalibration.calibrationRight =
- visionx::tools::createDefaultMonocularCalibration();
- stereoCalibration.calibrationLeft.cameraParam = RGBCameraIntrinsics;
- stereoCalibration.calibrationRight.cameraParam = DepthCameraIntrinsics;
- stereoCalibration.rectificationHomographyLeft =
- visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
- stereoCalibration.rectificationHomographyRight =
- visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
-
-
- return stereoCalibration;
- }
-
- bool
- getImagesAreUndistorted(const Ice::Current& c = Ice::emptyCurrent) override
- {
- return true;
- }
-
- std::string
- getReferenceFrame(const Ice::Current& c = Ice::emptyCurrent) override
- {
- return getProperty<std::string>("RobotNodeCamera");
- }
+ getStereoCalibration(const Ice::Current& c = Ice::emptyCurrent) override;
+
+ bool getImagesAreUndistorted(const Ice::Current& c = Ice::emptyCurrent) override;
+
+ std::string getReferenceFrame(const Ice::Current& c = Ice::emptyCurrent) override;
protected:
- /**
- * @see visionx::PointCloudProviderBase::onInitPointCloudProvider()
- */
- void
- onInitCapturingPointCloudProvider() override
- {
- }
-
- /**
- * @see visionx::PointCloudProviderBase::onExitPointCloudProvider()
- */
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitCapturingPointCloudProvider() override;
void onExitCapturingPointCloudProvider() override;
- /**
- * @see visionx::PointCloudProviderBase::onStartCapture()
- */
void onStartCapture(float frameRate) override;
-
- /**
- * @see visionx::PointCloudProviderBase::onStopCapture()
- */
- void
- onStopCapture() override
- {
- }
-
- /**
- * @see visionx::PointCloudProviderBase::doCapture()
- */
+ void onStopCapture() override;
bool doCapture() override;
- /**
- * @see visionx::CapturingImageProvider::onInitImageProvider()
- */
void onInitImageProvider() override;
-
void onDisconnectComponent() override;
- /**
- * @see visionx::CapturingImageProvider::onExitImageProvider()
- */
- void
- onExitImageProvider() override
- {
- }
-
- /**
- * @see PropertyUser::createPropertyDefinitions()
- */
- armarx::PropertyDefinitionsPtr
- createPropertyDefinitions() override
- {
- return new DepthImageProviderDynamicSimulationPropertyDefinitions(
- getConfigIdentifier());
- }
+ void onExitImageProvider() override;
// mixed inherited stuff
void onInitComponent() override;
@@ -289,7 +93,6 @@ namespace armarx
void onExitComponent() override;
bool render();
-
void syncVisu();
std::mutex captureMutex;