From 409f0aceeaca293c160205d18fa02b38e43a2cf4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 26 Jan 2018 12:16:23 +0100
Subject: [PATCH] Rename ConstraintFix and ConstraintFix3D

---
 src/capture_fix.cpp                           |  4 ++--
 src/capture_fix.h                             |  4 ++--
 ...{constraint_fix.h => constraint_pose_2D.h} | 22 +++++++++----------
 ...nstraint_fix_3D.h => constraint_pose_3D.h} | 16 +++++++-------
 src/examples/test_imuDock.cpp                 |  4 ++--
 src/examples/test_imuDock_autoKFs.cpp         |  4 ++--
 src/solver/qr_solver.h                        |  4 +---
 src/test/CMakeLists.txt                       | 16 +++++++-------
 src/test/gtest_constraint_imu.cpp             |  7 +++---
 ...t_fix.cpp => gtest_constraint_pose_2D.cpp} | 10 ++++-----
 ...ix_3D.cpp => gtest_constraint_pose_3D.cpp} |  8 +++----
 src/wolf.h                                    |  4 ++--
 12 files changed, 50 insertions(+), 53 deletions(-)
 rename src/{constraint_fix.h => constraint_pose_2D.h} (67%)
 rename src/{constraint_fix_3D.h => constraint_pose_3D.h} (61%)
 rename src/test/{gtest_constraint_fix.cpp => gtest_constraint_pose_2D.cpp} (84%)
 rename src/test/{gtest_constraint_fix_3D.cpp => gtest_constraint_pose_3D.cpp} (87%)

diff --git a/src/capture_fix.cpp b/src/capture_fix.cpp
index e502f4c42..7b3c9106f 100644
--- a/src/capture_fix.cpp
+++ b/src/capture_fix.cpp
@@ -23,9 +23,9 @@ void CaptureFix::emplaceFeatureAndConstraint()
 
     // Emplace constraint
     if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
-        feature_fix->addConstraint(std::make_shared<ConstraintFix>(feature_fix));
+        feature_fix->addConstraint(std::make_shared<ConstraintPose2D>(feature_fix));
     else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
-        feature_fix->addConstraint(std::make_shared<ConstraintFix3D>(feature_fix));
+        feature_fix->addConstraint(std::make_shared<ConstraintPose3D>(feature_fix));
     else
         throw std::runtime_error("Wrong data size in CaptureFix. Use 3 for 2D. Use 7 for 3D.");
 }
diff --git a/src/capture_fix.h b/src/capture_fix.h
index b399c3288..f9e666083 100644
--- a/src/capture_fix.h
+++ b/src/capture_fix.h
@@ -4,8 +4,8 @@
 //Wolf includes
 #include "capture_base.h"
 #include "feature_fix.h"
-#include "constraint_fix.h"
-#include "constraint_fix_3D.h"
+#include "constraint_pose_2D.h"
+#include "constraint_pose_3D.h"
 
 //std includes
 //
diff --git a/src/constraint_fix.h b/src/constraint_pose_2D.h
similarity index 67%
rename from src/constraint_fix.h
rename to src/constraint_pose_2D.h
index eb9221365..292d021fe 100644
--- a/src/constraint_fix.h
+++ b/src/constraint_pose_2D.h
@@ -1,6 +1,6 @@
 
-#ifndef CONSTRAINT_FIX_H_
-#define CONSTRAINT_FIX_H_
+#ifndef CONSTRAINT_POSE_2D_H_
+#define CONSTRAINT_POSE_2D_H_
 
 //Wolf includes
 #include "constraint_autodiff.h"
