Add torch map conversion

Saurav Agarwal · Saurav Agarwal · commit fe10155c8c90 · 2023-05-21T04:11:27.000-04:00
diff --git a/cppsrc/tests/torch_map_conversion.cpp b/cppsrc/tests/torch_map_conversion.cpp
@@ -0,0 +1,43 @@
+#include <torch/torch.h>
+#include <iostream>
+
+#include <iomanip>
+#include <iostream>
+#include <random>
+
+#include <CoverageControl/constants.h>
+#include <CoverageControl/typedefs.h>
+#include <CoverageControl/bivariate_normal_distribution.h>
+#include <CoverageControl/world_idf.h>
+#include <CoverageControl/robot_model.h>
+#include <CoverageControl/coverage_system.h>
+
+#include <CoverageControlTorch/type_conversions.h>
+
+using namespace CoverageControl;
+using namespace CoverageControlTorch;
+
+int main(int argc, char** argv) {
+	Parameters params;
+
+	params.pTruncationBND = 10;
+	std::unique_ptr <CoverageSystem> env;
+
+	std::vector <Point2> robot_positions;
+	robot_positions.push_back(Point2(100, 10));
+	robot_positions.push_back(Point2(100, 100));
+	robot_positions.push_back(Point2(10, 100));
+
+	WorldIDF world_idf(params);
+	world_idf.AddNormalDistribution(BivariateNormalDistribution(Point2(50, 50), 30));
+
+	env = std::make_unique<CoverageSystem> (params, world_idf, robot_positions);
+
+	auto env_map = env->GetWorldIDF();
+	auto env_map_tensor = EigenToLibTorch(env_map);
+	std::cout << env_map_tensor.dtype() << std::endl;
+
+	print(env_map_tensor[50][50]);
+
+	return 0;
+}
diff --git a/cppsrc/torch/include/CoverageControlTorch/type_conversions.h b/cppsrc/torch/include/CoverageControlTorch/type_conversions.h
@@ -0,0 +1,29 @@
+/**
+ * Type conversion between CoverageControl and Torch
+ *
+ * TODO: 
+ **/
+
+#ifndef COVERAGECONTROLTORCH_TYPE_CONVERSION_H_
+#define COVERAGECONTROLTORCH_TYPE_CONVERSION_H_
+
+#include <vector>
+#include <Eigen/Dense> // Eigen is used for maps
+
+#include <CoverageControl/typedefs.h>
+
+#include <torch/torch.h>
+
+using namespace CoverageControl;
+namespace CoverageControlTorch {
+
+		torch::Tensor EigenToLibTorch(MapType const &M) {
+			MapType M_copy = M;
+			std::vector<int64_t> dims = {M_copy.rows(), M_copy.cols()};
+			auto T = torch::from_blob(M_copy.data(), dims).clone();
+			return T;
+		}
+
+} // namespace CoverageControlTorch
+
+#endif // COVERAGECONTROLTORCH_TYPE_CONVERSION_H_
