/* * ===================================================================================== * * Filename: feature.cpp * * Description: Class for point features. Handles cases with and without * reflections. * * Version: 1.0 * Created: 03/18/2017 08:53:49 PM * Revision: none * Compiler: gcc * * Author: Martin Miller (MHM), miller7@illinois.edu * Organization: Aerospace Robotics and Controls Lab (ARC) * * ===================================================================================== */ #include "feature.h" /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature * Description: constructor for reflection type features *-------------------------------------------------------------------------------------- */ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr, const Vector3d &xbw, const Quaterniond &q, double z, Mat &p) { _id = id; Vector3d xib; xib = findDepth(q,z,xs,xr); X = x2p(xib); X0 = X; xb0w = xbw; q0 = q; _patch = p; lastseen = 0; } /* ----- end of method Feature::Feature (constructor) ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: Feature * Description: Constructor for mono type features *-------------------------------------------------------------------------------------- */ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xbw, const Quaterniond &q, Mat &p) { _id = id; Vector3d xib, pib; xib = xs/xs[0]; pib[0] = xib[1]; pib[1] = xib[2]; pib[2] = RHO_0; X = pib; X0 = X; xb0w = xbw; q0 = q; _patch = p; lastseen = 0; } void Feature::reinitialize(const Vector3d &xs, const Vector3d& xr, const Vector3d &xbw, const Quaterniond &q, Mat &p) { Vector3d xib; xib = findDepth(q,xbw[2],xs,xr); X = x2p(xib); X0 = X; xb0w = xbw; q0 = q; _patch = p; lastseen = 0; } /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: findDepth * Description: Compute the feature depth given the camera pose and the source * and reflection measurements. The measurements xs and xr should be given in * the body frame (i.e. transformed from the image frame into the camera frame * according to K and rotated from the camera frame into the body frame with * Rcb. The feature is then returned in body coordinates. *-------------------------------------------------------------------------------------- */ Vector3d Feature::findDepth ( const Quaterniond &qbw, double z, const Vector3d &xs, const Vector3d &xr ) { // The output vector Vector3d xb; Vector4d xb4; // Projection matrices Matrix P1,P2; P1 = Matrix::Identity(); // Rotation from body to world Matrix Rbw; Matrix Rbw4,Rwb4; Rbw = qbw.toRotationMatrix(); Rbw4 = Matrix::Identity(); Rbw4.block<3,3>(0,0) = Rbw; Rwb4 = Rbw4.transpose(); Matrix J; // Reflection matrix J = Matrix::Identity(); J(2,2) = -1; J(2,3) = -2*z; P2 = Rwb4*J*Rbw4; Matrix P; P.block<3,4>(0,0) = P1.block<3,4>(0,0); P.block<3,4>(3,0) = P2.block<3,4>(0,0); //Vector4d b; Matrix b; b.segment<3>(0) = xs; b[3] = xr[0]; b[4] = xr[1]; b[5] = xr[2]; xb4 = P.colPivHouseholderQr().solve(b); //xb4 = P.inverse()*b; xb4 /= xb4[3]; xb = xb4.segment<3>(0); return xb ; } /* ----- end of method Feature::findDepth ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: S * Description: Returns S, the information matrix for an independent * measurement of the feature. For correlated measurements, combine Hx and Hy * with other measurements outside of the instance and compute S. *-------------------------------------------------------------------------------------- */ Matrix Feature::S ( const Matrix &Pxx, const Matrix &Pxy, const Matrix &Pyy, const Vector3d &pos, const Quaterniond &q) { Matrix hx; Matrix hy; hx = Hx( pos, q); hy = Hy( pos, q); Matrix S; S = hx*Pxx*hx.transpose() + 2*hx*Pxy*hy.transpose() + hy*Pyy*hy.transpose() + R(); return S; } /* ----- end of method Feature::S ----- */ Matrix Feature::Q ( const double dt ) { Matrix Q; Q = Matrix::Identity(); Q *= 0.5*dt*dt*feature_noise*feature_noise;//*1e-1; return Q; } /* ----- end of method Feature::q ----- */ Matrix Feature::R ( ) { Matrix R; R = Matrix::Identity(); R.block<2,2>(0,0) *= view_noise; R.block<2,2>(2,2) *= initial_view_noise; R.block<2,2>(4,4) *= reflection_view_noise; return R; } /* ----- end of method Feature::R ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: L * Description: Returns the motion model matrix L. These can be stacked to * update the states of many features simultaneously. *-------------------------------------------------------------------------------------- */ Matrix Feature::L ( ) { double y0,y1,y2; Matrix L; y0 = X[0]; y1 = X[1]; y2 = X[2]; L << -y2, 0, y2*y0, y0*y1, -(1+y0*y0), y1, 0, -y2, y1*y2, 1+y1*y1, -y0*y1, -y0, 0, 0, y2*y2, y2*y1, -y2*y0, 0; return L; } /* ----- end of method Feature::L ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: motionModel * Description: Updates the feature state according to the motion model. *-------------------------------------------------------------------------------------- */ void Feature::motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt) { Matrix f; f = L(); Matrix V; V << vel[1], vel[2], vel[0], ang[1], ang[2], ang[0]; Matrix ydot; ydot = f*V; X += ydot*dt; return ; } /* ----- end of method Feature::motionModel ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: h * Description: Returns the predicted measurement vector. *-------------------------------------------------------------------------------------- */ Matrix Feature::h ( const Vector3d &x, const Quaterniond &q ) { Matrix h; h = Matrix::Identity(); Vector3d xib, xib0; Vector3d pib0; xib = p2x(X); // Predict initial view Quaterniond qbb0, qib, qw, qb0w; qib = Quaterniond(0, xib[0], xib[1], xib[2]); qw = Quaterniond(0,x[0],x[1],x[2]); qb0w = Quaterniond(0,xb0w[0],xb0w[1],xb0w[2]); qbb0 = q0.inverse()*q; xib0 = (qbb0*qib*qbb0.inverse()).vec() + (q.inverse()*qw*q).vec() - (q.inverse()*qb0w*q).vec(); pib0 = x2p(xib0); // Predict reflection using quaternions with rotation to world Vector3d ppb,xpb; Quaterniond qn; qn = Quaterniond(0.,0.,0.,-1.); // plane of water in world frame xpb = (q.inverse()*qn*q*qib*q.inverse()*qn*q).vec()+(q.inverse()*qn*qw*qn*q).vec()-(q.inverse()*qw*q).vec(); ppb = x2p(xpb); h[0] = X[0]; h[1] = X[1]; h[2] = pib0[0]; h[3] = pib0[1]; h[4] = ppb[0]; h[5] = ppb[1]; return h; } /* ----- end of method Feature::h ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: measurement_vector * Description: Returns the measurement_vector. xs and xr are in body * coordinates. *-------------------------------------------------------------------------------------- */ Matrix Feature::measurement_vector ( const Vector3d &xs, const Vector3d &xr ) { Matrix z; z = Matrix::Zero(); z.segment<4>(0) = measurement_vector(xs); Vector3d pr; pr = x2p(xr); z[4] = pr[0]; z[5] = pr[1]; return z; } /* ----- end of method Feature::measurement_vector ----- */ Matrix Feature::measurement_vector( const Vector3d &xs ) { Matrix z; z = Matrix::Zero(); Vector3d ps,p0; ps = x2p(xs); z[0] = ps[0]; z[1] = ps[1]; z[2] = X0[0]; z[3] = X0[1]; return z; } /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: p2x * Description: Returns x, the point p in inverse depth frame in the Cartesian * frame. *-------------------------------------------------------------------------------------- */ Vector3d Feature::p2x ( const Vector3d &p ) { Vector3d x; x[0] = 1./p[2]; x[1] = p[0]/p[2]; x[2] = p[1]/p[2]; return x; } /* ----- end of method Feature::p2x ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: x2p * Description: Returns p, the point x in the Cartesian body frame in the * inverse depth frame. *-------------------------------------------------------------------------------------- */ Vector3d Feature::x2p ( const Vector3d &x ) { Vector3d p; p[0] = x[1]/x[0]; p[1] = x[2]/x[0]; p[2] = 1./x[0]; return p; } /* ----- end of method Feature::x2p ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: Fx * Description: Returns the Jacobian of the motion model with respect to the * body. *-------------------------------------------------------------------------------------- */ Matrix Feature::Fx ( double dt ) { double y0,y1,y2; y0 = X[0]; y1 = X[1]; y2 = X[2]; Matrix F; F = Matrix::Zero(); F.block<3,3>(0,3) << y0*y2,-y2, 0., y1*y2, 0.,-y2, y2*y2, 0, 0; F *= dt; return F; } /* ----- end of method Feature::Fx ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: Fy * Description: Returns the Jacobian of the motion model with respect to the * feature. *-------------------------------------------------------------------------------------- */ Matrix Feature::Fy ( const Vector3d &v, const Vector3d &w, double dt ) { double y0,y1,y2; double v0,v1,v2; double w0,w1,w2; y0 = X[0]; y1 = X[1]; y2 = X[2]; v0 = v[0]; v1 = v[1]; v2 = v[2]; w0 = w[0]; w1 = w[1]; w2 = w[2]; Matrix F; F << v0*y2-2*w2*y0+y1*w1, w0+y0*w1, -v1+y0*v0, w0-y1*w2, v0*y2+2*w1*y1-y0*w2, -v2+y1*v0, -w2*y2, w1*y2, -w2*y0+w1*y1+2*v0*y2; F *= dt; F += Matrix::Identity(); return F; } /* ----- end of method Feature::Fy ----- */ Matrix Feature::Hx ( const Vector3d &pos, const Quaterniond &q ) { double xbw1,xbw2,xbw3; double xb0w1, xb0w2, xb0w3; double qbw1,qbw2,qbw3,qbw4; double qb0w1,qb0w2,qb0w3,qb0w4; double pib1, pib2, pib3; double hx11,hx12,hx13,hx21,hx22,hx23,hx33,hx43; xbw1 = pos[0]; xbw2 = pos[1]; xbw3 = pos[2]; qbw1 = q.x(); qbw2 = q.y(); qbw3 = q.z(); qbw4 = q.w(); pib1=X[0]; pib2=X[1]; pib3=X[2]; xb0w1=xb0w[0]; xb0w2=xb0w[1]; xb0w3=xb0w[2]; qb0w1=q0.x(); qb0w2=q0.y(); qb0w3=q0.z(); qb0w4=q0.w(); Matrix H; H = Matrix::Zero(); // Matlab generated C code. double t2,t3,t4,t5,t6,t7,t8,t9,t10,t11,t12,t13,t14,t15,t16,t17,t18,t19,t20, t21,t22,t23,t24,t25,t26,t27,t28,t29,t30,t31,t32,t33,t34,t35,t36,t37, t38,t39,t40,t41,t42,t43,t44,t45,t46,t47,t48,t49,t50,t51,t52,t53,t54, t55,t56,t57,t58,t59,t60,t61,t62,t63,t64,t65,t66,t67,t68,t69,t70,t71, t72,t73,t74,t75,t76,t77,t78,t79,t80,t81,t82,t83,t84,t85,t86,t87,t88, t89,t90,t91,t92,t93,t94,t95,t96,t97,t98,t99,t100,t101,t102,t103,t104, t105,t106,t107,t108,t109,t110,t111,t112,t113,t114,t115,t116,t117,t118, t119,t120,t121,t122,t123,t124,t125,t126,t127,t128,t129,t130,t131,t132, t133,t134,t135,t136,t137,t138,t139,t140,t141,t142,t143,t144,t145,t146, t147,t148,t149,t150,t151,t152,t153,t154,t155,t156,t157,t158,t159,t160, t161,t162,t163,t164,t165,t166,t167,t168,t169,t170,t171,t172,t173,t174, t175,t176,t177,t178,t179,t180,t181,t182,t183,t184,t185,t186,t187,t188, t189,t190,t191,t192,t193,t194,t195,t196,t197,t198,t199,t200,t201,t202, t203,t204,t205,t206,t207,t208,t209,t210,t211,t212,t213,t214,t215,t216, t217,t218,t219,t220,t221,t222,t223,t224,t225,t226,t227,t228,t229,t230, t231,t232; t2 = qb0w1*qb0w2*2.0; t3 = qb0w1*qb0w1; t4 = qb0w2*qb0w2; t5 = qb0w3*qb0w3; t6 = qb0w4*qb0w4; t7 = t3-t4-t5+t6; t8 = qb0w3*qb0w4*2.0; t9 = t2+t8; t10 = qb0w1*qb0w3*2.0; t13 = qb0w2*qb0w4*2.0; t11 = t10-t13; t12 = 1.0/pib3; t14 = qbw1*qbw1; t15 = qbw2*qbw2; t16 = qbw3*qbw3; t17 = qbw4*qbw4; t18 = qbw1*qbw2*2.0; t19 = qbw3*qbw4*2.0; t20 = qbw1*qbw4*2.0; t21 = qbw2*qbw3*2.0; t22 = qbw1*qbw3*2.0; t23 = xb0w1-xbw1; t24 = t7*t23; t25 = xb0w2-xbw2; t26 = t9*t25; t27 = xb0w3-xbw3; t28 = t11*t27; t29 = t14-t15-t16+t17; t30 = t7*t29; t31 = t18+t19; t32 = t9*t31; t33 = qbw2*qbw4*2.0; t34 = t20+t21; t35 = t11*t34; t36 = t14-t15+t16-t17; t37 = t18-t19; t38 = t7*t37; t55 = t9*t36; t39 = t35+t38-t55; t40 = t20-t21; t41 = t9*t40; t42 = t14+t15-t16-t17; t43 = t11*t42; t44 = t22+t33; t56 = t7*t44; t45 = t41+t43-t56; t46 = pib2*t12*t45; t47 = t2-t8; t48 = qb0w1*qb0w4*2.0; t49 = qb0w2*qb0w3*2.0; t50 = t48+t49; t51 = t22-t33; t52 = t3-t4+t5-t6; t53 = t11*t51; t54 = t30+t32+t53; t58 = t12*t54; t59 = pib1*t12*t39; t57 = t24+t26+t28+t46-t58-t59; t60 = 1.0/(t57*t57); t61 = t25*t52; t62 = t50*t51; t63 = t29*t47; t79 = t31*t52; t64 = t62+t63-t79; t65 = t12*t64; t66 = t36*t52; t67 = t37*t47; t68 = t34*t50; t69 = t66+t67+t68; t70 = pib1*t12*t69; t71 = t44*t47; t72 = t40*t52; t80 = t42*t50; t73 = t71+t72-t80; t74 = pib2*t12*t73; t77 = t23*t47; t78 = t27*t50; t75 = t61+t65+t70+t74-t77-t78; t76 = 1.0/t57; t81 = qbw1*t52*2.0; t82 = qbw2*t47*2.0; t83 = qbw4*t50*2.0; t84 = t81+t82+t83; t85 = qbw1*t47*2.0; t86 = qbw3*t50*2.0; t98 = qbw2*t52*2.0; t87 = t85+t86-t98; t88 = qbw2*t7*2.0; t89 = qbw4*t11*2.0; t112 = qbw1*t9*2.0; t90 = t88+t89-t112; t91 = qbw1*t7*2.0; t92 = qbw2*t9*2.0; t93 = qbw3*t11*2.0; t94 = t91+t92+t93; t95 = qbw4*t52*2.0; t96 = qbw3*t47*2.0; t109 = qbw1*t50*2.0; t97 = t95+t96-t109; t99 = qbw3*t52*2.0; t100 = qbw2*t50*2.0; t108 = qbw4*t47*2.0; t101 = t99+t100-t108; t102 = qbw1*t11*2.0; t103 = qbw4*t9*2.0; t111 = qbw3*t7*2.0; t104 = t102+t103-t111; t105 = qbw4*t7*2.0; t106 = qbw3*t9*2.0; t110 = qbw2*t11*2.0; t107 = t105+t106-t110; t113 = t10+t13; t114 = t48-t49; t115 = t3+t4-t5-t6; t116 = t27*t115; t117 = t25*t114; t118 = t31*t114; t119 = t51*t115; t132 = t29*t113; t120 = t118+t119-t132; t121 = t37*t113; t122 = t36*t114; t134 = t34*t115; t123 = t121+t122-t134; t124 = pib1*t12*t123; t125 = t42*t115; t126 = t44*t113; t127 = t40*t114; t128 = t125+t126+t127; t129 = pib2*t12*t128; t131 = t23*t113; t133 = t12*t120; t130 = t116+t117+t124+t129-t131-t133; t135 = t12*t94; t136 = pib1*t12*t90; t137 = t135+t136-pib2*t12*t104; t138 = qbw1*t114*2.0; t139 = qbw2*t113*2.0; t159 = qbw4*t115*2.0; t140 = t138+t139-t159; t141 = qbw3*t115*2.0; t142 = qbw2*t114*2.0; t151 = qbw1*t113*2.0; t143 = t141+t142-t151; t144 = pib1*t12*t94; t145 = pib2*t12*t107; t146 = t144+t145-t12*t90; t147 = qbw1*t115*2.0; t148 = qbw3*t113*2.0; t149 = qbw4*t114*2.0; t150 = t147+t148+t149; t152 = qbw2*t115*2.0; t153 = qbw4*t113*2.0; t158 = qbw3*t114*2.0; t154 = t152+t153-t158; t155 = t12*t104; t156 = pib2*t12*t94; t157 = t155+t156-pib1*t12*t107; t160 = t12*t107; t161 = pib1*t12*t104; t162 = pib2*t12*t90; t163 = t160+t161+t162; t164 = xbw3*2.0; t165 = t12*t51; t166 = pib1*t12*t34; t178 = pib2*t12*t42; t167 = t164+t165+t166-t178; t168 = t51*t167; t169 = pib1*t12*t36; t170 = pib2*t12*t40; t179 = t12*t31; t171 = t169+t170-t179; t172 = t31*t171; t173 = t12*t29; t174 = pib1*t12*t37; t175 = pib2*t12*t44; t176 = t173+t174+t175; t180 = t29*t176; t177 = t168+t172-t180; t181 = 1.0/t177; t182 = 1.0/(t177*t177); t183 = t37*t176; t184 = t36*t171; t199 = t34*t167; t185 = t183+t184-t199; t186 = pib1*qbw1*t12*2.0; t187 = pib2*qbw4*t12*2.0; t196 = qbw2*t12*2.0; t188 = t186+t187-t196; t189 = qbw3*t12*2.0; t190 = pib1*qbw4*t12*2.0; t205 = pib2*qbw1*t12*2.0; t191 = t189+t190-t205; t192 = qbw1*t12*2.0; t193 = pib1*qbw2*t12*2.0; t194 = pib2*qbw3*t12*2.0; t195 = t192+t193+t194; t197 = qbw3*t167*2.0; t198 = qbw2*t171*2.0; t200 = qbw4*t12*2.0; t201 = pib2*qbw2*t12*2.0; t206 = pib1*qbw3*t12*2.0; t202 = t200+t201-t206; t203 = qbw1*t171*2.0; t204 = qbw2*t176*2.0; t207 = qbw1*t167*2.0; t208 = qbw3*t176*2.0; t209 = qbw4*t171*2.0; t210 = qbw2*t167*2.0; t211 = qbw4*t176*2.0; t212 = qbw1*qbw3*4.0; t213 = t212-qbw2*qbw4*4.0; t214 = t44*t176; t215 = t40*t171; t216 = t42*t167; t217 = t214+t215+t216; t218 = t31*t188; t219 = t51*t191; t226 = qbw1*t176*2.0; t220 = t197+t198+t218+t219-t226-t29*t195; t221 = t31*t195; t222 = t51*t202; t223 = qbw4*t167*2.0; t224 = t29*t188; t225 = -t203-t204+t221+t222+t223+t224; t227 = t51*t195; t228 = t29*t191; t229 = t207+t208+t209+t227+t228-t31*t202; t230 = t31*t191; t231 = t29*t202; t232 = t210+t211+t230+t231-qbw3*t171*2.0-t51*t188; H(2,0) = -t47/(t24+t26+t28+t46-t12*(t30+t32+t11*(t22-qbw2*qbw4*2.0))-pib1*t12*t39)-t7*t60*t75; H(2,1) = t52*t76-t9*t60*t75; H(2,2) = -t50*t76-t11*t60*t75; #if STATESIZE==13 H(2,6) = -t76*(t12*t87+pib1*t12*t84+pib2*t12*t97)-t60*t75*t137; H(2,7) = t76*(t12*t84-pib1*t12*t87+pib2*t12*t101)-t60*t75*t146; H(2,8) = -t76*(-t12*t97+pib2*t12*t87+pib1*t12*t101)-t60*t75*t157; H(2,9) = t76*(t12*t101-pib2*t12*t84+pib1*t12*t97)-t60*t75*t163; #endif H(3,0) = -t76*t113-t7*t60*t130; H(3,1) = t76*t114-t9*t60*t130; H(3,2) = t76*t115-t11*t60*t130; #if STATESIZE==13 H(3,6) = -t76*(-t12*t143+pib1*t12*t140+pib2*t12*t150)-t60*t130*t137; H(3,7) = t76*(t12*t140+pib1*t12*t143-pib2*t12*t154)-t60*t130*t146; H(3,8) = t76*(t12*t150+pib2*t12*t143+pib1*t12*t154)-t60*t130*t157; H(3,9) = -t76*(t12*t154+pib2*t12*t140-pib1*t12*t150)-t60*t130*t163; #endif H(4,2) = t181*(qbw1*qbw4*4.0+qbw2*qbw3*4.0)+t182*t185*t213; #if STATESIZE==13 H(4,6) = -t181*(t203+t204-qbw4*t167*2.0+t36*t188-t34*t191+t37*t195)+t182*t185*t220; H(4,7) = t181*(t197+t198-qbw1*t176*2.0-t37*t188+t36*t195-t34*t202)-t182*t185*t225; H(4,8) = t181*(t210+t211-qbw3*t171*2.0+t37*t191+t34*t195+t36*t202)+t182*t185*t229; H(4,9) = t181*(t207+t208+t209+t34*t188+t36*t191-t37*t202)-t182*t185*t232; #endif H(5,2) = -t181*(t14*2.0+t15*2.0-t16*2.0-t17*2.0)+t182*t213*t217; #if STATESIZE==13 H(5,6) = -t181*(t207+t208+t209+t40*t188+t42*t191+t44*t195)+t182*t217*t220; H(5,7) = -t181*(t210+t211-qbw3*t171*2.0+t44*t188-t40*t195-t42*t202)-t182*t217*t225; H(5,8) = t181*(t197+t198-t226+t44*t191-t42*t195+t40*t202)+t182*t217*t229; H(5,9) = -t181*(t203+t204-t223+t42*t188-t40*t191+t44*t202)-t182*t217*t232; #endif return H; } /* ----- end of method Feature::Hx ----- */ Matrix Feature::Hy ( const Vector3d &pos, const Quaterniond &q ) { double xbw1,xbw2,xbw3; double xb0w1, xb0w2, xb0w3; double qbw1,qbw2,qbw3,qbw4; double qb0w1,qb0w2,qb0w3,qb0w4; double pib1, pib2, pib3; double hy11,hy12,hy13,hy21,hy22,hy23,hy31, hy32, hy33,hy41, hy42, hy43; xbw1 = pos[0]; xbw2 = pos[1]; xbw3 = pos[2]; qbw1 = q.x(); qbw2 = q.y(); qbw3 = q.z(); qbw4 = q.w(); pib1=X[0]; pib2=X[1]; pib3=X[2]; xb0w1=xb0w[0]; xb0w2=xb0w[1]; xb0w3=xb0w[2]; qb0w1=q0.x(); qb0w2=q0.y(); qb0w3=q0.z(); qb0w4=q0.w(); Matrix H; H = Matrix::Zero(); // Matlab generated code double t2,t3,t4,t5,t6,t7,t8,t9,t10,t11,t12,t13,t14,t15,t16,t17,t18,t19,t20, t21,t22,t23,t24,t25,t26,t27,t28,t29,t30,t31,t32,t33,t34,t35,t36,t37, t38,t39,t40,t41,t42,t43,t44,t45,t46,t47,t48,t49,t50,t51,t52,t53,t54, t55,t56,t57,t58,t59,t60,t61,t62,t63,t64,t65,t66,t67,t68,t69,t70,t71, t72,t73,t74,t75,t76,t77,t78,t79,t80,t81,t82,t83,t84,t85,t86,t87,t88, t89,t90,t91,t92,t93,t94,t95,t96,t97,t98,t99,t100,t101,t102,t103,t104, t105,t106,t107,t108,t109,t110,t111,t112,t113,t114,t115,t116,t117,t118, t119,t120,t121,t122,t123,t124,t125,t126,t127,t128,t129,t130,t131,t132, t133,t134,t135,t136,t137,t138,t139,t140,t141,t142,t143,t144,t145,t146, t147,t148,t149,t150,t151,t152,t153,t154,t155,t156,t157,t158; t2 = qb0w1*qb0w1; t3 = qb0w2*qb0w2; t4 = qb0w3*qb0w3; t5 = qb0w4*qb0w4; t6 = qb0w1*qb0w2*2.0; t7 = 1.0/pib3; t8 = t2-t3-t4+t5; t9 = qbw1*qbw1; t10 = qbw2*qbw2; t11 = qbw3*qbw3; t12 = qbw4*qbw4; t13 = qb0w3*qb0w4*2.0; t14 = t6+t13; t15 = qbw1*qbw2*2.0; t16 = qb0w1*qb0w3*2.0; t18 = qb0w2*qb0w4*2.0; t17 = t16-t18; t19 = qbw1*qbw4*2.0; t20 = qbw2*qbw3*2.0; t21 = t19+t20; t22 = t9-t10+t11-t12; t23 = qbw3*qbw4*2.0; t24 = qbw1*qbw3*2.0; t25 = t15-t23; t26 = t17*t21; t27 = t8*t25; t40 = t14*t22; t28 = t26+t27-t40; t29 = xb0w1-xbw1; t30 = t8*t29; t31 = xb0w2-xbw2; t32 = t14*t31; t33 = xb0w3-xbw3; t34 = t17*t33; t35 = t9-t10-t11+t12; t36 = t8*t35; t37 = t15+t23; t38 = t14*t37; t39 = qbw2*qbw4*2.0; t41 = t19-t20; t42 = t14*t41; t43 = t9+t10-t11-t12; t44 = t17*t43; t45 = t24+t39; t63 = t8*t45; t46 = t42+t44-t63; t47 = pib2*t7*t46; t48 = t2-t3+t4-t5; t49 = t6-t13; t50 = qb0w1*qb0w4*2.0; t51 = qb0w2*qb0w3*2.0; t52 = t50+t51; t53 = t24-t39; t54 = t22*t48; t55 = t25*t49; t56 = t21*t52; t57 = t54+t55+t56; t58 = t45*t49; t59 = t41*t48; t74 = t43*t52; t60 = t58+t59-t74; t61 = t17*t53; t62 = t36+t38+t61; t65 = t7*t62; t66 = pib1*t7*t28; t64 = t30+t32+t34+t47-t65-t66; t67 = 1.0/(t64*t64); t68 = t31*t48; t69 = t52*t53; t70 = t35*t49; t77 = t37*t48; t71 = t69+t70-t77; t72 = t7*t71; t73 = pib1*t7*t57; t75 = pib2*t7*t60; t80 = t29*t49; t81 = t33*t52; t76 = t68+t72+t73+t75-t80-t81; t78 = 1.0/(pib3*pib3); t79 = 1.0/t64; t82 = t2+t3-t4-t5; t83 = t16+t18; t84 = t50-t51; t85 = t25*t83; t86 = t22*t84; t97 = t21*t82; t87 = t85+t86-t97; t88 = t43*t82; t89 = t45*t83; t90 = t41*t84; t91 = t88+t89+t90; t92 = t33*t82; t93 = t31*t84; t94 = t37*t84; t95 = t53*t82; t101 = t35*t83; t96 = t94+t95-t101; t98 = pib1*t7*t87; t99 = pib2*t7*t91; t105 = t29*t83; t106 = t7*t96; t100 = t92+t93+t98+t99-t105-t106; t102 = t62*t78; t103 = pib1*t28*t78; t104 = t102+t103-pib2*t46*t78; t107 = xbw3*2.0; t108 = t7*t53; t109 = pib1*t7*t21; t121 = pib2*t7*t43; t110 = t107+t108+t109-t121; t111 = t53*t110; t112 = pib1*t7*t22; t113 = pib2*t7*t41; t122 = t7*t37; t114 = t112+t113-t122; t115 = t37*t114; t116 = t7*t35; t117 = pib1*t7*t25; t118 = pib2*t7*t45; t119 = t116+t117+t118; t123 = t35*t119; t120 = t111+t115-t123; t124 = 1.0/t120; t125 = 1.0/(t120*t120); t126 = t25*t119; t127 = t22*t114; t129 = t21*t110; t128 = t126+t127-t129; t130 = pib1*t22*t78; t131 = pib2*t41*t78; t154 = t37*t78; t132 = t130+t131-t154; t133 = t53*t78; t134 = pib1*t21*t78; t155 = pib2*t43*t78; t135 = t133+t134-t155; t136 = t35*t78; t137 = pib1*t25*t78; t138 = pib2*t45*t78; t139 = t136+t137+t138; t140 = t7*t22*t41; t141 = t7*t21*t43; t142 = t7*t25*t45; t143 = t140+t141+t142; t144 = t7*t22*t37; t145 = t7*t21*t53; t146 = t144+t145-t7*t25*t35; t147 = t7*t43*t53; t148 = t7*t35*t45; t149 = t147+t148-t7*t37*t41; t150 = t45*t119; t151 = t41*t114; t152 = t43*t110; t153 = t150+t151+t152; t156 = t37*t132; t157 = t53*t135; t158 = t156+t157-t35*t139; H(0,0) = 1.0; H(1,1) = 1.0; H(2,0) = -(t7*t57)/(t30+t32+t34+t47-t7*(t36+t38+t17*(t24-qbw2*qbw4*2.0))-pib1*t7*t28)-t7*t28*t67*t76; H(2,1) = -t7*t60*t79+t7*t46*t67*t76; H(2,2) = t79*(t71*t78+pib1*t57*t78+pib2*t60*t78)+t67*t76*t104; H(3,0) = -t7*t79*t87-t7*t28*t67*t100; H(3,1) = -t7*t79*t91+t7*t46*t67*t100; H(3,2) = t79*(-t78*t96+pib1*t78*t87+pib2*t78*t91)+t67*t100*t104; H(4,0) = -t124*(-t7*(t21*t21)+t7*(t22*t22)+t7*(t25*t25))+t125*t128*t146; H(4,1) = -t124*t143-t125*t128*t149; H(4,2) = t124*(t22*t132-t21*t135+t25*t139)-t125*t128*t158; H(5,0) = -t124*t143+t125*t146*t153; H(5,1) = -t124*(t7*(t41*t41)-t7*(t43*t43)+t7*(t45*t45))-t125*t149*t153; H(5,2) = t124*(t41*t132+t43*t135+t45*t139)-t125*t153*t158; return H ; } /* ----- end of method Feature::Hy ----- */ int Feature::id ( ) { return _id; } /* ----- end of method Feature::id ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: P0 * Description: Returns the initial P matrix. *-------------------------------------------------------------------------------------- */ Matrix Feature::P0 ( measurement_type t ) { Matrix P; double p0 = FEATURECOVX; double p1 = FEATURECOVY; double p2; if (t==BOTH) { #ifdef INITDEPTH p2 = FEATURECOVRHO; #else p2 = FEATURECOVRHO_MONO; #endif } else { p2 = FEATURECOVRHO_MONO; } P << p0, 0., 0., 0., p1, 0., 0., 0., p2; return P; } /* ----- end of method Feature::P0 ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: utm * Description: Return the feature position in world coordinates as a UTM * struct. *-------------------------------------------------------------------------------------- */ UTM Feature::utm ( Vector3d &xbw, Quaterniond &q ) { UTM utm; Vector3d xib,xiw; xib = p2x(X); xiw = xbw + q.toRotationMatrix()*xib; utm.northing = xiw[0]; utm.easting = xiw[1]; utm.up = -xiw[2]; utm.zone_i = 16; utm.zone_c = 'T'; return utm ; } /* ----- end of method Feature::utm ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: sane * Description: Check if the feature position is sane. *-------------------------------------------------------------------------------------- */ bool Feature::sane ( ) { bool rv; // Check that the depth is positive. if (X[2]<0) { rv = false; } else if (1./X[2]<1.) { // Too close rv = false; } else if (1./X[2]>40.) { // Too far rv = false; } else { rv = true; } return rv ; } /* ----- end of method Feature::sane ----- */ void Feature::dx ( const Vector3d &del ) { lastseen += 1; X += del; return ; } /* ----- end of method Feature::dx ----- */ /* *-------------------------------------------------------------------------------------- * Class: Feature * Method: Feature :: inFOV * Description: Returns true if the feature is in the field of view. This is a * somewhat loose interpretation of that because we don't know the actual camera * parameters. *-------------------------------------------------------------------------------------- */ bool Feature::inFOV ( ) { bool rv; double angleh,anglev; Vector3d xib; xib = p2x(X); angleh = atan2(xib[0],xib[1]); anglev = atan2(xib[0],xib[2]); if (xib[0]<1) { rv = false; } else if (anglev(3*M_PI_4)) { rv = false; } else if (angleh(3*M_PI_4)) { rv = false; } else { rv = true; } return rv; } /* ----- end of method Feature::inFOV ----- */ bool Feature::isInlier(const measurement_t &z, const Matrix &Pxx, const Matrix &Pxy, const Matrix &Pyy, const Vector3d &pos, const Quaterniond &q, double thr) { Matrix s; Matrix hi, zi, y; s = S(Pxx, Pxy, Pyy, pos, q); hi = h(pos,q); if (z.z_type==MONO) { s = s.topLeftCorner<4,4>(); hi = hi.head<4>(); zi = measurement_vector(z.source); } else { zi = measurement_vector(z.source, z.reflection); } double Y; y = zi-hi; Y = y.transpose()*s.inverse()*y; return (Y hi, zi, y; hi = h(pos,q); if (z.z_type==MONO) { hi = hi.head<4>(); zi = measurement_vector(z.source); } else { zi = measurement_vector(z.source, z.reflection); } double Y; y = zi-hi; Y = y.transpose()*y; return (Y