diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h index 3cdc53a..477f7d9 100644 --- a/GPU_version/vio/include/vio/frame.h +++ b/GPU_version/vio/include/vio/frame.h @@ -131,7 +131,7 @@ public: q = AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitX()) * AngleAxisd(0.261799, Vector3d::UnitY()) * AngleAxisd(0.0, Vector3d::UnitZ()); - SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0),1e-19,inv.translation()(1))); + SE3 out(q.toRotationMatrix(),Vector3d(0.0,inv.translation()(0),inv.translation()(1))); return out; } diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h index d4305f4..59d1438 100644 --- a/GPU_version/vio/include/vio/global.h +++ b/GPU_version/vio/include/vio/global.h @@ -95,7 +95,7 @@ namespace vio q = AngleAxisd(pitch(), Vector3d::UnitX()) * AngleAxisd(0.261799, Vector3d::UnitY()) * AngleAxisd(0.0, Vector3d::UnitZ()); - return SE3(q.toRotationMatrix(),Vector3d(T2_.translation()(0),1e-19,T2_.translation()(1))); + return SE3(q.toRotationMatrix(),Vector3d(1e-19,T2_.translation()(0),T2_.translation()(1))); } ~SE2_5(){} diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp index 71ca074..947f472 100644 --- a/GPU_version/vio/src/depth_filter.cpp +++ b/GPU_version/vio/src/depth_filter.cpp @@ -221,7 +221,7 @@ void DepthFilter::updateSeeds(FramePtr frame) q = AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitX()) * AngleAxisd(0.261799, Vector3d::UnitY()) * AngleAxisd(0.0, Vector3d::UnitZ()); - SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(T.translation()(0),0.0,T.translation()(1))); + SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(0.0,T.translation()(0),T.translation()(1))); const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) ); if(xyz_f.z() < 0.0) { ++it; // behind the camera diff --git a/GPU_version/vio/src/homography.cpp b/GPU_version/vio/src/homography.cpp index 4044496..87e0850 100644 --- a/GPU_version/vio/src/homography.cpp +++ b/GPU_version/vio/src/homography.cpp @@ -91,7 +91,7 @@ namespace vk { q.w() *= 1/q.norm(); q.x()=0.0; q.z()=0.0; - T_c2_from_c1=Sophus::SE3(q.toRotationMatrix(),Eigen::Vector3d(decompositions.front().T.translation().x(),0.0,decompositions.front().T.translation().z())); + T_c2_from_c1=Sophus::SE3(q.toRotationMatrix(),Eigen::Vector3d(0.0,decompositions.front().T.translation().x(),decompositions.front().T.translation().z())); return true; }