2020//
2121#include < Eigen/Dense>
2222#include < Eigen/Geometry>
23+ #include < iostream>
2324#include < string>
2425#include < vector>
2526
@@ -54,7 +55,7 @@ class Umeyama
5455 virtual void calcTransform () = 0;
5556 /* *
5657 * @brief transform point with transformation
57- *
58+ *
5859 * @param[in] pt - Eigen::Vector3d:
5960 * point to transform
6061 */
@@ -65,7 +66,6 @@ class Umeyama
6566 std::vector<Eigen::Vector3d> src_;
6667 std::vector<Eigen::Vector3d> target_;
6768};
68-
6969/* *
7070 * @brief inherited class for 2D Umeyama transformation
7171 */
@@ -78,7 +78,7 @@ class Umeyama_2D : public Umeyama
7878 void calcTransform () override ;
7979 /* *
8080 * @brief transform point with 2D transformation
81- *
81+ *
8282 * @param[in] pt - Eigen::Vector3d:
8383 * point to transform
8484 */
@@ -87,7 +87,7 @@ class Umeyama_2D : public Umeyama
8787private:
8888 /* *
8989 * @brief convert trajectory to 2xX-matrix
90- *
90+ *
9191 * @param[in] ls - std::vector<Eigen::Vector3d>
9292 * trajectory
9393 * @param[out] - Eigen::MatrixXd:
@@ -97,7 +97,6 @@ class Umeyama_2D : public Umeyama
9797 // 3x3 transformation matrix for 2D transformation
9898 Eigen::Matrix3d trans_;
9999};
100-
101100/* *
102101 * @brief inherited class for 3D Umeyama transformation
103102 */
@@ -110,7 +109,7 @@ class Umeyama_3D : public Umeyama
110109 void calcTransform () override ;
111110 /* *
112111 * @brief transform point with 3D transformation
113- *
112+ *
114113 * @param[in] pt - Eigen::Vector3d
115114 * point to transform
116115 */
@@ -119,7 +118,7 @@ class Umeyama_3D : public Umeyama
119118private:
120119 /* *
121120 * @brief convert trajectory to 3xX-matrix
122- *
121+ *
123122 * @param[in] ls - std::vector<Eigen::Vector3d>
124123 * trajectory
125124 * @param[out] - Eigen::MatrixXd:
@@ -129,23 +128,20 @@ class Umeyama_3D : public Umeyama
129128 // 4x4 transformation matrix for 3D transformation
130129 Eigen::Matrix4d trans_;
131130};
132-
133131/* *
134132 * @brief insert source vector of transformation
135133 *
136134 * @param[in] src - std::vector<Eigen::Vector3d>:
137135 * source trajectory
138136 */
139137void Umeyama::insertSource (const std::vector<Eigen::Vector3d> & src) { this ->src_ = src; }
140-
141138/* *
142139 * @brief insert target vector of transformation
143140 *
144141 * @param[in] target - std::vector<Eigen::Vector3d>:
145142 * target trajectory
146143 */
147144void Umeyama::insertTarget (const std::vector<Eigen::Vector3d> & target) { this ->target_ = target; }
148-
149145/* *
150146 * @brief calculate rigid transformation matrix in 2D
151147 */
@@ -172,7 +168,6 @@ void Umeyama_2D::calcTransform()
172168 }
173169 this ->trans_ = trans.inverse ();
174170}
175-
176171/* *
177172 * @brief calculate rigid transformation matrix in 3D
178173 */
@@ -199,10 +194,9 @@ void Umeyama_3D::calcTransform()
199194 }
200195 this ->trans_ = trans.inverse ();
201196}
202-
203197/* *
204198 * @brief transform point with 2D transformation
205- *
199+ *
206200 * @param[in] pt - Eigen::Vector3d
207201 * point to transform
208202 */
@@ -213,10 +207,9 @@ void Umeyama_2D::transformPoint(Eigen::Vector3d & pt) const
213207 const Eigen::Vector3d pt_al = this ->trans_ * point;
214208 pt = Eigen::Vector3d (pt_al (0 ), pt_al (1 ), pt (2 ));
215209}
216-
217210/* *
218211 * @brief transform point with 3D transformation
219- *
212+ *
220213 * @param[in] pt - Eigen::Vector3d
221214 * point to transform
222215 */
@@ -227,10 +220,9 @@ void Umeyama_3D::transformPoint(Eigen::Vector3d & pt) const
227220 const Eigen::Vector4d pt_al = this ->trans_ * point;
228221 pt = Eigen::Vector3d (pt_al (0 ), pt_al (1 ), pt_al (2 ));
229222}
230-
231223/* *
232224 * @brief convert trajectory to 2xX-matrix
233- *
225+ *
234226 * @param[in] ls - std::vector<Eigen::Vector3d>
235227 * trajectory
236228 * @param[out] - Eigen::MatrixXd:
@@ -246,10 +238,9 @@ Eigen::MatrixXd Umeyama_2D::ls2mat(const std::vector<Eigen::Vector3d> & ls)
246238 }
247239 return mat;
248240}
249-
250241/* *
251242 * @brief convert trajectory to 3xX-matrix
252- *
243+ *
253244 * @param[in] ls - std::vector<Eigen::Vector3d>
254245 * trajectory
255246 * @param[out] - Eigen::MatrixXd:
0 commit comments