%-----------------------------------------------------------------------
%?1-point?RANSAC?EKF?SLAM?from?a?monocular?sequence
%-----------------------------------------------------------------------

%?Copyright?（C）?2010?Javier?Civera?and?J.?M.?M.?Montiel
%?Universidad?de?Zaragoza?Zaragoza?Spain.

%?This?program?is?free?software:?you?can?redistribute?it?and/or?modify
%?it?under?the?terms?of?the?GNU?General?Public?License?as?published?by
%?the?Free?Software?Foundation.?Read?http://www.gnu.org/copyleft/gpl.html?for?details

%?If?you?use?this?code?for?academic?work?please?reference:
%???Javier?Civera?Oscar?G.?Grasa?Andrew?J.?Davison?J.?M.?M.?Montiel
%???1-Point?RANSAC?for?EKF?Filtering:?Application?to?Real-Time?Structure?from?Motion?and?Visual?Odometry
%???to?appear?in?Journal?of?Field?Robotics?October?2010.

%-----------------------------------------------------------------------
%?Authors:??Javier?Civera?--?jcivera@unizar.es?
%???????????J.?M.?M.?Montiel?--?josemari@unizar.es

%?Robotics?Perception?and?Real?Time?Group
%?Arag髇?Institute?of?Engineering?Research?（I3A）
%?Universidad?de?Zaragoza?50018?Zaragoza?Spain
%?Date???:??May?2010
%-----------------------------------------------------------------------

function?P_RES?=?add_a_feature_covariance_inverse_depth（?P?uvd?Xv?std_pxl?std_rho?cam?）

fku?=??cam.K（11）;
fkv?=??cam.K（22）;
U0??=??cam.K（13）;
V0??=??cam.K（23）;

q_wc?=?Xv（4:7）;
R_wc?=?q2r（q_wc）;

uvu?=?undistort_fm（?uvd?cam?）;
uu?=?uvu（1）;
vu?=?uvu（2）;

X_c?=?-（U0-uu）/fku;
Y_c?=?-（V0-vu）/fkv;
Z_c?=?1;

XYZ_c?=?[X_c;?Y_c;?Z_c];

XYZ_w?=?R_wc*XYZ_c;
X_w?=?XYZ_w（1）;
Y_w?=?XYZ_w（2）;
Z_w?=?XYZ_w（3）;

%?Derivatives

dtheta_dgw?=?[?Z_w/（X_w^2+Z_w^2）?0?-X_w/（X_w^2+Z_w^2）?];
dphi_dgw?=?[?（X_w*Y_w）/（（X_w^2+Y_w^2+Z_w^2）*sqrt（X_w^2+Z_w^2））...
????????-sqrt（X_w^2+Z_w^2）/（X_w^2+Y_w^2+Z_w^2）?（Z_w*Y_w）/（（X_w^2+Y_w^2+Z_w^2）*sqrt（X_w^2+Z_w^2））?];
dgw_dqwr?=?dRq_times_a_by_dq（?q_wc?XYZ_c?）;

dtheta_dqwr?=?dtheta_dgw*dgw_dqwr;
dphi_dqwr?=?dphi_dgw*dgw_dqwr;
dy_dqwr?=?sparse（?[?zeros（34）;?dtheta_dqwr;?dphi_dqwr;?zeros（14）?]?）;
dy_drw?=?sparse（?[?eye（3）;?zeros（33）?]?）;

dy_dxv?=?sparse（?[?dy_drw?dy_dqwr?zeros（66）?]?）;

dyprima_dgw?=?sparse（?[?zeros（33）;?dtheta_dgw;?dphi_dgw?]?）;
dgw_dgc?=?R_wc;
dgc_dhu?=?sparse（?[?+1/fku??????0???????0;
????????????????0???+1/fkv??????0]‘?）;
dhu_dhd?=?jacob_undistor_fm（?cam??uvd?）;

dyprima_dhd?=?dyprima_dgw*dgw_dgc*dgc_dhu*dhu_dhd;

dy_dhd?=?sparse（?[?dyprima_dhd?zeros（51）;
????????????zeros（12）??????1?]?）;

Ri?=?speye（2）*std_pxl^2;

Padd?=?sparse（?[Ri??zeros（21）;
????zeros（12）??std_rho^2]?）;

