Skip to content

Instantly share code, notes, and snippets.

@graugans

graugans/xyz_image.patch

Last active July 26, 2018 06:45
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save graugans/21ec5d5d2ffc382d12dbda076ced8ef0 to your computer and use it in GitHub Desktop.
Save graugans/21ec5d5d2ffc382d12dbda076ced8ef0 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
xyz_image.patch
diff --git a/simpleimage/example/ex-simpleImage_ppm_io.cpp b/simpleimage/example/ex-simpleImage_ppm_io.cpp
index 081617f..c2f6123 100644
--- a/simpleimage/example/ex-simpleImage_ppm_io.cpp
+++ b/simpleimage/example/ex-simpleImage_ppm_io.cpp
@@ -32,12 +32,8 @@
#include <ifm3d/fg.h>
#include <ifm3d/simpleimage.h>
-
-
using namespace std;
-
-
bool writePPMFile(ifm3d::SimpleImageBuffer::Img &img, std::string const& filename)
{
auto const write_width = (size_t)img.width;
@@ -106,9 +102,31 @@ void findMinAndMax(ifm3d::SimpleImageBuffer::Img &input, ifm3d::SimpleImageBuffe
}
}
+
+void seprateXYZImages(ifm3d::SimpleImageBuffer::Img &input, ifm3d::SimpleImageBuffer::Img &X, ifm3d::SimpleImageBuffer::Img &Y, ifm3d::SimpleImageBuffer::Img &Z)
+{
+ X.width =Y.width =Z.width = input.width;
+ X.height =Y.height =Z.height = input.height;
+ X.format = Y.format = ifm3d::pixel_format::FORMAT_16S;
+ Z.format = ifm3d::pixel_format::FORMAT_16U;
+ X.data.resize(input.width*input.height * sizeof(int16_t));
+ Y.data.resize(input.width*input.height * sizeof(int16_t));
+ Z.data.resize(input.width*input.height * sizeof(uint16_t));
+
+ for (int index = 0; index < input.width * input.height; index++ )
+ {
+ *((int16_t*)(X.data.data()) + index) = *((int16_t*)(input.data.data()) + (index*3));
+ *((int16_t*)(Y.data.data()) + index) = *((int16_t*)(input.data.data()) + (index*3 + 1));
+ *((uint16_t*)(Z.data.data()) + index) = *((uint16_t*)(input.data.data()) + (index*3 + 2));
+
+ }
+}
+
+
+
int main(int argc, const char **argv)
{
- auto cam = ifm3d::Camera::MakeShared();
+ auto cam = ifm3d::Camera::MakeShared("");
ifm3d::SimpleImageBuffer::Ptr img = std::make_shared<ifm3d::SimpleImageBuffer>();
ifm3d::FrameGrabber::Ptr fg =
@@ -120,11 +138,13 @@ int main(int argc, const char **argv)
std::cerr << "Timeout waiting for camera!" << std::endl;
return -1;
}
+
// acquiring data from the device
ifm3d::SimpleImageBuffer::Img confidence = img->ConfidenceImage();
+
ifm3d::SimpleImageBuffer::Img amplitude = img->AmplitudeImage();
ifm3d::SimpleImageBuffer::Img distance = img->DistanceImage();
-
+
// for storing scaled output
ifm3d::SimpleImageBuffer::Img distance_scaled;
ifm3d::SimpleImageBuffer::Img amplitude_scaled;
@@ -144,9 +164,9 @@ int main(int argc, const char **argv)
//for 16u data O3D camera
else if(distance.format == ifm3d::pixel_format::FORMAT_16U)
{ //if data format is 16U then distances are in millimeters, Hence max distance is multiplied by 1000.
- scaleImageToRGB<unsigned short>(distance, confidence, distance_scaled, min_distance, max_distance * 1000);
- findMinAndMax<unsigned short>(amplitude, confidence, min, max);
- scaleImageToRGB<unsigned short>(amplitude, confidence, amplitude_scaled, min, max);
+ scaleImageToRGB<uint16_t>(distance, confidence, distance_scaled, min_distance, max_distance * 1000);
+ findMinAndMax<uint16_t>(amplitude, confidence, min, max);
+ scaleImageToRGB<uint16_t>(amplitude, confidence, amplitude_scaled, min, max);
}
else
{
@@ -166,6 +186,42 @@ int main(int argc, const char **argv)
return -1;
}
+/* additional code for XYZ assuming O3D camera*/
+
+ ifm3d::SimpleImageBuffer::Img xyz = img->XYZImage();
+
+ ifm3d::SimpleImageBuffer::Img X_image;
+ ifm3d::SimpleImageBuffer::Img Y_image;
+ ifm3d::SimpleImageBuffer::Img Z_image;
+
+ ifm3d::SimpleImageBuffer::Img X_image_scaled;
+ ifm3d::SimpleImageBuffer::Img Y_image_scaled;
+ ifm3d::SimpleImageBuffer::Img Z_image_scaled;
+
+ seprateXYZImages(xyz, Z_image, X_image, Y_image);
+
+ scaleImageToRGB< int16_t>(X_image, confidence, X_image_scaled, -max_distance*1000, max_distance * 1000);
+ scaleImageToRGB< int16_t>(Y_image, confidence, Y_image_scaled, -max_distance*1000, max_distance * 1000);
+ scaleImageToRGB<uint16_t>(Z_image, confidence, Z_image_scaled, min_distance, max_distance * 1000);
+
+ if (!writePPMFile(X_image_scaled, "xImage.ppm"))
+ {
+ std::cerr << "Not able to write the x data in ppm format" << std::endl;
+ return -1;
+ }
+ if (!writePPMFile(Y_image_scaled, "yImage.ppm"))
+ {
+ std::cerr << "Not able to write the y data in ppm format" << std::endl;
+ return -1;
+ }
+
+ if (!writePPMFile(Z_image_scaled, "zImage.ppm"))
+ {
+ std::cerr << "Not able to write the z data in ppm format" << std::endl;
+ return -1;
+ }
+
+
std::cout << "Done with simpleimage ppmio example" << std::endl;
return 0;
}
@graugans
Copy link
Author 

