arpg · martinruenz · Mar 29, 2019 · Mar 29, 2019 · Mar 29, 2019 · Mar 29, 2019
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -118,7 +118,7 @@ SET(SOURCES
 ## Find required dependencies
 find_package( Eigen3 REQUIRED )
 find_package( Sophus REQUIRED )
-find_package( GLog REQUIRED )
+find_package( GLog CONFIG REQUIRED )
 
 find_package(TinyXML2 REQUIRED)
 list(APPEND CALIBU_INC ${TinyXML2_INCLUDE_DIRS} )
@@ -192,7 +192,7 @@ endif()
 
 # build calibu library
 add_library( calibu ${SOURCES} )
-target_link_libraries( calibu ${LINK_LIBS} )
+target_link_libraries( calibu ${LINK_LIBS} glog::glog)
 
 # install everything
 install_package(
@@ -219,4 +219,3 @@ endif()
  add_custom_target(uninstall
      COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake)
  ###########################
-
diff --git a/cmake_modules/FindGLog.cmake b/cmake_modules/FindGLog.cmake
diff --git a/include/calibu/calib/ReprojectionCost.h b/include/calibu/calib/ReprojectionCost.h
@@ -46,8 +46,8 @@ struct ReprojectionCost
     bool Evaluate(T const* const* parameters, T* residuals) const
     {
         Eigen::Map<Eigen::Matrix<T,2,1> > r(residuals);
-        const Eigen::Map<const Sophus::SE3Group<T> > T_kw(parameters[0]);
-        const Eigen::Map<const Sophus::SE3Group<T> > T_ck(parameters[1]);
+        const Eigen::Map<const Sophus::SE3<T> > T_kw(parameters[0]);
+        const Eigen::Map<const Sophus::SE3<T> > T_ck(parameters[1]);
         T const* camparam = parameters[2];
 
         const Eigen::Matrix<T,3,1> Pc = T_ck * (T_kw * m_Pw.cast<T>());

diff --git a/include/calibu/calib/ReprojectionCostFunctor.h b/include/calibu/calib/ReprojectionCostFunctor.h
@@ -48,8 +48,8 @@ struct ReprojectionCostFunctor
             ) const
     {
         Eigen::Map<Eigen::Matrix<T,2,1> > r(residuals);
-        const Eigen::Map<const Sophus::SE3Group<T> > T_kw(pT_kw);
-        const Eigen::Map<const Sophus::SE3Group<T> > T_ck(pT_ck);
+        const Eigen::Map<const Sophus::SE3<T> > T_kw(pT_kw);
+        const Eigen::Map<const Sophus::SE3<T> > T_ck(pT_ck);
 
         const Eigen::Matrix<T,3,1> Pc = T_ck * (T_kw * m_Pw.cast<T>());
         Eigen::Matrix<T,2,1> pc;

diff --git a/include/calibu/cam/camera_crtp.h b/include/calibu/cam/camera_crtp.h
@@ -40,7 +40,7 @@ class CameraInterface {
   typedef Eigen::Matrix<Scalar, 2, 1> Vec2t;
   typedef Eigen::Matrix<Scalar, 3, 1> Vec3t;
   typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VecXt;
-  typedef Sophus::SE3Group<Scalar> SE3t;
+  typedef Sophus::SE3<Scalar> SE3t;
 
 public:
   CameraInterface() {}
@@ -50,6 +50,9 @@ class CameraInterface {
 
   virtual ~CameraInterface() {}
 
+  /** Clone a camera model, useful when camera type is not known at compile time */
+  virtual std::shared_ptr<CameraInterface<Scalar>> Clone() = 0;
+
   /** Change camera model image size. */
   virtual void Scale( const Scalar& s ) = 0;
 

diff --git a/include/calibu/cam/camera_crtp_impl.h b/include/calibu/cam/camera_crtp_impl.h
@@ -48,10 +48,16 @@ class CameraImpl : public CameraInterface<Scalar> {
 
   CameraImpl() {}
   virtual ~CameraImpl() {}
-  CameraImpl(const Eigen::VectorXd& params, Eigen::Vector2i& image_size) :
+  CameraImpl(const Eigen::VectorXd& params, const Eigen::Vector2i& image_size) :
       CameraInterface<Scalar>(params, image_size) {
   }
 
+  /** Clone a camera model, useful when camera type is not known at compile time */
+  virtual std::shared_ptr<CameraInterface<Scalar>>
+  Clone() {
+      return std::make_shared<Derived>(static_cast<const Derived&>(*this));
+  };
+
   void
   Scale(const Scalar& s) override {
     Derived::Scale( s, this->params_.data() );

diff --git a/include/calibu/cam/camera_models_kb4.h b/include/calibu/cam/camera_models_kb4.h
@@ -41,6 +41,7 @@ class KannalaBrandtCamera : public CameraImpl<Scalar, 8, KannalaBrandtCamera<Sca
   typedef CameraImpl<Scalar, 8, KannalaBrandtCamera<Scalar> > Base;
  public:
   using Base::Base;
+  using Base::Scale, Base::K, Base::Unproject, Base::Project, Base::dProject_dparams, Base::dUnproject_dparams, Base::dProject_dray;
 
   static constexpr int NumParams = 8;
 

diff --git a/include/calibu/cam/camera_models_poly.h b/include/calibu/cam/camera_models_poly.h
@@ -38,6 +38,7 @@ class FovCamera : public CameraImpl<Scalar, 5, FovCamera<Scalar> > {
   typedef CameraImpl<Scalar, 5, FovCamera<Scalar> > Base;
  public:
   using Base::Base;
+  using Base::Scale, Base::K, Base::Unproject, Base::Project, Base::dProject_dparams, Base::dUnproject_dparams, Base::dProject_dray;
 
   static constexpr int NumParams = 5;
 

diff --git a/include/calibu/cam/camera_models_rational.h b/include/calibu/cam/camera_models_rational.h
@@ -41,6 +41,7 @@ class Rational6Camera : public CameraImpl<Scalar, 10, Rational6Camera<Scalar> >
   typedef CameraImpl<Scalar, 10, Rational6Camera<Scalar> > Base;
  public:
   using Base::Base;
+  using Base::Scale, Base::K, Base::Unproject, Base::Project, Base::dProject_dparams, Base::dUnproject_dparams, Base::dProject_dray;
 
   static constexpr int NumParams = 10;
 

diff --git a/include/calibu/cam/camera_rig.h b/include/calibu/cam/camera_rig.h
@@ -41,12 +41,12 @@ static const Sophus::SO3d RdfRobotics =
 
 // T_2b_1b = T_ba * T_2a_1a * T_ab
 template<typename Scalar=double>
-inline Sophus::SE3Group<Scalar> ToCoordinateConvention(
-        const Sophus::SE3Group<Scalar>& T_2a_1a,
-        const Sophus::SO3Group<Scalar>& R_ba
+inline Sophus::SE3<Scalar> ToCoordinateConvention(
+        const Sophus::SE3<Scalar>& T_2a_1a,
+        const Sophus::SO3<Scalar>& R_ba
         )
 {
-    Sophus::SE3Group<Scalar> T_2b_1b;
+    Sophus::SE3<Scalar> T_2b_1b;
     T_2b_1b.so3() = R_ba * T_2a_1a.so3() * R_ba.inverse();
     T_2b_1b.translation() = R_ba * T_2a_1a.translation();
     return T_2b_1b;
@@ -55,14 +55,14 @@ inline Sophus::SE3Group<Scalar> ToCoordinateConvention(
 template<typename Scalar=double>
 inline std::shared_ptr<Rig<double>> ToCoordinateConvention(
     const std::shared_ptr<Rig<double>>& rig,
-    const Sophus::SO3Group<Scalar>& rdf
+    const Sophus::SO3<Scalar>& rdf
   )
 {
   std::shared_ptr<calibu::Rig<Scalar>> ret(new calibu::Rig<Scalar>());
   *ret = *rig;
   for(size_t c=0; c<ret->NumCams(); ++c) {
-    const Sophus::SO3Group<Scalar> M =
-        rdf * Sophus::SO3Group<Scalar>(rig->cameras_[c]->RDF()).inverse();
+    const Sophus::SO3<Scalar> M =
+        rdf * Sophus::SO3<Scalar>(rig->cameras_[c]->RDF()).inverse();
     ret->cameras_[c]->SetPose(ToCoordinateConvention(rig->cameras_[c]->Pose(), M));
     ret->cameras_[c]->SetRDF(rdf.matrix());
   }

diff --git a/include/calibu/cam/camera_utils.h b/include/calibu/cam/camera_utils.h
@@ -25,6 +25,9 @@
 */
 
 #pragma once
+
+#include <calibu/cam/camera_crtp_impl.h>
+
 namespace calibu {
 struct CameraUtils {
   /** Euclidean distance from (0, 0) to given pixel */
@@ -148,6 +151,7 @@ class LinearCamera : public CameraImpl<Scalar, 4, LinearCamera<Scalar> > {
   typedef CameraImpl<Scalar, 4, LinearCamera<Scalar> > Base;
  public:
   using Base::Base;
+  using Base::Scale, Base::K, Base::Unproject, Base::Project, Base::dProject_dparams, Base::dUnproject_dparams, Base::dProject_dray;
 
   static constexpr int NumParams = 4;
 
@@ -211,4 +215,3 @@ class LinearCamera : public CameraImpl<Scalar, 4, LinearCamera<Scalar> > {
 };
 
 }//end namespace calibu
-
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
@@ -61,11 +61,7 @@ SET(SOURCES
 ## Find required dependencies
 
 find_package( Eigen3 REQUIRED )
-
-find_package( GLog REQUIRED )
-set(HAVE_GLOG 1)
-list( APPEND LINK_LIBS ${GLog_LIBRARIES} )
-list( APPEND CALIBU_INC ${Glog_INCLUDE_DIRS} )
+find_package( GLog CONFIG REQUIRED )
 
 # https://github.com/gwu-robotics/Sophus.git
 find_package( Sophus REQUIRED )

diff --git a/src/target/TargetGridDot.cpp b/src/target/TargetGridDot.cpp
@@ -359,16 +359,16 @@ bool TargetGridDot::Match(std::map<Eigen::Vector2i const, Vertex*,
 //        if( num_matches == 1 )
         {
             // Found unique match
-            Sophus::SE2Group<int> T_0x[4] = {
-                Sophus::SE2Group<int>(Sophus::SO2Group<int>(1,0), Eigen::Vector2i(0,0) ),
-                Sophus::SE2Group<int>(Sophus::SO2Group<int>(0,1), Eigen::Vector2i(grid_size_[0]-1,0) ),
-                Sophus::SE2Group<int>(Sophus::SO2Group<int>(-1,0), Eigen::Vector2i(grid_size_[0]-1,grid_size_[1]-1) ),
-                Sophus::SE2Group<int>(Sophus::SO2Group<int>(0,-1), Eigen::Vector2i(0,grid_size_[1]-1) )
+            Sophus::SE2<int> T_0x[4] = {
+                Sophus::SE2<int>(Sophus::SO2<int>(1,0), Eigen::Vector2i(0,0) ),
+                Sophus::SE2<int>(Sophus::SO2<int>(0,1), Eigen::Vector2i(grid_size_[0]-1,0) ),
+                Sophus::SE2<int>(Sophus::SO2<int>(-1,0), Eigen::Vector2i(grid_size_[0]-1,grid_size_[1]-1) ),
+                Sophus::SE2<int>(Sophus::SO2<int>(0,-1), Eigen::Vector2i(0,grid_size_[1]-1) )
             };
 
-            Sophus::SE2Group<int> T_xm(Sophus::SO2Group<int>(), Eigen::Vector2i(bc,br));
+            Sophus::SE2<int> T_xm(Sophus::SO2<int>(), Eigen::Vector2i(bc,br));
 
-            Sophus::SE2Group<int> T_0m = T_0x[bg] * T_xm;
+            Sophus::SE2<int> T_0m = T_0x[bg] * T_xm;
 
             for(auto i = obs.begin(); i != obs.end(); ++i) {
                 i->second->pg = T_0m * i->second->pg;