public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Revert "flip camera rollback"

Browse Source 
This reverts commit db16aec1f5.
This commit is contained in:
gfuse
2021-08-30 12:55:10 +02:00
parent db16aec1f5
commit c40e0ed7ab
5 changed files with 13 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/frame.h
+3 -3
View File
@@ -128,10 +128,10 @@ namespace vio {
//TODO add 15 degree roll orientation //TODO add 15 degree roll orientation
SE2 inv=T_f_w_.inverse(); SE2 inv=T_f_w_.inverse();
Quaterniond q; Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX()) q = AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitX())
* AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitY()) * AngleAxisd(0.261799, Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ()); * AngleAxisd(0.0, Vector3d::UnitZ());
SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0),0.0,inv.translation()(1))); SE3 out(q.toRotationMatrix(),Vector3d(0.0,inv.translation()(0),inv.translation()(1)));
return out; return out;
} }
GPU_version/vio/include/vio/global.h
+3 -3
View File
@@ -92,10 +92,10 @@ namespace vio
SE3 se3() const{ SE3 se3() const{
//Todo add 15 roll orientation //Todo add 15 roll orientation
Quaterniond q; Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX()) q = AngleAxisd(pitch(), Vector3d::UnitX())
* AngleAxisd(pitch(), Vector3d::UnitY()) * AngleAxisd(0.261799, Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ()); * AngleAxisd(0.0, Vector3d::UnitZ());
return SE3(q.toRotationMatrix(),Vector3d(T2_.translation()(0), 0.0,T2_.translation()(1))); return SE3(q.toRotationMatrix(),Vector3d(1e-19,T2_.translation()(0),T2_.translation()(1)));
} }
~SE2_5(){} ~SE2_5(){}
GPU_version/vio/kernel/compute-residual.cl
+3 -3
View File
@@ -31,8 +31,8 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
float R20=-sin(pitch);//-sin(pitch) float R20=-sin(pitch);//-sin(pitch)
float R21=cos(pitch)*sin(roll);//0 float R21=cos(pitch)*sin(roll);//0
float R22=cos(pitch)*cos(roll);//cos(pitch) float R22=cos(pitch)*cos(roll);//cos(pitch)
return (float3)(R00*ref_feature.x+R01*ref_feature.y+R02*ref_feature.z+error.x, return (float3)(R00*ref_feature.x+R01*ref_feature.y+R02*ref_feature.z,
R10*ref_feature.x+R11*ref_feature.y+R12*ref_feature.z, R10*ref_feature.x+R11*ref_feature.y+R12*ref_feature.z+error.x,
R20*ref_feature.x+R21*ref_feature.y+R22*ref_feature.z+error.y); R20*ref_feature.x+R21*ref_feature.y+R22*ref_feature.z+error.y);
/* /*
if(ref.z < 0.0) if(ref.z < 0.0)
@@ -112,7 +112,7 @@ float2 uv_ref_i = floor(uv_ref);
if(uv_ref_i.x - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.y - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.x + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref).x || uv_ref_i.y + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref).y)return; if(uv_ref_i.x - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.y - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.x + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref).x || uv_ref_i.y + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref).y)return;
// evaluate projection jacobian // evaluate projection jacobian
float frame_jac[6]={0.0}; // 2X3 float frame_jac[6]={0.0}; // 2X3
jacobian_xyz2uv(sqrt(pow(ref_feature[f] - (float3)(ref_pose[0].x, 0.0, ref_pose[0].y), 2.0)), &frame_jac); jacobian_xyz2uv(sqrt(pow(ref_feature[f] - (float3)(0.0, ref_pose[0].x, ref_pose[0].y), 2.0)), &frame_jac);
// compute bilateral interpolation weights for reference image // compute bilateral interpolation weights for reference image
float2 subpix= uv_ref - uv_ref_i; float2 subpix= uv_ref - uv_ref_i;
GPU_version/vio/src/depth_filter.cpp
+3 -3
View File
@@ -218,10 +218,10 @@ namespace vio {
SE2 T(it->ftr->frame->T_f_w_.pitch()-frame->T_f_w_.pitch(),it->ftr->frame->T_f_w_.se2().translation()- frame->T_f_w_.se2().translation()); SE2 T(it->ftr->frame->T_f_w_.pitch()-frame->T_f_w_.pitch(),it->ftr->frame->T_f_w_.se2().translation()- frame->T_f_w_.se2().translation());
///TODO add 15 degrees roll orientation ///TODO add 15 degrees roll orientation
Quaterniond q; Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX()) q = AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitX())
* AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY()) * AngleAxisd(0.261799, Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ()); * 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) ); const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
if(xyz_f.z() < 0.0) { if(xyz_f.z() < 0.0) {
++it; // behind the camera ++it; // behind the camera
GPU_version/vio/src/homography.cpp
+1 -1
View File
@@ -92,7 +92,7 @@ namespace vk {
q.w() *= 1/q.norm(); q.w() *= 1/q.norm();
q.x()=0.0; q.x()=0.0;
q.z()=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; return true;
} }