diff --git a/simpleimage/example/ex-simpleImage_ppm_io.cpp b/simpleimage/example/ex-simpleImage_ppm_io.cpp
index 081617f..a82e599 100644
--- a/simpleimage/example/ex-simpleImage_ppm_io.cpp
+++ b/simpleimage/example/ex-simpleImage_ppm_io.cpp
@@ -32,12 +32,8 @@
 #include <ifm3d/fg.h>
 #include <ifm3d/simpleimage.h>
 
-
-
 using namespace std;
 
-
-
 bool writePPMFile(ifm3d::SimpleImageBuffer::Img &img, std::string const& filename)
 {
 	auto const write_width = (size_t)img.width;
@@ -106,6 +102,28 @@ void findMinAndMax(ifm3d::SimpleImageBuffer::Img &input, ifm3d::SimpleImageBuffe
 	}
 }
 
+// We consider here a Right Hand rule and for a mobile robot using ROS, the z-axis points upward, 
+//the x-axis points forward, and the y-axis points to the left.
+void seprateXYZImages(ifm3d::SimpleImageBuffer::Img &input, ifm3d::SimpleImageBuffer::Img &X, ifm3d::SimpleImageBuffer::Img &Y, ifm3d::SimpleImageBuffer::Img &Z)
+{
+	X.width  =Y.width  =Z.width  = input.width;
+	X.height =Y.height =Z.height = input.height;
+	Y.format = Z.format = ifm3d::pixel_format::FORMAT_16S;
+	X.format = ifm3d::pixel_format::FORMAT_16U;
+	X.data.resize(input.width*input.height * sizeof(uint16_t));
+	Y.data.resize(input.width*input.height * sizeof(int16_t));
+	Z.data.resize(input.width*input.height * sizeof(int16_t));
+	
+	for (int index = 0; index < input.width * input.height; index++ )
+	{
+		*((uint16_t*)(X.data.data()) + index) = *((uint16_t*)(input.data.data()) + (index*3));
+		*((int16_t*)(Y.data.data()) + index) = *((int16_t*)(input.data.data()) + (index*3 + 1));
+		*((int16_t*)(Z.data.data()) + index) =  *((int16_t*)(input.data.data()) + (index*3 + 2));
+	}
+}
+
+
+
 int main(int argc, const char **argv)
 {
 	auto cam = ifm3d::Camera::MakeShared();
@@ -120,11 +138,13 @@ int main(int argc, const char **argv)
 		std::cerr << "Timeout waiting for camera!" << std::endl;
 		return -1;
 	}
