Go to the documentation of this file.00001
00002 #include "HMatrix2DComputeMLESAC.h"
00003 #include <mvl/HomgOperator2D.h>
00004 #include <vgl/vgl_homg_point_2d.h>
00005 #include <vgl/algo/vgl_homg_operators_2d.h>
00006 #if 0
00007 #include <vnl/vnl_fastops.h>
00008 #include <vnl/vnl_inverse.h>
00009 #endif
00010
00011 HMatrix2DComputeMLESAC::HMatrix2DComputeMLESAC(double std)
00012 {
00013 std_ = std;
00014 }
00015
00016 HMatrix2DComputeMLESAC::~HMatrix2DComputeMLESAC() {}
00017
00018 double HMatrix2DComputeMLESAC::calculate_term(vcl_vector<double>& residuals, vcl_vector<bool>& inlier_list, int& count)
00019 {
00020 double inthresh = 5.99*std_*std_;
00021 double sse = 0.0;
00022 for (unsigned int i = 0; i < residuals.size(); i++) {
00023 if (residuals[i] < inthresh) {
00024 inlier_list[i] = true;
00025 sse += residuals[i];
00026 count++;
00027 } else {
00028 inlier_list[i] = false;
00029 sse += inthresh;
00030 }
00031 }
00032 return sse;
00033 }
00034
00035 double HMatrix2DComputeMLESAC::calculate_residual(vgl_homg_point_2d<double>& one,
00036 vgl_homg_point_2d<double>& two,
00037 HMatrix2D* H)
00038 {
00039 vnl_double_2 r;
00040 r[0] = vgl_homg_operators_2d<double>::distance_squared(H->transform_to_plane2(one), two);
00041 r[1] = vgl_homg_operators_2d<double>::distance_squared(H->transform_to_plane1(two), one);
00042 return r[0] + r[1];
00043 }
00044
00045 double HMatrix2DComputeMLESAC::calculate_residual(HomgPoint2D& one, HomgPoint2D& two, HMatrix2D* H)
00046 {
00047 vnl_double_2 r;
00048 r[0] = HomgOperator2D::distance_squared(H->transform_to_plane2(one), two);
00049 r[1] = HomgOperator2D::distance_squared(H->transform_to_plane1(two), one);
00050 #ifdef DEBUG
00051 vcl_cerr << "r[0] : " << r[0] << " r[1] : " << r[1] << vcl_endl;
00052 #endif
00053 #if 0
00054 vnl_double_2 p1 = one.get_double2();
00055 vnl_double_2 p2 = two.get_double2();
00056 vcl_cerr << H->transform_to_plane2(one) << " : " << p2 << " : " << r[0] << vcl_endl
00057 << H->transform_to_plane1(two) << " : " << p1 << " : " << r[1] << vcl_endl;
00058 if (r[0] < 100.0 && r[1] < 100.0)
00059 {
00060 vnl_double_3x3 const& mat = H->get_matrix();
00061 double t13 = - mat(2,0)*p1[0] - mat(2,1)*p1[1] - mat(2,2);
00062 double t24 = - mat(2,0)*p1[0] - mat(2,1)*p2[1] - mat(2,2);
00063 vnl_matrix<double> J(2, 4);
00064 J(0,0) = mat(0,0)-mat(2,0)*p2[0]; J(0,1) = mat(0,1)-mat(2,1)*p2[0]; J(0,2)= t13; J(0,3)= 0.0;
00065 J(1,0) = mat(1,0)-mat(2,0)*p2[1]; J(1,1) = mat(1,1)-mat(2,1)*p2[1]; J(1,2)= 0.0; J(1,3)= t24;
00066
00067 vnl_matrix<double> res(2,2);
00068 vnl_fastops::ABt(res, J, J);
00069 vnl_double_2 g = vnl_inverse(res)*r;
00070 return r[0]*g[0] + r[1]*g[1];
00071 }
00072 else
00073 #endif
00074 return r[0] + r[1];
00075 }
