From 3147d3170992e96f5ff14ad7a894283e0f102586 Mon Sep 17 00:00:00 2001
From: Hui <hui.li@rvbust.com>
Date: Wed, 18 Jul 2018 17:06:19 +0800
Subject: [PATCH] verified the lib with random matrices. the lib works well.

---
 .../calib/HandEyeCalibration_test2.cc         | 69 +++++++++++++++++++
 src/camodocal/calib/Makefile                  | 20 ++++++
 2 files changed, 89 insertions(+)
 create mode 100644 src/camodocal/calib/HandEyeCalibration_test2.cc
 create mode 100644 src/camodocal/calib/Makefile

diff --git a/src/camodocal/calib/HandEyeCalibration_test2.cc b/src/camodocal/calib/HandEyeCalibration_test2.cc
new file mode 100644
index 0000000..fa4f18e
--- /dev/null
+++ b/src/camodocal/calib/HandEyeCalibration_test2.cc
@@ -0,0 +1,69 @@
+#include "HandEyeCalibration.h"
+#include <Eigen/Geometry>
+#include <Eigen/Dense>
+
+using namespace Eigen;
+
+#define DEBUG(msg) std::cout << msg << std::endl;
+
+
+/*
+-------------------------------------------------------------
+
+-------------------------------------------------------------
+*/
+
+template <typename Input>
+static Eigen::Vector3d
+ToEulerAngle(Input eigenQuat)
+{
+    Eigen::AngleAxisd ax3d(eigenQuat);
+    return ax3d.angle() * ax3d.axis();
+}
+
+static Isometry3d
+MakeRandomT()
+{
+    Isometry3d t;
+    Quaterniond q = Quaterniond::UnitRandom();
+    t = q;
+    t.translation() = Vector3d::Random();
+    return t; 
+}
+
+typedef std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >  Eigen3dVec;
+
+int
+main(int argc, char *argv[]) {
+    srand((unsigned int) time(0));
+    
+    Isometry3d X = MakeRandomT();
+    std::vector<Isometry3d> As(5), Bs(5);
+    for (size_t i = 0; i < As.size(); ++i)
+        As[i] = MakeRandomT();
+
+    Isometry3d Xinv = X.inverse();
+    for (size_t i = 0; i < As.size(); ++i)
+        Bs[i] = Xinv * As[i] * X;
+
+    // camodocal::HandEyeCalibration calib;
+    Eigen3dVec Ras, Tas, Rbs, Tbs;
+    
+    for (size_t i = 0; i < As.size(); ++i) {
+        Ras.push_back(ToEulerAngle(As[i].rotation()));
+        Tas.push_back(As[i].translation());
+        Rbs.push_back(ToEulerAngle(Bs[i].rotation()));
+        Tbs.push_back(Bs[i].translation());        
+    }
+
+    std::cout << X.matrix() << std::endl;
+    std::cout << Ras[0] << std::endl;
+    std::cout << Rbs[0] << std::endl;
+    std::cout << Tas[0] << std::endl;
+    
+    Eigen::Matrix4d X_estimated; 
+    camodocal::HandEyeCalibration::estimateHandEyeScrew(Ras, Tas, Rbs, Tbs, X_estimated);
+    std::cout << X_estimated - X.matrix() << std::endl;
+    return 0;
+}
+
diff --git a/src/camodocal/calib/Makefile b/src/camodocal/calib/Makefile
new file mode 100644
index 0000000..b4f233f
--- /dev/null
+++ b/src/camodocal/calib/Makefile
@@ -0,0 +1,20 @@
+all: test2
+
+CXX=g++
+# INCS+=-I../../ -I/home/rvbust/Rvbust/Install/Eigen/include/eigen3 \
+# 	-I/home/rvbust/Rvbust/Install/GFlags/include \
+# 	-I/home/rvbust/Rvbust/Install/GLog/include \
+# 	-I/home/rvbust/Rvbust/Install/CeresSolver/include
+
+
+INCS += -I../../ -I/home/rvbust/Rvbust/Install/OpenCV/include -L/home/rvbust/Rvbust/Install/OpenCV/lib -lopencv_core -lopencv_imgproc -lopencv_imgcodecs -lopencv_highgui -L/home/rvbust/Rvbust/Install/Miniconda3/lib -llapack -I/home/rvbust/Rvbust/Install/Eigen/include/eigen3 -I/home/rvbust/Rvbust/Sources/Pybind11/include -I/home/rvbust/Rvbust/Install/Miniconda3/include/python3.6m -L/home/rvbust/Rvbust/Install/Miniconda3/lib -I/home/rvbust/Rvbust/Install/StringFmt/include -lpython3.6m -I/home/rvbust/Rvbust/Install/CeresSolver/include -L/home/rvbust/Rvbust/Install/CeresSolver/lib -lceres -I/home/rvbust/Rvbust/Install/GLog/include -L/home/rvbust/Rvbust/Install/GLog/lib -lglog -I/home/rvbust/Rvbust/Install/GFlags/include -L/home/rvbust/Rvbust/Install/GFlags/lib -lgflags -lpthread -lgomp
+
+
+#LIBS+= -lglog -lgflags -lceres
+
+test2: HandEyeCalibration_test2.cc HandEyeCalibration.cc
+	$(CXX) -o $@ $^ $(INCS)
+
+clean:
+	rm -rf test2
+