+
 	// acquiring data from the device 
 	ifm3d::SimpleImageBuffer::Img confidence = img->ConfidenceImage();
+
 	ifm3d::SimpleImageBuffer::Img amplitude = img->AmplitudeImage();
 	ifm3d::SimpleImageBuffer::Img distance = img->DistanceImage();
-
+	
 	// for storing scaled output 
 	ifm3d::SimpleImageBuffer::Img distance_scaled;
 	ifm3d::SimpleImageBuffer::Img amplitude_scaled;
@@ -144,9 +164,9 @@ int main(int argc, const char **argv)
 	//for 16u data  O3D camera
 	else if(distance.format == ifm3d::pixel_format::FORMAT_16U)
 	{	  //if data format is 16U then distances are in millimeters, Hence max distance is multiplied by 1000. 
-		scaleImageToRGB<unsigned short>(distance, confidence, distance_scaled, min_distance, max_distance * 1000);
-		findMinAndMax<unsigned short>(amplitude, confidence, min, max);
-		scaleImageToRGB<unsigned short>(amplitude, confidence, amplitude_scaled, min, max);
+		scaleImageToRGB<uint16_t>(distance, confidence, distance_scaled, min_distance, max_distance * 1000);
+		findMinAndMax<uint16_t>(amplitude, confidence, min, max);
+		scaleImageToRGB<uint16_t>(amplitude, confidence, amplitude_scaled, min, max);
 	}
 	else
 	{
@@ -166,6 +186,45 @@ int main(int argc, const char **argv)
 		return -1;
 	}
 
+/* additional code for XYZ assuming O3D camera*/
+
+	ifm3d::SimpleImageBuffer::Img xyz = img->XYZImage();
+
+	ifm3d::SimpleImageBuffer::Img X_image;
+	ifm3d::SimpleImageBuffer::Img Y_image;
+	ifm3d::SimpleImageBuffer::Img Z_image;
+
+	ifm3d::SimpleImageBuffer::Img X_image_scaled;
+	ifm3d::SimpleImageBuffer::Img Y_image_scaled;
+	ifm3d::SimpleImageBuffer::Img Z_image_scaled;
+
+	// We consider here a Right Hand rule and for a mobile robot using ROS, the z-axis points upward, 
+	//the x-axis points forward, and the y-axis points to the left.
+
+	seprateXYZImages(xyz, X_image, Y_image, Z_image);
+
+	scaleImageToRGB< int16_t>(X_image, confidence, X_image_scaled, -max_distance*1000, max_distance * 1000);
+	scaleImageToRGB< int16_t>(Y_image, confidence, Y_image_scaled, -max_distance*1000, max_distance * 1000);
+	scaleImageToRGB<uint16_t>(Z_image, confidence, Z_image_scaled, min_distance, max_distance * 1000);
+	
+	if (!writePPMFile(X_image_scaled, "FrontImage.ppm"))
+	{
+		std::cerr << "Not able to write the x data in ppm format" << std::endl;
+		return -1;
+	}
+	if (!writePPMFile(Y_image_scaled, "LeftToRightImage.ppm"))
+	{
+		std::cerr << "Not able to write the y data in ppm format" << std::endl;
+		return -1;
+	}
+
+	if (!writePPMFile(Z_image_scaled, "upToBottom.ppm"))
+	{
+		std::cerr << "Not able to write the z data in ppm format" << std::endl;
+		return -1;
+	}
+
+
 	std::cout << "Done with simpleimage ppmio example" << std::endl;
 	return 0;
 }

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment