x

1 files changed, 68 insertions, 35 deletions

diff --git a/meowpp.test/src/autostitch_RansacCheck.cpp b/meowpp.test/src/autostitch_RansacCheck.cpp
index 0410396..becc7da 100644
--- a/meowpp.test/src/autostitch_RansacCheck.cpp
+++ b/meowpp.test/src/autostitch_RansacCheck.cpp
@@ -3,27 +3,30 @@
 #include "meowpp/math/Matrix.h"
 #include "meowpp/math/Vector.h"
 #include <utility>
+#include <cmath>
+#include <algorithm>
+
 
 using namespace meow;
 
 double MyRansacCheck::threshold = 5.0;
 
-meow::Usage MyRansacCheck::usage(){
+meow::Usage MyRansacCheck::usage() {
   Usage usg;
-  usg.optionAdd('t',
+  usg.optionAdd("ransac-threshold",
                 "Threshold for RANSAC",
                 "<floating point>", stringPrintf("%.10f", threshold),
                 false);
   return usg;
 }
 
-bool MyRansacCheck::usage(Usage const& usg){
+bool MyRansacCheck::usage(Usage const& usg) {
   threshold = inRange(0.0000001, 1000.0,
-                      atof(usg.optionValue('t', 0).c_str()));
+                      atof(usg.optionValue("ransac-threshold", 0).c_str()));
   return true;
 }
 
-MyRansacCheck::MyRansacCheck(){
+MyRansacCheck::MyRansacCheck() {
 }
 
 
@@ -44,44 +47,58 @@ 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++){
+Vector<double> MyRansacCheck::vCalc(std::vector<FeaturePointIndexPair> const& __sample) const {
+  Matrix<double> m(__sample.size() * 2, 9, 0.0);
+  for (size_t i = 0, I = __sample.size(); i < I; ++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    , 6, -(*_from)[__sample[i].from.second](0) * (*_to)[__sample[i].to.second](0));
+    m(i * 2    , 7, -(*_from)[__sample[i].from.second](1) * (*_to)[__sample[i].to.second](0));
+    m(i * 2    , 8,                                         (*_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(i * 2 + 1, 6, -(*_from)[__sample[i].from.second](0) * (*_to)[__sample[i].to.second](1));
+    m(i * 2 + 1, 7, -(*_from)[__sample[i].from.second](1) * (*_to)[__sample[i].to.second](1));
+    m(i * 2 + 1, 8,                                         (*_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);
+  if (__sample.size() == 4) {
+    m.triangulared();
+    Vector<double> x(8, 0.0);
+    for (ssize_t i = 7; i >= 0; i--) {
+      double sum = 0;
+      for (size_t j = i + 1; j < 8u; j++) {
+        sum += x(j) * m(i, j);
+      }
+      x.entry(i, (m(i, 8) - sum) / m(i, i));
     }
-    x.entry(i, (m(i, 6) - sum) / m(i, i));
+    return x;
+  }
+  else {
+    Matrix<double> b(m.col(8));
+    m.cols(8, 0.0);
+    Vector<double> v((m.transpose() * m).inverse() * m.transpose() * b);
+    return v;
   }
-  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;
+                                  const& __sample) {
+  Vector<double> x(vCalc(__sample));
+  a_ = x(0);
+  b_ = x(1);
+  c_ = x(2);
+  d_ = x(3);
+  e_ = x(4);
+  f_ = x(5);
+  A_ = x(6);
+  B_ = x(7);
 }
 
 
-bool MyRansacCheck::ok(FeaturePointIndexPair const& __m) const{
+bool MyRansacCheck::ok(FeaturePointIndexPair const& __m) const {
   Vector2D<double> from(
     (*_from)[__m.from.second](0),
     (*_from)[__m.from.second](1));
@@ -96,9 +113,9 @@ bool MyRansacCheck::ok(FeaturePointIndexPair const& __m) const{
 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++){
+                                 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;
@@ -106,16 +123,32 @@ double MyRansacCheck::operator()(std::vector<FeaturePointIndexPair>
   }
   ((MyRansacCheck*)this)->rememberVCalc(__sample);
   size_t ret = 0;
-  for(size_t i = 0, I = __data.size(); i < I; i++){
-    if(ok(__data[i])){
+  for (size_t i = 0, I = __data.size(); i < I; i++) {
+    if (ok(__data[i])) {
       ret++;
     }
   }
   return 0.001 + ret;
 }
 
+bool MyRansacCheck::check(double r, double a) {
+  return true;
+  Vector2D<double> v_x(a_, b_);
+  Vector2D<double> v_y(c_, d_);
+  double len1 = v_x.length() * r;
+  double len2 = v_x.length() / r;
+  double len = v_y.length();
+  if (len1 > len2) std::swap(len1, len2);
+  if (len < len1 || len2 < len) return false;
+  double sn = fabs(sin(a));
+  double msn = fabs(v_x.cross(v_y) / v_x.length() / v_y.length());
+  if (msn < sn) return false;
+  return true;
+}
 
-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));
+Vector2D<double> MyRansacCheck::to(Vector2D<double> const& v) const {
+  return Vector2D<double>(
+    (v.x() * a_ + v.y() * b_ + c_) / (A_ * v.x() + B_ * v.y() + 1),
+    (v.x() * d_ + v.y() * e_ + f_) / (A_ * v.x() + B_ * v.y() + 1)
+  );
 }