ImageToArMem.cpp
Go to the documentation of this file.
1 #include "ImageToArMem.h"
2 
3 #include <sstream>
4 #include <unordered_set>
5 
9 
13 
15 #include <VisionX/libraries/armem/vision/camera/core/aron/CameraCalibration.aron.generated.h>
18 
19 #include <Image/ByteImage.h>
20 #include <Image/ImageProcessor.h>
21 
22 namespace visionx::armem_images
23 {
24 
25  ImageToArMem::ImageToArMem() : ImageAdapter("ImageToArMem")
26  {
27  }
28 
29  template <class AronImageT>
30  void
31  ImageToArMem::addImages(const armarx::armem::MemoryID& entityID,
32  const std::vector<size_t>& imageIndices)
33  {
34  auto& images = imagesByEntity[entityID];
35 
36  for (size_t i = 0; i < imageIndices.size(); ++i)
37  {
38  armarx::armem::MemoryID instanceID = entityID;
39  instanceID.instanceIndex = static_cast<int>(i);
40 
41  size_t index = imageIndices[i];
42  images.emplace_back(std::make_unique<Image<AronImageT>>(instanceID, index));
43  }
44  }
45 
46  void
47  ImageToArMem::setWritingMemory(armarx::armem::server::WritingMemoryInterfacePrx memory)
48  {
50  }
51 
52  void
54  {
55  memoryWriter = writer;
56  }
57 
58  void
60  const std::vector<size_t>& imageIndices)
61  {
62  if (imageIndices.empty())
63  {
64  return;
65  }
66 
67  addImages<arondto::ImageRGB>(entityID, imageIndices);
68  for (const auto& image : imagesByEntity.at(entityID))
69  {
70  ARMARX_CHECK_EQUAL(image->getImage().type(), CV_8UC3);
71  }
72  }
73 
74  void
76  const std::vector<size_t>& imageIndices)
77  {
78  if (imageIndices.empty())
79  {
80  return;
81  }
82 
83  addImages<arondto::ImageDepth>(entityID, imageIndices);
84  for (const auto& image : imagesByEntity.at(entityID))
85  {
86  ARMARX_CHECK_EQUAL(image->getImage().type(), CV_32FC1);
87  }
88  }
89 
92  {
93  auto it = imagesByEntity.begin();
94  armarx::armem::MemoryID id = it->first;
95  for (; it != imagesByEntity.end(); ++it)
96  {
97  ARMARX_CHECK_EQUAL(it->first.memoryName, id.memoryName)
98  << "Currently, all images must be from the same memory.";
99 
100  ARMARX_CHECK_EQUAL(it->first.providerSegmentName, id.providerSegmentName)
101  << "Currently, all images must be from the same provider.";
102  }
103  return id;
104  }
105 
106  armarx::armem::data::AddSegmentsInput
108  {
109  std::unordered_set<armarx::armem::MemoryID> providerIDs;
110  for (const auto& [entityID, image] : imagesByEntity)
111  {
112  providerIDs.insert(entityID.getProviderSegmentID());
113  }
114  armarx::armem::data::AddSegmentsInput inputs;
115  for (const armarx::armem::MemoryID& id : providerIDs)
116  {
117  armarx::armem::data::AddSegmentInput& input = inputs.emplace_back();
118  input.coreSegmentName = id.coreSegmentName;
119  input.providerSegmentName = id.providerSegmentName;
120  input.clearWhenExists = clearWhenExists;
121  }
122  return inputs;
123  }
124 
125  armarx::armem::data::AddSegmentsResult
126  ImageToArMem::addProviderSegments(bool clearWhenExists, int verbose)
127  {
128  const armarx::armem::data::AddSegmentsInput inputs = makeAddSegmentsInput(clearWhenExists);
129  const armarx::armem::data::AddSegmentsResult results = memoryWriter.addSegments(inputs);
130  if (verbose >= 1)
131  {
132  for (size_t i = 0; i < results.size(); ++i)
133  {
134  if (!results.at(i).success)
135  {
136  const armarx::armem::data::AddSegmentInput& in = inputs.at(i);
137  ARMARX_ERROR << "Failed to add provider segment '" << in.coreSegmentName << "/"
138  << in.providerSegmentName << "'."
139  << "\n"
140  << results.at(0).errorMessage;
141  }
142  }
143  }
144  return results;
145  }
146 
147  void
148  ImageToArMem::useImageBuffers(CByteImage** inputImageBuffer, armarx::armem::Time timeProvided)
149  {
150  for (auto& [entityID, images] : imagesByEntity)
151  {
152  for (std::unique_ptr<ImageBase>& image : images)
153  {
154  image->usePixels(inputImageBuffer);
155  }
156  }
157  this->timestamp = timeProvided;
158  }
159 
160  void
161  ImageToArMem::usePixelBuffers(void** inputImageBuffers, armarx::armem::Time timeProvided)
162  {
163  for (auto& [entityID, images] : imagesByEntity)
164  {
165  for (std::unique_ptr<ImageBase>& image : images)
166  {
167  image->usePixels(inputImageBuffers);
168  }
169  }
170  this->timestamp = timeProvided;
171  }
172 
175  {
176  armarx::armem::Commit commit;
177  for (const auto& [entityID, images] : imagesByEntity)
178  {
179  if (images.size())
180  {
181  armarx::armem::EntityUpdate& update = commit.updates.emplace_back();
182  update.entityID = entityID;
183  update.referencedTime = timestamp;
184  for (const auto& image : images)
185  {
186  update.instancesData.push_back(image->toAron());
187  }
188  }
189  }
190  return commit;
191  }
192 
193  void
195  {
197  ARMARX_DEBUG << "Got a new image for processing. Sending images to vision memory.";
198 
199  TIMING_START(ToAron);
202 
203  TIMING_START(Commit);
206 
207  ARMARX_CHECK_EQUAL(results.results.size(), commit.updates.size());
208  {
209  std::stringstream ss;
210  for (size_t i = 0; i < results.results.size(); ++i)
211  {
212  ss << "\n- " << commit.updates.at(i).entityID << ": \n" << results.results.at(i);
213  }
214  ARMARX_DEBUG << "Commit results: " << ss.str();
215  }
216 
217  if (debugObserver)
218  {
219  debugObserver->setDebugChannel(
221  {
222  {"ToAron [us]", new armarx::Variant(ToAron.toMicroSecondsDouble())},
223  {"Commit [us]", new armarx::Variant(Commit.toMicroSecondsDouble())},
224  });
225  }
226  }
227 
228  void
229  ImageToArMem::commitCameraCalibration(const visionx::StereoCalibration& calib)
230  {
231  visionx::arondto::StereoCameraCalibration aron;
232  visionx::armem::camera::fromIce(aron, calib);
233 
235  armarx::armem::EntityUpdate& eu = c.add();
236 
237  eu.confidence = 1.0;
238 
242  getMemoryID().providerSegmentName,
244 
245  eu.instancesData = {aron.toAron()};
246 
249  auto result = memoryWriter.commit(c);
250 
251  if (not result.allSuccess())
252  {
253  ARMARX_ERROR << "An error occured when sending pointclouds to the memory. "
254  "Try to continue with next PC. The message was: "
255  << result.allErrorMessages();
256  }
257  }
258 
259  void
260  ImageToArMem::commitCameraCalibration(const visionx::MonocularCalibration& calib)
261  {
262  visionx::arondto::MonocularCameraCalibration aron;
263  visionx::armem::camera::fromIce(aron, calib);
264 
266  armarx::armem::EntityUpdate& eu = c.add();
267 
268  eu.confidence = 1.0;
269 
273  getMemoryID().providerSegmentName,
275 
276  eu.instancesData = {aron.toAron()};
277 
280  auto result = memoryWriter.commit(c);
281 
282  if (not result.allSuccess())
283  {
284  ARMARX_ERROR << "An error occured when sending pointclouds to the memory. "
285  "Try to continue with next PC. The message was: "
286  << result.allErrorMessages();
287  }
288  }
289 
290  std::string
292  {
293  std::stringstream ss;
294  for (const auto& [entityID, images] : imagesByEntity)
295  {
296  ss << "- Entity " << entityID << ": \n";
297  for (const auto& image : images)
298  {
299  ss << "- - [image #" << image->imageIndex << "] id = " << image->instanceID << "\n";
300  }
301  }
302  return ss.str();
303  }
304 
305  void
306  ImageToArMem::initImages(int width, int height, CByteImage::ImageType type)
307  {
308  for (auto& [entityID, images] : imagesByEntity)
309  {
310  for (auto& image : images)
311  {
312  image->resetImage(height, width);
313  }
314  }
315  }
316 
317  void
318  ImageToArMem::initImages(const ImageFormatInfo& info)
319  {
320  for (auto& [entityID, images] : imagesByEntity)
321  {
322  for (auto& image : images)
323  {
324  image->resetImage(info.dimension.height, info.dimension.width);
325  }
326  }
327  }
328 
329  std::vector<CByteImage>
331  {
332  std::vector<CByteImage> buffer;
333  for (auto& [entityID, images] : imagesByEntity)
334  {
335  for (auto& image : images)
336  {
337  if (image->imageIndex >= buffer.size())
338  {
339  buffer.resize(image->imageIndex + 1);
340  }
341  cv::Mat& mat = image->getImage();
342  buffer[image->imageIndex].Set(mat.cols, mat.rows, CByteImage::eRGB24);
343  }
344  }
345  return buffer;
346  }
347 
348 } // namespace visionx::armem_images
visionx::armem_images::ImageToArMem::summarizeStructure
std::string summarizeStructure() const override
Definition: ImageToArMem.cpp:291
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
TIMING_START
#define TIMING_START(name)
Definition: TimeUtil.h:280
visionx::armem_images::ImageToArMem::initImages
void initImages(int width, int height, CByteImage::ImageType type)
Definition: ImageToArMem.cpp:306
armarx::armem::EntityUpdate::instancesData
std::vector< aron::data::DictPtr > instancesData
The entity data.
Definition: Commit.h:33
client.h
armarx::armem::Commit
A bundle of updates to be sent to the memory.
Definition: Commit.h:89
index
uint8_t index
Definition: EtherCATFrame.h:59
armarx::core::time::DateTime::Now
static DateTime Now()
Definition: DateTime.cpp:55
visionx::armem_images::ImageToArMem::addImagesRGB
void addImagesRGB(const armarx::armem::MemoryID &entityID, const std::vector< size_t > &imageIndices) override
Definition: ImageToArMem.cpp:59
visionx::armem_images::detail::ImageAdapter::debugObserverChannelName
std::string debugObserverChannelName
Definition: ImageAdapter.h:34
visionx::armem_images::ImageToArMem::useImageBuffers
void useImageBuffers(CByteImage **inputImageBuffer, armarx::armem::Time timeProvided)
Store image data from the given buffer.
Definition: ImageToArMem.cpp:148
armarx::armem::client::Writer::setWritingMemory
void setWritingMemory(server::WritingMemoryInterfacePrx memory)
Definition: Writer.cpp:97
visionx::armem::camera::ENTITY_NAME
const constexpr char * ENTITY_NAME
Definition: constants.h:16
armarx::armem::EntityUpdate::confidence
float confidence
An optional confidence, may be used for things like decay.
Definition: Commit.h:45
armarx::armem::MemoryID::str
std::string str(bool escapeDelimiters=true) const
Get a string representation of this memory ID.
Definition: MemoryID.cpp:102
visionx::armem_images::ImageToArMem::setWritingMemory
void setWritingMemory(armarx::armem::server::WritingMemoryInterfacePrx memory)
Definition: ImageToArMem.cpp:47
visionx::armem_images::ImageToArMem::makeCommit
armarx::armem::Commit makeCommit() const
Build the commit.
Definition: ImageToArMem.cpp:174
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
query_fns.h
armarx::armem::MemoryID::instanceIndex
int instanceIndex
Definition: MemoryID.h:55
visionx::armem_images::ImageToArMem::memoryWriter
armarx::armem::client::Writer memoryWriter
Definition: ImageToArMem.h:74
visionx::armem_images::ImageToArMem::getMemoryID
armarx::armem::MemoryID getMemoryID() const override
Definition: ImageToArMem.cpp:91
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
TIMING_END_STREAM
#define TIMING_END_STREAM(name, os)
Definition: TimeUtil.h:300
armarx::armem::Commit::updates
std::vector< EntityUpdate > updates
The entity updates.
Definition: Commit.h:97
armarx::armem::client::Writer::commit
CommitResult commit(const Commit &commit)
Writes a Commit to the memory.
Definition: Writer.cpp:51
visionx::armem_images::ImageToArMem::makeAddSegmentsInput
armarx::armem::data::AddSegmentsInput makeAddSegmentsInput(bool clearWhenExists=true)
Build inputs for adding the required provider segments.
Definition: ImageToArMem.cpp:107
visionx::armem::camera::fromIce
void fromIce(visionx::arondto::CameraParameters &aron, const visionx::CameraParameters &ice)
Definition: ice_conversions.cpp:31
visionx::armem_images::ImageToArMem::usePixelBuffers
void usePixelBuffers(void **inputImageBuffer, armarx::armem::Time timeProvided)
Definition: ImageToArMem.cpp:161
visionx::armem_images
Definition: ImageReader.cpp:25
armarx::armem::MemoryID
A memory ID.
Definition: MemoryID.h:47
visionx::armem_images::detail::ImageAdapter::debugObserver
armarx::DebugObserverInterfacePrx debugObserver
Definition: ImageAdapter.h:33
constants.h
visionx::armem_images::ImageToArMem::ImageToArMem
ImageToArMem()
Definition: ImageToArMem.cpp:25
ice_conversions.h
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
visionx::armem_images::ImageToArMem::addProviderSegments
armarx::armem::data::AddSegmentsResult addProviderSegments(bool clearWhenExists=true, int verbose=1)
Add the required provider segments.
Definition: ImageToArMem.cpp:126
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:19
visionx::armem_images::ImageToArMem::makeCByteImageBuffer
std::vector< CByteImage > makeCByteImageBuffer()
Definition: ImageToArMem.cpp:330
armarx::armem::EntityUpdate
An update of an entity for a specific point in time.
Definition: Commit.h:27
armarx::armem::CommitResult
Result of a Commit.
Definition: Commit.h:110
armarx::armem::client::Writer
Helps a memory client sending data to a memory.
Definition: Writer.h:22
visionx::armem_images::ImageToArMem::timestamp
armarx::armem::Time timestamp
Definition: ImageToArMem.h:78
visionx::armem_images::ImageToArMem::addImagesDepth
void addImagesDepth(const armarx::armem::MemoryID &entityID, const std::vector< size_t > &imageIndices) override
Definition: ImageToArMem.cpp:75
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::armem::MemoryID::getProviderSegmentID
MemoryID getProviderSegmentID() const
Definition: MemoryID.cpp:297
visionx::armem_images::ImageToArMem::imagesByEntity
std::map< armarx::armem::MemoryID, std::vector< std::unique_ptr< ImageBase > > > imagesByEntity
Entity ID to memory images (instances).
Definition: ImageToArMem.h:77
visionx::armem::camera::STEREO_CAMERA_CALIBRATION_CORE_SEGMENT_NAME
const constexpr char * STEREO_CAMERA_CALIBRATION_CORE_SEGMENT_NAME
Definition: constants.h:13
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:67
ExpressionException.h
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::armem::EntityUpdate::referencedTime
Time referencedTime
Time when this entity update was created (e.g.
Definition: Commit.h:39
TimeUtil.h
armarx::armem::CommitResult::results
std::vector< EntityUpdateResult > results
Definition: Commit.h:112
armarx::armem::client::Writer::addSegments
data::AddSegmentsResult addSegments(const data::AddSegmentsInput &input)
Definition: Writer.cpp:42
armarx::armem::laser_scans::constants::memoryName
const std::string memoryName
Definition: constants.h:28
visionx::armem_images::Image
A class template implementing the interface defined by ImageBase for a specific aron-generated class.
Definition: forward_declarations.h:33
visionx::armem_images::ImageToArMem::commitCameraCalibration
void commitCameraCalibration(const visionx::StereoCalibration &)
Calibration.
Definition: ImageToArMem.cpp:229
Builder.h
ImageUtil.h
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
visionx::armem_images::ImageToArMem::commitImages
void commitImages()
Commit the stored image data.
Definition: ImageToArMem.cpp:194
armarx::armem::EntityUpdate::entityID
MemoryID entityID
The entity's ID.
Definition: Commit.h:30
Variant.h
armarx::armem::EntityUpdate::sentTime
Time sentTime
Time when this update was sent to the memory server.
Definition: Commit.h:55
ImageToArMem.h
visionx::armem_images::ImageToArMem::setWriter
void setWriter(const armarx::armem::client::Writer &writer)
Definition: ImageToArMem.cpp:53
armarx::human::MemoryID
const armem::MemoryID MemoryID
Definition: memory_ids.cpp:29