1 files changed, 121 insertions, 0 deletions
diff --git a/meowpp.test/src/autostitch_RansacCheck.cpp b/meowpp.test/src/autostitch_RansacCheck.cpp
new file mode 100644
index 0000000..0410396
--- /dev/null
+++ b/meowpp.test/src/autostitch_RansacCheck.cpp
@@ -0,0 +1,121 @@
+#include "autostitch.h"
+
+#include "meowpp/math/Matrix.h"
+#include "meowpp/math/Vector.h"
+#include <utility>
+
+using namespace meow;
+
+double MyRansacCheck::threshold = 5.0;
+
+meow::Usage MyRansacCheck::usage(){
+  Usage usg;
+  usg.optionAdd('t',
+                "Threshold for RANSAC",
+                "<floating point>", stringPrintf("%.10f", threshold),
+                false);
+  return usg;
+}
+
+bool MyRansacCheck::usage(Usage const& usg){
+  threshold = inRange(0.0000001, 1000.0,
+                      atof(usg.optionValue('t', 0).c_str()));
+  return true;
+}
+
+MyRansacCheck::MyRansacCheck(){
+}
+
+
+MyRansacCheck::MyRansacCheck(MyRansacCheck const& __rc):
+_from(__rc._from),
+_to(__rc._to){
+}
+
+
+MyRansacCheck::MyRansacCheck(std::vector<Vector<double> > const* __from,
+                             std::vector<Vector<double> > const* __to):
+_from(__from),
+_to(__to){
+}
+
+
+MyRansacCheck::~MyRansacCheck(){
+}
+
+
+std::pair<Vector3D<double>, Vector3D<double> > MyRansacCheck::vCalc(
+  std::vector<FeaturePointIndexPair> const& __sample
+) const{
+  Matrix<double> m(6, 7, 0.0);
+  for(size_t i = 0; i < 3u; i++){
+    m(i * 2    , 0, (*_from)[__sample[i].from.second](0));
+    m(i * 2    , 1, (*_from)[__sample[i].from.second](1));
+    m(i * 2    , 2, 1.0);
+    m(i * 2    , 6, (*_to)[__sample[i].to.second](0));
+    m(i * 2 + 1, 3, (*_from)[__sample[i].from.second](0));
+    m(i * 2 + 1, 4, (*_from)[__sample[i].from.second](1));
+    m(i * 2 + 1, 5, 1.0);
+    m(i * 2 + 1, 6, (*_to)[__sample[i].to.second](1));
+  }
+  m.triangulared();
+  Vector<double> x(6, 0.0);
+  for(ssize_t i = 5; i >= 0; i--){
+    double sum = 0;
+    for(size_t j = i + 1; j < 6u; j++){
+      sum += x(j) * m(i, j);
+    }
+    x.entry(i, (m(i, 6) - sum) / m(i, i));
+  }
+  Vector3D<double> vX(x(0), x(1), x(2));
+  Vector3D<double> vY(x(3), x(4), x(5));
+  return std::pair<Vector3D<double>, Vector3D<double> >(vX, vY);
+}
+
+
+void MyRansacCheck::rememberVCalc(std::vector<FeaturePointIndexPair>
+                                  const& __sample){
+  std::pair<Vector3D<double>, Vector3D<double> > p(vCalc(__sample));
+  _vX = p.first;
+  _vY = p.second;
+}
+
+
+bool MyRansacCheck::ok(FeaturePointIndexPair const& __m) const{
+  Vector2D<double> from(
+    (*_from)[__m.from.second](0),
+    (*_from)[__m.from.second](1));
+  Vector2D<double> me(
+    (*_to)[__m.to.second](0),
+    (*_to)[__m.to.second](1));
+  Vector2D<double> me2(to(from));
+  return ((me - me2).length2() <= threshold);
+}
+
+
+double MyRansacCheck::operator()(std::vector<FeaturePointIndexPair>
+                                 const& __sample,
+                                 std::vector<FeaturePointIndexPair>
+                                 const& __data) const{
+  for(size_t i = 0, I = __sample.size(); i < I; i++){
+    for(size_t j = 0, J = __sample.size(); j < J; j++){
+      if(i == j) continue;
+      if(__sample[i].from.second == __sample[j].from.second) return -1;
+      if(__sample[i].to  .second == __sample[j].to  .second) return -1;
+    }
+  }
+  ((MyRansacCheck*)this)->rememberVCalc(__sample);
+  size_t ret = 0;
+  for(size_t i = 0, I = __data.size(); i < I; i++){
+    if(ok(__data[i])){
+      ret++;
+    }
+  }
+  return 0.001 + ret;
+}
+
+
+Vector2D<double> MyRansacCheck::to(Vector2D<double> const& __v) const{
+  Vector3D<double> v(__v(0), __v(1), 1);
+  return Vector2D<double>(v.dot(_vX), v.dot(_vY));
+}