Implement configurable frames for virtual objects #109

erblinium · 2023-02-22T16:21:01Z

No description provided.

benthie

thank you very much for your effort.

Is there any chance you could show me the contents of an objectFile together with the corresponding linkTree (the same ways you did it in your last PR)? I am trying to get my head around the involved transformations and their frames ;)

Kind Regards
Benny

benthie · 2023-02-23T10:05:30Z

ensenso_camera/src/virtual_object_handler.cpp

@@ -41,7 +41,7 @@ struct VirtualObjectMarkerPublisher
  VirtualObjectMarkerPublisher(ensenso::ros::NodeHandle _nh, const std::string& topic, double publishRate,
                               const NxLibItem& objects, const std::string& frame, std::atomic_bool& stop_)


frame isn't used anymore

benthie · 2023-02-23T10:20:06Z

ensenso_camera/include/ensenso_camera/virtual_object_handler.h

@@ -27,6 +27,7 @@ class VirtualObjectHandler

  /// Original object transforms in the base frame
  std::vector<tf2::Transform> originalTransforms;
+  std::vector<std::string> objectFrames;


Are the object frames going to change during operation? I guess not, and if so, I think we do not need to store them but can read them from the NxLib tree, see the comments in the .cpp file.

benthie · 2023-02-23T10:22:07Z

ensenso_camera/src/virtual_object_handler.cpp

