BOOST_FIXTURE_TEST_CASE( math2d__norms, Some2dPoints )
{
BOOMST_CHECK_APPROX_EQUAL( 4_r, norm1(p2) );
BOOMST_CHECK_APPROX_EQUAL( 10_r, norm2sq(p2) );
BOOMST_CHECK_APPROX_EQUAL( 2_r, norm_max(p3) );
BOOMST_CHECK_APPROX_EQUAL( 0_r, norm_max(zero2) );
}
//Пусть эта штука будет рекурсивно себя вызывать и возвращать число итераций
int layer(double eps,int n,double tau,double h,double *v,double *hat_v){
double err,gamma,*a,*c,*d,*b,*w,*pts[5]; int i;
new_double(5,n,pts);
a = pts[0]; c = pts[1]; d = pts[2]; b = pts[3]; w = pts[4];
_G(n,b,tau,h,v,hat_v);
err = norm_max(n,b);
if (err<eps) printf("Ошибка %e\neps %e\n",norm_max(n,b),eps);
if (norm_max(n,b) < eps) {
clean_all(5,pts); return 0;
}
for(i=n-6,a[n-5] = 1/tau;i>=0;i--) {
a[i] = 1/tau;
c[i] = hat_v[i+3]/(2*h); d[i] = -hat_v[i+2]/(2*h);
}
thomas(n-4,w+2,a,c,d,b); for (gamma=1.;gamma>AESH_MIN;gamma/=2.){
if (gamma<10*AESH_MIN) exit(-1);
for (i=2;i<n-2;i++) hat_v[i] += gamma * w[i];
_G(n,b,tau,h,v,hat_v); printf("Ошибка %e\n",norm_max(n,b)); if (norm_max(n,b)<err) break;
for (i=2;i<n-2;i++) hat_v[i] -= gamma * w[i];
}
clean_all(5,pts);
return 1 + layer(eps,n,tau,h,v,hat_v);
}
void Point3D::optimize(const size_t n_iter)
{
Vector3d old_point = pos_;
double chi2 = 0.0;
Matrix3d A;
Vector3d b;
for (size_t i = 0; i < n_iter; i++)
{
A.setZero();
b.setZero();
double new_chi2 = 0.0;
// 计算残差
for (auto it = obs_.begin(); it != obs_.end(); ++it)
{
Matrix23d J;
const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
Point3D::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J);
const Vector2d e(project2d((*it)->f) - project2d(p_in_f));
new_chi2 += e.squaredNorm();
A.noalias() += J.transpose() * J;
b.noalias() -= J.transpose() * e;
}
// 求解线性系统
const Vector3d dp(A.ldlt().solve(b));
// 检测误差有没有增长
if ((i > 0 && new_chi2 > chi2) || (bool)std::isnan((double)dp[0]))
{
pos_ = old_point; // 回滚
break;
}
// 更新模型
Vector3d new_point = pos_ + dp;
old_point = pos_;
pos_ = new_point;
chi2 = new_chi2;
// 收敛则停止
if (norm_max(dp) <= EPS)
break;
}
}
int main(int argc,char**argv){
int N,M,i,j; double tau,h,eps,*v,*hat_v,*delta_v,*pts[3];
if (argc<4) {printf("%s 1/tau 1/h eps\n",argv[0]); return 0;}
N = atoi(argv[1]); M = atoi(argv[2]); eps = atof(argv[3]);
h = 1./M; tau = 1./N;
new_double(3,N,pts); v = pts[0]; hat_v = pts[1]; delta_v = pts[2];
v0(N,v);
// pl(N,v);
for(j=0;j<M;j++){
v_bounds(N,j/M,hat_v);
for (i=2;i<N-2;i++) hat_v[i] = v[i];
layer(eps,N,tau,h,v,hat_v);
for (i=0;i<N;i++) delta_v[i] = fabs(analitic( ((double)j)/M ,((double)i)/N) - hat_v[i] );
printf("layer %d с ошибкой %e\n",j,norm_max(N,delta_v));
for (i=2;i<N-2;i++) v[i] = hat_v[i];
}
clean_all(3,pts);
return 0;
}
R dist_max(V2<R> const& p, V2<R> const& q)
{ return norm_max(q-p); }