%
P_xv?=?P（?1:13?1:13?）;
P_yxv?=?P（?14:end?1:13?）;
P_y?=?P（?14:end?14:end?）;
P_xvy?=?P（?1:13?14:end?）;
P_RES?=?[?P_xv??????????P_xvy???????????????????????P_xv*dy_dxv‘;
??????????P_yxv?????????P_y?????????????????????????P_yxv*dy_dxv‘;
??????????dy_dxv*P_xv???dy_dxv*P_xvy????????????????dy_dxv*P_xv*dy_dxv‘+...
??????????

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

????I.A....?????????3??2017-12-14?19:39??ekfmonoslam\.svn\entries

????I.A....?????????3??2017-12-14?19:39??ekfmonoslam\.svn\format

????I.A...R??????1327??2017-12-14?19:39??ekfmonoslam\.svn\pristine\03\033a001575e80c9f7d8753be8e90234e2232c227.svn-base

????I.A...R??????3326??2017-12-14?20:06??ekfmonoslam\.svn\pristine\07\0758be254ed20cd9ab8420be9282c71200ec4ad5.svn-base

????I.A...R??????2018??2017-12-14?19:39??ekfmonoslam\.svn\pristine\08\08e4e160e3291e6a1a296b6f89f59b5ac90f5719.svn-base

????I.A...R??????1313??2017-12-14?19:39??ekfmonoslam\.svn\pristine\0c\0c51429c746aefa0f8e00c45039d4295447b12ec.svn-base

????I.A...R??????1789??2017-12-14?20:06??ekfmonoslam\.svn\pristine\0d\0d3e40a4a3d82ad8230381d8b377d453710f9696.svn-base

????I.A...R??????2479??2017-12-14?19:39??ekfmonoslam\.svn\pristine\0e\0ea798aeba666f4beec77bb6e642f79617c6bc99.svn-base

????I.A...R??????1597??2017-12-14?19:39??ekfmonoslam\.svn\pristine\0f\0f5e54bd09d582db3177d2f02dfbd6637de0b2a3.svn-base

????I.A...R??????3813??2017-12-14?20:06??ekfmonoslam\.svn\pristine\11\11539ed46bfed5865732d1979af03cec42b3f8a4.svn-base

????I.A...R??????1973??2017-12-14?19:39??ekfmonoslam\.svn\pristine\14\14c6038c6b968f87fa86564205568189859ab0cd.svn-base

????I.A...R???????321??2017-12-14?20:06??ekfmonoslam\.svn\pristine\16\16b6f6fc3a5c574fbadb0db53f0b6d611dbb8cb9.svn-base

????I.A...R??????1438??2017-12-14?19:39??ekfmonoslam\.svn\pristine\19\1987ff338ecf6649d236478a94dc62b08a0af6ba.svn-base

????I.A...R??????5126??2017-12-14?19:39??ekfmonoslam\.svn\pristine\1a\1a869286e39159cbcfa305ccbd95e9c30358f959.svn-base

????I.A...R??????1865??2017-12-14?19:39??ekfmonoslam\.svn\pristine\20\20de4a03acc85fa43555b9288d22357d09a2c32f.svn-base

????I.A...R??????1532??2017-12-14?20:06??ekfmonoslam\.svn\pristine\27\27d6a50fb923afd66fb6dcf2073cc2be4ba6e94f.svn-base

????I.A...R??????1753??2017-12-14?19:39??ekfmonoslam\.svn\pristine\29\2980d2c708d1517503794c0eacb6984041fbcda7.svn-base

????I.A...R??????3905??2017-12-14?20:06??ekfmonoslam\.svn\pristine\2b\2bdee5218c620cc6be2de94318074c180d81a0ad.svn-base

????I.A...R??????1639??2017-12-14?19:39??ekfmonoslam\.svn\pristine\32\3235b465b4331b719065237601ad38e9bddbc3f5.svn-base

????I.A...R??????1335??2017-12-14?19:39??ekfmonoslam\.svn\pristine\33\33cb887fee49019214abb8ab1b602538f5ee1c1e.svn-base

????I.A...R??????2459??2017-12-14?19:39??ekfmonoslam\.svn\pristine\34\34a098936fff6aa0d2ba1a257f3470d546d034f1.svn-base

????I.A...R??????1786??2017-12-14?19:39??ekfmonoslam\.svn\pristine\34\34d16e28bef6863bf9bc6a185df87dcfa1aa6377.svn-base

????I.A...R??????1743??2017-12-14?19:39??ekfmonoslam\.svn\pristine\35\351ab106706cd5e01bf0fdc691419eba9fd7f3b5.svn-base

????I.A...R????221316??2017-12-14?20:06??ekfmonoslam\.svn\pristine\38\3812a64443c2b6ef4ec2839b6070ed6fb20ff7cf.svn-base

????I.A...R????164469??2017-12-14?20:06??ekfmonoslam\.svn\pristine\39\3979604a19f967e5b9b9fc52c51f7cf36709196a.svn-base

????I.A...R??????1213??2017-12-14?20:06??ekfmonoslam\.svn\pristine\3b\3b1ac77aa4b4920f96c1b3d055b1d391a87627f7.svn-base

????I.A...R??????3532??2017-12-14?20:06??ekfmonoslam\.svn\pristine\3b\3b583517006a128faf31e26660bbba7e8465c8d9.svn-base

????I.A...R??????1545??2017-12-14?19:39??ekfmonoslam\.svn\pristine\3e\3e7565585f97ec9c099df5321ebc690bfd1259d5.svn-base

????I.A...R??????2185??2017-12-14?20:06??ekfmonoslam\.svn\pristine\3f\3f9b1db753476d5dbe547a56ba8301921a8e495a.svn-base

????I.A...R??????1473??2017-12-14?20:06??ekfmonoslam\.svn\pristine\44\4496495c528e339e19d499228dfbc17493aa9aa1.svn-base

............此處省略347個文件信息