@@ -171,6 +171,7 @@ VirtualObjectHandler::VirtualObjectHandler(ensenso::ros::NodeHandle& nh, const s
  for (int i = 0; i < objects.count(); ++i)
  {
    originalTransforms.push_back(transformFromNxLib(objects[i][itmLink]));
+    objectFrames.push_back(objects[i][itmLink][itmTarget].asString());


If we do not need to store the objects frames, this line can be removed.

benthie · 2023-02-23T10:25:25Z

ensenso_camera/src/virtual_object_handler.cpp

@@ -201,24 +202,26 @@ void VirtualObjectHandler::updateObjectLinks()
  {
    return;
  }
-
-  // Find transform from the frame in which the objects were defined to the current optical frame
+


Suggested change

void VirtualObjectHandler::updateObjectLinks()

{

// Exit early if we have no work to do

if (originalTransforms.empty())

{

return;

}

auto objects = NxLibItem[itmObjects];

// Apply the transform to all of the original transforms

for (int i = 0; i < objects.count(); ++i)

{

auto objectLink = objects[i][itmLink];

try

{

// Find the transformation from the object frame to the current optical frame

auto objectFrame = objectLink[itmTarget].asString();

auto cameraTransform = fromMsg(tfBuffer.lookupTransform(linkFrame, objectFrame, ensenso::ros::Time(0)).transform);

// Transform object back to original frame

tf2::Transform objectTransform = cameraTransform * originalTransforms[i];

writeTransformToNxLib(objectTransform.inverse(), objectLink);

}

catch (const tf2::TransformException& e)

{

ENSENSO_WARN("Could not look up the virtual object pose due to the TF error: %s", e.what());

return;

}

}

}

benthie · 2023-02-23T10:31:22Z

ensenso_camera/src/virtual_object_handler.cpp

@@ -171,6 +171,7 @@ VirtualObjectHandler::VirtualObjectHandler(ensenso::ros::NodeHandle& nh, const s
  for (int i = 0; i < objects.count(); ++i)
  {
    originalTransforms.push_back(transformFromNxLib(objects[i][itmLink]));
+    objectFrames.push_back(objects[i][itmLink][itmTarget].asString());


Lines 166 and 167 could be changed to the following in order to make the interaction with the tree more clear:

// Read the file content and write it to the objects node in the tree auto objects = NxLibItem[itmObjects];

Since the objects are now written to the tree, we can simply read them from the tree whenever we need and do not have to pass the objects around. Both objects and objectsFrame then do not have to be passed to the VirtualObjectMarkerPublisher constructor.

benthie · 2023-02-23T10:36:41Z

ensenso_camera/src/virtual_object_handler.cpp

@@ -41,7 +41,7 @@ struct VirtualObjectMarkerPublisher
  VirtualObjectMarkerPublisher(ensenso::ros::NodeHandle _nh, const std::string& topic, double publishRate,
                               const NxLibItem& objects, const std::string& frame, std::atomic_bool& stop_)


As mentioned below, objects are not needed to be passed in but can be read from the tree:
auto objects = NxLibItem[itmObjects];

Getting the following error when using the suggested updateObjectLinks() function:
src/ros_driver/ensenso_camera2/src/virtual_object_handler.cpp:205:27: error: expected primary-expression before ‘[’ token 205 | auto objects = NxLibItem[itmObjects];

Sorry, that must be NxLibItem()[itmObjects];

Got the following runtime error now after using NxLibItem()[itmObjects];:
NxLibException 13 (ItemTypeNotCompatible) for item /Objects/\0/Link/Target

If it's okay for you, push your temporary changes and then I can have a look ;)

Which version of Ensenso SDK do you use?
I am using 3.3.1417

I just tried it with both 3.3.1417 and 3.4.743, both with the same results.

In line 168 you write the objects to the NxLib tree. From there on they are in the tree. Getting the target name in line 213 should work, unless you are modifying the tree from somewhere else? Are you perhaps adding or removing objects in the meantime? The error could e.g. occur if you added another object meanwhile, which has no target node.

In case you want to be able to add and remove objects during operation, storing the object frames as you initially did would be required.

Yes I do add other objects in the meantime. I have two ensenso_camera_node nodes each with their own virtual objects file. They are both started from the same launch file.

Okay I see. Then there is the problem.

Since you guys are currently the only one developing (and maybe even using) this feature, it is quite hard for me - or us in general - to review the code. Do you think you could maybe write a test case that also acts as kind of a documentation? Then I can get an idea of how your code is supposed to work.

erblinium · 2023-02-24T07:56:41Z

Hi @erblinium,

thank you very much for your effort.

Is there any chance you could show me the contents of an objectFile together with the corresponding linkTree (the same ways you did it in your last PR)? I am trying to get my head around the involved transformations and their frames ;)

Kind Regards Benny

Hi @benthie,

I added the NxTree as attachment (I could not record while running my application, but I recorded when I opened the camera and objects in NxView).
Here is part of of an object json file:

[
	{
    "Fixed": true,
    "Lighting": {
      "Ambient": 100.0,
      "Color": [
        0,
        0,
        0
      ],
      "Diffuse": 1.0,
      "MaterialBlur": 0,
      "Shininess": 100,
      "Specular": 0.0
    },
    "Link": {
      "Inverse": true,
      "Rotation": {
        "Angle": 0,
        "Axis": [
          1,
          0,
          0
        ]
      },
      "Target": "aruco",
      "Translation": [
        -250.0,
        250.0,
        0.0
      ]
    },
    "Mass": 1,
    "Type": "Cuboid",
    "Width": 100.0,
    "Height": 20.0,
    "Depth": 100.0
  }
]

nxTree_tmp.nxlog.txt

benthie · 2023-02-24T08:37:55Z

I was actually hoping you could provide me the tf link tree as you did last time, I think you ran rosrun tf view_frames back then. You can directly post the pdf file here.

The NxLog file is not that good to read, could you maybe post the complete tree by either

graphically: Open NxTreeEdit when your camera is open and then right-click on the root \ element at the top and "Copy value as JSON string" or
programmatically: getting the tree from within your code with auto root = NxLibItem().asJson(true); and then printing the string?

If your objects file does not contain any information you do not want to be public, I would also be interested in the complete file.

I hope that is not too much to ask ;)

erblinium · 2023-02-24T09:34:00Z

Oh sorry. I remember now :)

aruco_objects.json.txt

benthie · 2023-09-06T09:54:17Z

Hi @erblinium,

I finally found some time to have a look at your recent changes on this branch and at the new features in the other two pull requests.

As I asked in one of the above conversations, could you write a test case for your feature? This would massively help us understand this feature and make reviewing it easier. For the test you could simply create a virtual camera in NxView containing some virtual objects, save it as ZIP-file and use it as the base for your test. Additionally you could maybe also save your objects file from NxView . The test case could look something like this:

Open the virtual camera together with the objects file
Move the camera
Check that the objects have also moved
Add/remove an object
Move the camera
Check that the objects have also moved
In case you create your objects file from within your code, it would be helpful to include the creation of the file into the test case as kind of a documentation.

In the conversation above you said that you are adding other objects in the meantime. As far as I understand the feature at the moment, you have an objects file with initial virtual object information, containing the original transformation of that object, which is later used to determine the new object pose after - I guess - the camera has been moved (?); what happens if we add a new object? Don't we have to remember its original transformation as well? And in case we also want to be able to remove an object, the corresponding original transformation has to be removed as well, right?

Kind regards
Benny

Implement configurable frames for virtual objects

cb31082

benthie requested changes Feb 23, 2023

View reviewed changes

WIP Refactor updateObjectLinks()

bfae7e2

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Implement configurable frames for virtual objects #109

Implement configurable frames for virtual objects #109

erblinium commented Feb 22, 2023

benthie left a comment

benthie Feb 23, 2023

benthie Feb 23, 2023 •

edited

Loading

benthie Feb 23, 2023

benthie Feb 23, 2023

benthie Feb 23, 2023

benthie Feb 23, 2023

erblinium Feb 24, 2023

benthie Feb 24, 2023

erblinium Feb 24, 2023 •

edited

Loading

benthie Feb 24, 2023

erblinium Feb 24, 2023

erblinium Feb 24, 2023

benthie Feb 24, 2023

erblinium Feb 27, 2023

benthie Feb 27, 2023

erblinium commented Feb 24, 2023 •

edited

Loading

benthie commented Feb 24, 2023

erblinium commented Feb 24, 2023

benthie commented Sep 6, 2023

		@@ -41,7 +41,7 @@ struct VirtualObjectMarkerPublisher
		VirtualObjectMarkerPublisher(ensenso::ros::NodeHandle _nh, const std::string& topic, double publishRate,
		const NxLibItem& objects, const std::string& frame, std::atomic_bool& stop_)

+void VirtualObjectHandler::updateObjectLinks()
+{
+  // Exit early if we have no work to do
+  if (originalTransforms.empty())
+  {
+    return;
+  }
+  auto objects = NxLibItem[itmObjects];
+  // Apply the transform to all of the original transforms
+  for (int i = 0; i < objects.count(); ++i)
+  {
+    auto objectLink = objects[i][itmLink];
+    try
+    {
+      // Find the transformation from the object frame to the current optical frame
+      auto objectFrame = objectLink[itmTarget].asString();
+      auto cameraTransform = fromMsg(tfBuffer.lookupTransform(linkFrame, objectFrame, ensenso::ros::Time(0)).transform);
+      // Transform object back to original frame
+      tf2::Transform objectTransform = cameraTransform * originalTransforms[i];
+      writeTransformToNxLib(objectTransform.inverse(), objectLink);
+    }
+    catch (const tf2::TransformException& e)
+    {
+      ENSENSO_WARN("Could not look up the virtual object pose due to the TF error: %s", e.what());
+      return;
+    }
+  }
+}

Implement configurable frames for virtual objects #109

Are you sure you want to change the base?

Implement configurable frames for virtual objects #109

Conversation

erblinium commented Feb 22, 2023

benthie left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

benthie Feb 23, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

erblinium Feb 24, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

erblinium commented Feb 24, 2023 • edited Loading

benthie commented Feb 24, 2023

erblinium commented Feb 24, 2023

benthie commented Sep 6, 2023

benthie Feb 23, 2023 •

edited

Loading

erblinium Feb 24, 2023 •

edited

Loading

erblinium commented Feb 24, 2023 •

edited

Loading