Skip to content

Commit d7ab199

Browse files
committed
Preserve shared world-reference uncertainty in native active mapping
1 parent fd3591f commit d7ab199

4 files changed

Lines changed: 229 additions & 8 deletions

File tree

‎native/basalt/README.md‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -158,6 +158,17 @@ reference position and heading select a conditional mean coordinate convention;
158158
they supply no measurement or world covariance. Reference roll and pitch remain
159159
variables. This interface applies to relative/gravity NFR graphs.
160160

161+
`worldInformation()` returns current finite world points and their joint
162+
linearized covariance, including landmark conditional noise and shared old/new
163+
pose errors. It uses the original VIO position/world-z-rotation reference once,
164+
with the coupling to reference roll at tilted attitudes. Up to eight points are
165+
queried jointly; propagate both joint covariance and `unbounded_gram` when
166+
forming relative coordinates. Finite covariance alone cannot certify an
167+
unobserved direction. Zero
168+
reference weight remains unbounded. `worldLinearization()` reuses one solve
169+
across an export; updates invalidate it. These queries do not establish
170+
statistical calibration or replace the existing point-admission rules.
171+
161172
`step()` uses backtracking on the hybrid Gaussian/robust objective, rejects a
162173
step that loses a previously valid projection, and restores poses, landmarks
163174
and the published owner on cancellation or failure. Callers exclusively own
@@ -199,6 +210,15 @@ existing full-history optimizer and limits; automatic rollover is not enabled.
199210
좌표를 선택하며 측정이나 세계 공분산을 추가하지 않습니다. 기준 자세의 롤·피치는
200211
계속 추정합니다. 상대 자세·중력 NFR 그래프에 사용하는 인터페이스입니다.
201212

213+
`worldInformation()`은 현재의 유한한 세계 좌표 지도점과 결합 선형 공분산을
214+
반환합니다. 랜드마크 조건부 잡음과 과거·현재 자세의 공유 오차를 포함합니다.
215+
원래 VIO의 위치·세계 수직축 회전 기준을 한 번 반영하며, 기울어진 기준 자세의
216+
롤과 회전 기준 사이의 결합도 처리합니다. 최대 8점을 함께 조회하며 상대 좌표를
217+
계산할 때 결합 공분산과 `unbounded_gram`을 모두 전파합니다. 유한 공분산만으로
218+
관측 불가 방향을 유효 판정할 수 없습니다. 기준 가중치가 0이면 해당 자유도는 관측
219+
불가로 남습니다. `worldLinearization()`은 내보내기 동안 한 번의 풀이를 재사용하며
220+
갱신 후에는 다시 생성합니다. 통계적 보정이나 기존 점 채택 기준을 대신하지 않습니다.
221+
202222
`step()`은 가우시안·강건 손실을 결합한 목적함수로 갱신 보폭을 조정합니다.
203223
유효했던 투영이 사라지는 갱신은 거부하며, 취소나 실패 시 자세·랜드마크·발행된
204224
정보를 복원합니다. 호출자는 구간 객체가 존재하는 동안 지도를 독점 사용합니다.

‎native/basalt/mapping_graph_probe.cc‎

Lines changed: 105 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -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

194298
int 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);

‎native/basalt/native_mapping_linearization.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -158,7 +158,7 @@ struct NativeMappingPointReduction {
158158
SubmapLinearization::Rows likelihood;
159159
Eigen::MatrixXd backsub;
160160
Eigen::VectorXd offset;
161-
Eigen::MatrixXd unbounded;
161+
Eigen::MatrixXd noise,unbounded;
162162
};
163163
inline std::map<int64_t,int> nativeMappingPoseOffsets(const std::vector<int64_t>& poses) {
164164
std::map<int64_t,int> offsets;
@@ -224,6 +224,7 @@ inline NativeMappingPointReduction reduceNativeMappingPoint(const NativeMappingP
224224
out.likelihood.originals.push_back("pixel:"+std::to_string(stamp)+":"+std::to_string(camera)+":"+std::to_string(feature));
225225
out.likelihood.jacobian=reduced.reduced.jacobian;out.likelihood.residual=reduced.reduced.residual;
226226
out.backsub=reduced.reduced.backsub_matrix;out.offset=reduced.reduced.backsub_offset;
227+
out.noise=reduced.observable_noise_factor;
227228
out.unbounded=reduced.unobservable_basis;
228229
return out;
229230
}

0 commit comments

Comments
 (0)