@@ -12,20 +12,20 @@
 
 namespace wolf {
     
-WOLF_PTR_TYPEDEFS(ConstraintFix);
+WOLF_PTR_TYPEDEFS(ConstraintPose2D);
 
 //class
-class ConstraintFix: public ConstraintAutodiff<ConstraintFix,3,2,1>
+class ConstraintPose2D: public ConstraintAutodiff<ConstraintPose2D,3,2,1>
 {
     public:
-        ConstraintFix(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-                ConstraintAutodiff<ConstraintFix, 3, 2, 1>(CTR_FIX, nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+        ConstraintPose2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+                ConstraintAutodiff<ConstraintPose2D, 3, 2, 1>(CTR_POSE_2D, nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
         {
             setType("FIX");
-//            std::cout << "created ConstraintFix " << std::endl;
+//            std::cout << "created ConstraintPose2D " << std::endl;
         }
 
-        virtual ~ConstraintFix() = default;
+        virtual ~ConstraintPose2D() = default;
 
         template<typename T>
         bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
@@ -43,7 +43,7 @@ class ConstraintFix: public ConstraintAutodiff<ConstraintFix,3,2,1>
 };
 
 template<typename T>
-inline bool ConstraintFix::operator ()(const T* const _p, const T* const _o, T* _residuals) const
+inline bool ConstraintPose2D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
 {
     // measurement
     Eigen::Matrix<T,3,1> meas =  getMeasurement().cast<T>();
@@ -74,8 +74,8 @@ inline bool ConstraintFix::operator ()(const T* const _p, const T* const _o, T*
 //    J.row(2) = ((Jet<Scalar, 3>)(res(2))).v;
 //    if (sizeof(er(0)) != sizeof(double))
 //    {
-//        std::cout << "ConstraintFix::Jacobian(c" << id() << ") = \n " << J << std::endl;
-//        std::cout << "ConstraintFix::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+//        std::cout << "ConstraintPose2D::Jacobian(c" << id() << ") = \n " << J << std::endl;
+//        std::cout << "ConstraintPose2D::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
 //        std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() << std::endl;
 //    }
     ////////////////////////////////////////////////////////
diff --git a/src/constraint_fix_3D.h b/src/constraint_pose_3D.h
similarity index 61%
rename from src/constraint_fix_3D.h
rename to src/constraint_pose_3D.h
index c9c7b8b59..d0132756a 100644
--- a/src/constraint_fix_3D.h
+++ b/src/constraint_pose_3D.h
@@ -1,6 +1,6 @@
 
-#ifndef CONSTRAINT_FIX_3D_H_
-#define CONSTRAINT_FIX_3D_H_
+#ifndef CONSTRAINT_POSE_3D_H_
+#define CONSTRAINT_POSE_3D_H_
 
 //Wolf includes
 #include "constraint_autodiff.h"
@@ -10,20 +10,20 @@
 
 namespace wolf {
 
-WOLF_PTR_TYPEDEFS(ConstraintFix3D);
+WOLF_PTR_TYPEDEFS(ConstraintPose3D);
 
 //class
-class ConstraintFix3D: public ConstraintAutodiff<ConstraintFix3D,6,3,4>
+class ConstraintPose3D: public ConstraintAutodiff<ConstraintPose3D,6,3,4>
 {
     public:
 
-        ConstraintFix3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-            ConstraintAutodiff<ConstraintFix3D,6,3,4>(CTR_FIX_3D, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+        ConstraintPose3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+            ConstraintAutodiff<ConstraintPose3D,6,3,4>(CTR_POSE_3D, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
         {
             setType("FIX3D");
         }
 
-        virtual ~ConstraintFix3D() = default;
+        virtual ~ConstraintPose3D() = default;
 
         template<typename T>
         bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
@@ -36,7 +36,7 @@ class ConstraintFix3D: public ConstraintAutodiff<ConstraintFix3D,6,3,4>
 };
 
 template<typename T>
-inline bool ConstraintFix3D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
+inline bool ConstraintPose3D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
 {
 
     // states
diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
index 3c7ce8403..cc3845909 100644
--- a/src/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -14,12 +14,12 @@
 #include "processor_odom_3D.h"
 
 //Constraints headers
-#include "constraint_fix_3D.h"
 #include "constraint_fix_bias.h"
 
 //std
 #include <iostream>
 #include <fstream>
+#include "../constraint_pose_3D.h"
 
 #define OUTPUT_RESULTS
 //#define ADD_KF3
@@ -183,7 +183,7 @@ int main(int argc, char** argv)
     featureFix_cov(5,5) = pow( .01 , 2); // yaw variance
     CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
     FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
-    ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintFix3D>(featureFix->addConstraint(std::make_shared<ConstraintFix3D>(featureFix)));
+    ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintPose3D>(featureFix->addConstraint(std::make_shared<ConstraintPose3D>(featureFix)));
 
     Eigen::MatrixXs featureFixBias_cov(6,6);
     featureFixBias_cov = Eigen::MatrixXs::Identity(6,6); 
diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
index a74771426..660c6a240 100644
--- a/src/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -14,12 +14,12 @@
 #include "processor_odom_3D.h"
 
 //Constraints headers
-#include "constraint_fix_3D.h"
 #include "constraint_fix_bias.h"
 
 //std
 #include <iostream>
 #include <fstream>
+#include "../constraint_pose_3D.h"
 
 #define OUTPUT_RESULTS
 //#define AUTO_KFS
@@ -198,7 +198,7 @@ int main(int argc, char** argv)
     featureFix_cov(5,5) = pow( .001 , 2); // yaw variance
     CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
     FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
-    ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintFix3D>(featureFix->addConstraint(std::make_shared<ConstraintFix3D>(featureFix)));
+    ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintPose3D>(featureFix->addConstraint(std::make_shared<ConstraintPose3D>(featureFix)));
 
     Eigen::MatrixXs featureFixBias_cov(6,6);
     featureFixBias_cov = Eigen::MatrixXs::Identity(6,6); 
diff --git a/src/solver/qr_solver.h b/src/solver/qr_solver.h
index d8d097fb7..493ee2dc8 100644
--- a/src/solver/qr_solver.h
+++ b/src/solver/qr_solver.h
@@ -15,9 +15,6 @@
 //Wolf includes
 #include "state_block.h"
 #include "../constraint_sparse.h"
-#include "../constraint_fix.h"
-//#include "../constraint_gps_2D.h"
-//#include "../constraint_gps_pseudorange_2D.h"
 #include "../constraint_odom_2D.h"
 #include "../constraint_corner_2D.h"
 #include "../constraint_container.h"
@@ -31,6 +28,7 @@
 #include <eigen3/Eigen/OrderingMethods>
 #include <eigen3/Eigen/SparseQR>
 #include <Eigen/StdVector>
+#include "../constraint_pose_2D.h"
 
 namespace wolf
 {
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 6014f3530..579035b17 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -94,18 +94,18 @@ target_link_libraries(gtest_trajectory ${PROJECT_NAME})
 wolf_add_gtest(gtest_constraint_absolute gtest_constraint_absolute.cpp)
 target_link_libraries(gtest_constraint_absolute ${PROJECT_NAME})
 
-# ConstraintFix3D class test
-wolf_add_gtest(gtest_constraint_fix_3D gtest_constraint_fix_3D.cpp)
-target_link_libraries(gtest_constraint_fix_3D ${PROJECT_NAME})
-
-# ConstraintFix class test
-wolf_add_gtest(gtest_constraint_fix gtest_constraint_fix.cpp)
-target_link_libraries(gtest_constraint_fix ${PROJECT_NAME})
-
 # ConstraintOdom3D class test
 wolf_add_gtest(gtest_constraint_odom_3D gtest_constraint_odom_3D.cpp)
 target_link_libraries(gtest_constraint_odom_3D ${PROJECT_NAME})
 
+# ConstraintPose2D class test
+wolf_add_gtest(gtest_constraint_pose_2D gtest_constraint_pose_2D.cpp)
+target_link_libraries(gtest_constraint_pose_2D ${PROJECT_NAME})
+
+# ConstraintPose3D class test
+wolf_add_gtest(gtest_constraint_pose_3D gtest_constraint_pose_3D.cpp)
+target_link_libraries(gtest_constraint_pose_3D ${PROJECT_NAME})
+
 # FeatureIMU test
 wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
 target_link_libraries(gtest_feature_imu ${PROJECT_NAME})
diff --git a/src/test/gtest_constraint_imu.cpp b/src/test/gtest_constraint_imu.cpp
index cfc0ade33..efa1647b7 100644
--- a/src/test/gtest_constraint_imu.cpp
+++ b/src/test/gtest_constraint_imu.cpp
@@ -11,13 +11,12 @@
 #include "processor_imu.h"
 #include "processor_odom_3D.h"
 #include "ceres_wrapper/ceres_manager.h"
-#include "constraint_fix_3D.h"
-
 #include "utils_gtest.h"
 #include "../src/logging.h"
 
 #include <iostream>
 #include <fstream>
+#include "../constraint_pose_3D.h"
 
 //#define GET_RESIDUALS
 
@@ -2466,7 +2465,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1
     featureFix_cov(5,5) = 0.1;
     CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
     FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
-    ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintFix3D>(ffix->addConstraint(std::make_shared<ConstraintFix3D>(ffix)));
+    ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintPose3D>(ffix->addConstraint(std::make_shared<ConstraintPose3D>(ffix)));
     
     //prepare problem for solving
     origin_KF->getPPtr()->fix();
@@ -2524,7 +2523,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V
     featureFix_cov(5,5) = 0.1;
     CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
     FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
-    ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintFix3D>(ffix->addConstraint(std::make_shared<ConstraintFix3D>(ffix)));
+    ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintPose3D>(ffix->addConstraint(std::make_shared<ConstraintPose3D>(ffix)));
     
     //prepare problem for solving
     origin_KF->getPPtr()->fix();
diff --git a/src/test/gtest_constraint_fix.cpp b/src/test/gtest_constraint_pose_2D.cpp
similarity index 84%
rename from src/test/gtest_constraint_fix.cpp
rename to src/test/gtest_constraint_pose_2D.cpp
index 7e90af10c..220ead9cf 100644
--- a/src/test/gtest_constraint_fix.cpp
+++ b/src/test/gtest_constraint_pose_2D.cpp
@@ -1,14 +1,14 @@
 /**
- * \file gtest_constraint_fix.cpp
+ * \file gtest_constraint_pose_2D.cpp
  *
  *  Created on: Mar 30, 2017
  *      \author: jsola
  */
 
 
+#include "../constraint_pose_2D.h"
 #include "utils_gtest.h"
 
-#include "constraint_fix.h"
 #include "capture_motion.h"
 
 #include "ceres_wrapper/ceres_manager.h"
@@ -37,11 +37,11 @@ FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeS
 // Capture, feature and constraint from frm1 to frm0
 CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>(0, nullptr, pose, 3, 3, nullptr));
 FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov));
-ConstraintFixPtr ctr0 = std::static_pointer_cast<ConstraintFix>(fea0->addConstraint(std::make_shared<ConstraintFix>(fea0)));
+ConstraintPose2DPtr ctr0 = std::static_pointer_cast<ConstraintPose2D>(fea0->addConstraint(std::make_shared<ConstraintPose2D>(fea0)));
 
 ////////////////////////////////////////////////////////
 
-TEST(ConstraintFix, check_tree)
+TEST(ConstraintPose2D, check_tree)
 {
     ASSERT_TRUE(problem->check(0));
 }
@@ -51,7 +51,7 @@ TEST(ConstraintFix, check_tree)
 //    ASSERT_EIGEN_APPROX(ctr0->expectation() , delta);
 //}
 
-TEST(ConstraintFix, solve)
+TEST(ConstraintPose2D, solve)
 {
 
     // Fix frame 0, perturb frm1
diff --git a/src/test/gtest_constraint_fix_3D.cpp b/src/test/gtest_constraint_pose_3D.cpp
similarity index 87%
rename from src/test/gtest_constraint_fix_3D.cpp
rename to src/test/gtest_constraint_pose_3D.cpp
index 76b13526c..a22d361c8 100644
--- a/src/test/gtest_constraint_fix_3D.cpp
+++ b/src/test/gtest_constraint_pose_3D.cpp
@@ -6,9 +6,9 @@
  */
 
 
+#include "../constraint_pose_3D.h"
 #include "utils_gtest.h"
 
-#include "constraint_fix_3D.h"
 #include "capture_motion.h"
 
 #include "ceres_wrapper/ceres_manager.h"
@@ -44,12 +44,12 @@ FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeS
 // Capture, feature and constraint
 CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>(0, nullptr, pose7, 7, 6, nullptr));
 FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov));
-ConstraintFix3DPtr ctr0 = std::static_pointer_cast<ConstraintFix3D>(fea0->addConstraint(std::make_shared<ConstraintFix3D>(fea0)));
+ConstraintPose3DPtr ctr0 = std::static_pointer_cast<ConstraintPose3D>(fea0->addConstraint(std::make_shared<ConstraintPose3D>(fea0)));
 
 
 ////////////////////////////////////////////////////////
 
-TEST(ConstraintFix3D, check_tree)
+TEST(ConstraintPose3D, check_tree)
 {
     ASSERT_TRUE(problem->check(0));
 }
@@ -59,7 +59,7 @@ TEST(ConstraintFix3D, check_tree)
 //    ASSERT_EIGEN_APPROX(ctr0->expectation() , delta);
 //}
 
-TEST(ConstraintFix3D, solve)
+TEST(ConstraintPose3D, solve)
 {
 
     // Fix frame 0, perturb frm1
diff --git a/src/wolf.h b/src/wolf.h
index 2978cc603..24ded0a36 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -212,8 +212,8 @@ typedef enum
     CTR_GPS_FIX_2D = 1,         ///< 2D GPS Fix constraint.
     CTR_GPS_PR_2D,              ///< 2D GPS Pseudorange constraint.
     CTR_GPS_PR_3D,              ///< 3D GPS Pseudorange constraint.
-    CTR_FIX,                    ///< Fix constraint (for priors).
-    CTR_FIX_3D,                 ///< Fix constraint (for priors) in 3D.
+    CTR_POSE_2D,                ///< Pose constraint (for priors) in 2D.
+    CTR_POSE_3D,                ///< Pose constraint (for priors) in 3D.
     CTR_FIX_BIAS,               ///< Fix constraint (for priors) on bias.
     CTR_ODOM_2D,                ///< 2D Odometry constraint .
     CTR_ODOM_3D,                ///< 3D Odometry constraint .
-- 
GitLab