1+ #include < iostream>
2+ #include < vector>
3+ #include < chrono>
4+ #include < random>
5+ #include < tf2/LinearMath/Quaternion.h>
6+ #include < tf2/impl/utils.h>
7+ #include < chrono>
8+ #include < functional>
9+ #include < memory>
10+ #include < string>
11+ #include < eigen3/Eigen/Dense>
12+ #include < eigen3/Eigen/Core>
13+ #include < eigen3/Eigen/SVD>
14+ #include < cstdlib>
15+ #include < cmath>
16+ #include < algorithm>
17+ #include < netinet/in.h>
18+ #include < sys/types.h>
19+ #include < stdlib.h>
20+ #include < time.h>
21+ #include < fstream>
22+ #include < stdio.h>
23+ #include < unistd.h>
24+ #include < sys/stat.h>
25+ #include < math.h>
26+
27+
28+ #include " rclcpp/rclcpp.hpp"
29+ #include " turtlebot3_msgs/msg/gaussian.hpp"
30+ #include " turtlebot3_msgs/msg/gmm.hpp"
31+
32+
33+ #include " gaussian_mixture_model/gaussian_mixture_model.h"
34+
35+
36+
37+ class GMM_Test : public rclcpp ::Node
38+ {
39+ private:
40+ GaussianMixtureModel gmm;
41+ std::vector<Eigen::MatrixXd> covariances;
42+ std::vector<Eigen::VectorXd> mean_points;
43+ std::vector<double > weights;
44+ Eigen::MatrixXd samples;
45+
46+ // ROS Related Variables
47+ rclcpp::TimerBase::SharedPtr timer_;
48+ turtlebot3_msgs::msg::GMM gmm_msg;
49+ rclcpp::Publisher<turtlebot3_msgs::msg::GMM>::SharedPtr gmm_pub_;
50+
51+ public:
52+
53+ // initialize GMM with 5 random components
54+ GMM_Test (): Node(" gmm_test" 4 )
55+ {
56+ std::cout << " Constructor called"
57+
58+ // Initialize ROS variables
59+ timer_ = this ->create_wall_timer (std::chrono::milliseconds (100 ), std::bind (&GMM_Test::timerCallback, this ));
60+ gmm_pub_ = this ->create_publisher <turtlebot3_msgs::msg::GMM>(" /gaussian_mixture_model" 10 );
61+
62+ samples.resize (2 ,200 );
63+ double dev = 0.5 ;
64+ std::default_random_engine gen;
65+
66+ // Set desired values
67+ Eigen::VectorXd p1 (2 );
68+ p1 << -6.0 , 0.0 ;
69+ // Generate samples
70+ std::normal_distribution<double > dist_x (p1 (0 ), dev);
71+ std::normal_distribution<double > dist_y (p1 (1 ), dev);
72+
73+ for (int i = 0 ; i < 50 ; i++)
74+ {
75+ samples (0 ,i) = dist_x (gen);
76+ samples (1 ,i) = dist_y (gen);
77+ }
78+
79+ // Set desired values
80+ Eigen::VectorXd p2 (2 );
81+ p2 << -4.0 , 4.0 ;
82+ // Generate samples
83+ std::normal_distribution<double > dist_x2 (p2 (0 ), dev);
84+ std::normal_distribution<double > dist_y2 (p2 (1 ), dev);
85+
86+ for (int i = 50 ; i < 100 ; i++)
87+ {
88+ samples (0 ,i) = dist_x2 (gen);
89+ samples (1 ,i) = dist_y2 (gen);
90+ }
91+
92+
93+ // Set desired values
94+ Eigen::VectorXd p3 (2 );
95+ p3 << 4.0 , 6.0 ;
96+ // Generate samples
97+ std::normal_distribution<double > dist_x3 (p3 (0 ), dev);
98+ std::normal_distribution<double > dist_y3 (p3 (1 ), dev);
99+
100+ for (int i = 100 ; i < 150 ; i++)
101+ {
102+ samples (0 ,i) = dist_x3 (gen);
103+ samples (1 ,i) = dist_y3 (gen);
104+ }
105+
106+ // Set desired values
107+ Eigen::VectorXd p4 (2 );
108+ p4 << 4.0 , -6.0 ;
109+ // Generate samples
110+ std::normal_distribution<double > dist_x4 (p4 (0 ), dev);
111+ std::normal_distribution<double > dist_y4 (p4 (1 ), dev);
112+
113+ for (int i = 150 ; i < 200 ; i++)
114+ {
115+ samples (0 ,i) = dist_x4 (gen);
116+ samples (1 ,i) = dist_y4 (gen);
117+ }
118+
119+ auto timerstart = std::chrono::high_resolution_clock::now ();
120+ gmm.fitgmm (samples, 4 , 1000 , 1e-3 , false );
121+ auto end = std::chrono::high_resolution_clock::now ();
122+ std::cout<<" Computation time for EM: -------------: " count ()<<" ms :-------------\n "
123+
124+ mean_points = gmm.getMeans ();
125+ covariances = gmm.getCovariances ();
126+ weights = gmm.getWeights ();
127+
128+ std::cout << " GMM Initialized"
129+
130+ std::cout << " Mean Points: "
131+ for (int i = 0 ; i < mean_points.size (); i++)
132+ {
133+ std::cout << mean_points[i] << std::endl;
134+ }
135+
136+ std::cout << " Covariances: "
137+ for (int i = 0 ; i < covariances.size (); i++)
138+ {
139+ std::cout << covariances[i] << std::endl;
140+ }
141+
142+ std::cout << " Weights: "
143+ for (int i = 0 ; i < weights.size (); i++)
144+ {
145+ std::cout << weights[i] << std::endl;
146+ }
147+
148+ // Create ROS msg
149+ gmm_msg.weights = weights;
150+
151+ for (int i = 0 ; i < mean_points.size (); i++)
152+ {
153+ turtlebot3_msgs::msg::Gaussian gaussian;
154+ gaussian.mean_point .x = mean_points[i](0 );
155+ gaussian.mean_point .y = mean_points[i](1 );
156+ gaussian.covariance .push_back (covariances[i](0 ,0 ));
157+ gaussian.covariance .push_back (covariances[i](0 ,1 ));
158+ gaussian.covariance .push_back (covariances[i](1 ,0 ));
159+ gaussian.covariance .push_back (covariances[i](1 ,1 ));
160+ gmm_msg.gaussians .push_back (gaussian);
161+ }
162+
163+ std::cout << " ROS MSG Initialized"
164+
165+ }
166+
167+ ~GMM_Test ()
168+ {
169+ std::cout<<" DESTROYER HAS BEEN CALLED"
170+ }
171+
172+ void timerCallback ()
173+ {
174+ gmm_pub_->publish (gmm_msg);
175+ }
176+
177+
178+
179+ };// End of class SubscribeAndPublish
180+
181+
182+ int main (int argc, char **argv)
183+ {
184+ rclcpp::init (argc, argv);
185+ rclcpp::spin (std::make_shared<GMM_Test>());
186+ rclcpp::shutdown ();
187+ return 0 ;
188+ }
0 commit comments