@@ -189,12 +189,117 @@ void nativeWindowControls(const Fixture& fixture) {
189189 assert ((first->getFramePoses ().at (10 ).getPose ().translation ()-untouched->getFramePoses ().at (10 ).getPose ().translation ()).norm ()==0 );
190190 std::cout<<" Native active window: fixed-chart derivatives, gauge convention, selected originals, capacity, nonlinear update and cancellation rollback pass; derivative error " <<maximum<<" .\n " ;
191191}
192+ void nativeWorldControls (const Fixture& fixture) {
193+ auto mapper=fixture.mapper ();mapper->config .vio_init_pose_weight =2 .;
194+ mapper->calib .T_i_c [0 ]=Sophus::SE3d (Sophus::SO3d::exp (Eigen::Vector3d (.03 ,-.06 ,.02 )),Eigen::Vector3d (.12 ,-.04 ,.07 ));
195+ mapper->lmdb .getLandmark (99 ).inv_dist =.2 ;
196+ for (auto & [stamp,state]:mapper->getFramePoses ()) {
197+ Sophus::Vector6d tilt=Sophus::Vector6d::Zero ();tilt.tail <3 >()=Eigen::Vector3d (.25 ,-.3 ,.4 );state.applyInc (tilt);
198+ }
199+ mapper->rel_pose_factors .clear ();
200+ for (const auto pair:{std::pair<int64_t ,int64_t >{10 ,20 },{20 ,30 }}) {
201+ basalt::RelPoseFactor factor;factor.t_i_ns =pair.first ;factor.t_j_ns =pair.second ;
202+ factor.T_i_j =mapper->getFramePoses ().at (pair.first ).getPose ().inverse ()*mapper->getFramePoses ().at (pair.second ).getPose ();
203+ factor.cov_inv =10 *Sophus::Matrix6d::Identity ();mapper->rel_pose_factors .push_back (factor);
204+ }
205+ mapper->roll_pitch_factors [0 ].R_w_i_meas =mapper->getFramePoses ().at (10 ).getPose ().so3 ();
206+ auto world=[&](int64_t id) {
207+ const auto & p=mapper->lmdb .getLandmark (id);
208+ const auto host=mapper->getFramePoses ().at (p.host_kf_id .frame_id ).getPose ()*mapper->calib .T_i_c .at (p.host_kf_id .cam_id );
209+ const Eigen::Vector4d ray=basalt::StereographicParam<double >::unproject (p.direction );
210+ return Eigen::Vector3d (host*(ray.head <3 >()/p.inv_dist ));
211+ };
212+ for (int64_t id:{42 ,99 }) {
213+ const auto point=world (id);auto & p=mapper->lmdb .getLandmark (id);
214+ for (auto & [image,pixel]:p.obs ) {
215+ const auto camera=mapper->getFramePoses ().at (image.frame_id ).getPose ()*mapper->calib .T_i_c .at (image.cam_id );
216+ Eigen::Vector4d local;local.head <3 >()=camera.inverse ()*point;local[3 ]=1 ;
217+ assert (mapper->calib .intrinsics .at (image.cam_id ).project (local,pixel));
218+ }
219+ }
220+ auto dense=[&] {
221+ auto rows=posim_slam::nativeMappingRows (*mapper);int count=4 ;
222+ std::sort (rows.points .begin (),rows.points .end (),[](const auto & a,const auto & b){return a.id <b.id ;});
223+ for (const auto & group:rows.factors )count+=group.jacobian .rows ();
224+ for (const auto & group:rows.points )count+=group.pose .rows ();
225+ Eigen::MatrixXd jacobian=Eigen::MatrixXd::Zero (count,24 );int offset=0 ;
226+ const auto poses=posim_slam::nativeMappingPoseOffsets (rows.poses );
227+ for (const auto & group:rows.factors ) {
228+ for (size_t i=0 ;i<group.poses .size ();++i)jacobian.block (offset,poses.at (group.poses [i]),group.jacobian .rows (),6 )=group.jacobian .middleCols <6 >(6 *i);
229+ offset+=group.jacobian .rows ();
230+ }
231+ for (size_t p=0 ;p<rows.points .size ();++p) {
232+ const auto & group=rows.points [p];
233+ for (size_t i=0 ;i<group.poses .size ();++i)jacobian.block (offset,poses.at (group.poses [i]),group.pose .rows (),6 )=group.pose .middleCols <6 >(6 *i);
234+ jacobian.block (offset,18 +3 *p,group.pose .rows (),3 )=group.landmark ;offset+=group.pose .rows ();
235+ }
236+ jacobian.block <3 ,3 >(offset,0 )=std::sqrt (2 .)*Eigen::Matrix3d::Identity ();jacobian (offset+3 ,5 )=std::sqrt (2 .);
237+ Eigen::JacobiSVD<Eigen::MatrixXd> svd (jacobian,Eigen::ComputeThinV);assert (svd.singularValues ().minCoeff ()>1e-5 );
238+ const Eigen::MatrixXd root=svd.matrixV ()*svd.singularValues ().cwiseInverse ().asDiagonal ();
239+ // Numerical world derivatives independently exercise host, calibration and
240+ // inverse-distance transport, without using the window's analytic chain.
241+ Eigen::MatrixXd output=Eigen::MatrixXd::Zero (6 ,24 );const double epsilon=1e-6 ;
242+ for (size_t p=0 ;p<rows.points .size ();++p) {
243+ const auto id=rows.points [p].id ;
244+ for (const auto & [stamp,index]:poses)for (int axis=0 ;axis<6 ;++axis) {
245+ const auto state=mapper->getFramePoses ().at (stamp);Sophus::Vector6d d=Sophus::Vector6d::Zero ();d[axis]=epsilon;
246+ mapper->getFramePoses ().at (stamp).applyInc (d);const auto plus=world (id);
247+ mapper->getFramePoses ().at (stamp)=state;mapper->getFramePoses ().at (stamp).applyInc (-d);const auto minus=world (id);
248+ mapper->getFramePoses ().at (stamp)=state;output.block <3 ,1 >(3 *p,index+axis)=(plus-minus)/(2 *epsilon);
249+ }
250+ auto & point=mapper->lmdb .getLandmark (id);const auto direction=point.direction ;const double inv=point.inv_dist ;
251+ for (int axis=0 ;axis<3 ;++axis) {
252+ if (axis<2 )point.direction [axis]+=epsilon;else point.inv_dist +=epsilon;const auto plus=world (id);
253+ point.direction =direction;point.inv_dist =inv;
254+ if (axis<2 )point.direction [axis]-=epsilon;else point.inv_dist -=epsilon;const auto minus=world (id);
255+ point.direction =direction;point.inv_dist =inv;output.block <3 ,1 >(3 *p,18 +3 *p+axis)=(plus-minus)/(2 *epsilon);
256+ }
257+ }
258+ const Eigen::MatrixXd finite=output*root;return Eigen::MatrixXd (finite*finite.transpose ());
259+ };
260+ posim_slam::NativeMappingWindow window (*mapper,10 ,{10 ,20 ,30 });const auto before=window.snapshot ();
261+ const auto query=window.worldInformation ({42 ,99 });const auto expected=dense ();
262+ assert (query.observed && query.unbounded_gram .norm ()<1e-16 );
263+ assert ((query.covariance -expected).norm ()/expected.norm ()<1e-8 );
264+ assert ((query.positions .head <3 >()-world (42 )).norm ()==0 && window.snapshot ()==before);
265+ const auto single=window.worldInformation ({42 });assert ((single.covariance -query.covariance .topLeftCorner <3 ,3 >()).norm ()<1e-10 );
266+ rejects ([&]{window.worldInformation ({42 ,42 });}," native_window_world_points" );
267+ rejects ([&]{window.worldInformation ({123 });}," native_window_unknown_point" );
268+ auto & infinite=mapper->lmdb .getLandmark (99 );const double inv=infinite.inv_dist ;infinite.inv_dist =0 ;
269+ rejects ([&]{window.worldInformation ({99 });}," native_window_nonfinite_world_point" );infinite.inv_dist =inv;
270+ // Pure translation reference error is shared. Subtracting two world points
271+ // cancels it, while independent per-point treatment leaves an extra I.
272+ Eigen::Matrix<double ,3 ,6 > relative;relative<<Eigen::Matrix3d::Identity (),-Eigen::Matrix3d::Identity ();
273+ const double relative_error=(relative*(query.covariance -expected)*relative.transpose ()).norm ()/
274+ std::max (1 .,(relative*expected*relative.transpose ()).norm ());
275+ assert (relative_error<1e-8 );
276+ const Eigen::Matrix3d wrong=query.covariance .topLeftCorner <3 ,3 >()+query.covariance .bottomRightCorner <3 ,3 >();
277+ assert ((wrong-relative*query.covariance *relative.transpose ()).norm ()>1 .);
278+ // A changed active tangent must still match the original physical reference.
279+ Sophus::Vector6d delta=Sophus::Vector6d::Zero ();delta[0 ]=.004 ;delta[4 ]=.002 ;
280+ mapper->getFramePoses ().at (30 ).applyInc (delta);
281+ posim_slam::NativeMappingWindow changing (*mapper,10 ,{10 ,20 ,30 });
282+ const auto stale=changing.worldLinearization ();
283+ rejects ([&]{window.worldInformation (stale,{42 });}," native_window_stale_world_linearization" );
284+ const auto step=changing.step ();assert (step.accepted );
285+ rejects ([&]{changing.worldInformation (stale,{42 });}," native_window_stale_world_linearization" );
286+ const auto changed=changing.worldInformation ({42 ,99 });const auto changed_expected=dense ();
287+ assert (changed.observed && (changed.covariance -changed_expected).norm ()/changed_expected.norm ()<1e-8 );
288+ mapper->config .vio_init_pose_weight =0 ;
289+ posim_slam::NativeMappingWindow unreferenced (*mapper,10 ,{10 ,20 ,30 });const auto unbounded=unreferenced.worldInformation ({42 ,99 });
290+ assert (!unbounded.observed && unbounded.unbounded_gram .norm ()>1 .);
291+ // Existing owners retain the original weight instead of rereading settings.
292+ assert ((changing.worldInformation ({42 ,99 }).covariance -changed.covariance ).norm ()==0 );
293+ std::cout<<" Native world information: tilted reference, camera extrinsics, shared point covariance, changed chart, missing reference and invalid candidates pass; world covariance relative error "
294+ <<(query.covariance -expected).norm ()/expected.norm ()<<" , relative-point covariance error " <<relative_error<<" .\n " ;
295+ }
192296} // namespace
193297
194298int main (int argc,char ** argv) {
195299 if (argc!=2 )return 2 ;const std::filesystem::path directory=argv[1 ];Fixture fixture;
196300 nativeRowsControls (fixture);
197301 nativeWindowControls (fixture);
302+ nativeWorldControls (fixture);
198303 Registry registry (fixture.calibration ,fixture.config ," fixture-segment" );auto original=fixture.mapper ();
199304 const auto first=registry.capture (fixture.input ,*original,10 ,original->getFramePoses ().at (10 ).getPose (),Sophus::SE3d ());
200305 assert (first->stable_ids .at (42 )==0 && first->stable_ids .at (99 )==1 );
0 commit comments