Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix the seg fault with the tracking lost. #84

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

fix the seg fault with the tracking lost. #84

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5da0843
fix the seg fault with the tracking lost.
blackball Sep 29, 2017
7859849
remove comment
blackball Sep 29, 2017
3ca8bbc
fix indentions and remove auto
blackball Oct 2, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 6 additions & 4 deletions InfiniTAM/ITMLib/Engines/Reconstruction/CPU/ITMSceneReconstructionEngine_CPU.tpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ void ITMSceneReconstructionEngine_CPU<TVoxel, ITMVoxelBlockHash>::IntegrateIntoS
ITMRenderState_VH *renderState_vh = (ITMRenderState_VH*)renderState;

M_d = trackingState->pose_d->GetM();
if (HasNaN(M_d)) return ; // TODO: means the pose is invalid, need to fix that pose
if (TVoxel::hasColorInformation) M_rgb = view->calib.trafo_rgb_to_depth.calib_inv * M_d;

projParams_d = view->calib.intrinsics_d.projectionParamsSimple.all;
Expand Down Expand Up @@ -127,14 +128,14 @@ void ITMSceneReconstructionEngine_CPU<TVoxel, ITMVoxelBlockHash>::AllocateSceneF
ITMRenderState_VH *renderState_vh = (ITMRenderState_VH*)renderState;
if (resetVisibleList) renderState_vh->noVisibleEntries = 0;

M_d = trackingState->pose_d->GetM(); M_d.inv(invM_d);

projParams_d = view->calib.intrinsics_d.projectionParamsSimple.all;
M_d = trackingState->pose_d->GetM(); M_d.inv(invM_d);
projParams_d = view->calib.intrinsics_d.projectionParamsSimple.all;
invProjParams_d = projParams_d;
invProjParams_d.x = 1.0f / invProjParams_d.x;
invProjParams_d.y = 1.0f / invProjParams_d.y;

float mu = scene->sceneParams->mu;
float mu = scene->sceneParams->mu;

float *depth = view->depth->GetData(MEMORYDEVICE_CPU);
int *voxelAllocationList = scene->localVBA.GetAllocationList();
Expand Down Expand Up @@ -346,6 +347,7 @@ void ITMSceneReconstructionEngine_CPU<TVoxel, ITMPlainVoxelArray>::IntegrateInto
Vector4f projParams_d, projParams_rgb;

M_d = trackingState->pose_d->GetM();

if (TVoxel::hasColorInformation) M_rgb = view->calib.trafo_rgb_to_depth.calib_inv * M_d;

projParams_d = view->calib.intrinsics_d.projectionParamsSimple.all;
Expand Down
2 changes: 1 addition & 1 deletion InfiniTAM/ITMLib/Engines/Reconstruction/Shared/ITMSceneReconstructionEngine_Shared.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ _CPU_AND_GPU_CODE_ inline float computeUpdatedVoxelDepthInfo(DEVICEPTR(TVoxel) &
pt_image.x = projParams_d.x * pt_camera.x / pt_camera.z + projParams_d.z;
pt_image.y = projParams_d.y * pt_camera.y / pt_camera.z + projParams_d.w;
if ((pt_image.x < 1) || (pt_image.x > imgSize.x - 2) || (pt_image.y < 1) || (pt_image.y > imgSize.y - 2)) return -1;

// get measured depth from image
depth_measure = depth[(int)(pt_image.x + 0.5f) + (int)(pt_image.y + 0.5f) * imgSize.x];
if (depth_measure <= 0.0f) return -1;
Expand Down
4 changes: 3 additions & 1 deletion InfiniTAM/ITMLib/Trackers/Interface/ITMColorTracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,9 +37,11 @@ void ITMColorTracker::TrackCamera(ITMTrackingState *trackingState, const ITMView
minimizeLM(*this, currentPara);
}

const Matrix4f paraM = currentPara.GetM();
if (HasNaN(paraM)) return ;
// these following will coerce the result back into the chosen
// parameterization for rotations
trackingState->pose_d->SetM(view->calib.trafo_rgb_to_depth.calib * currentPara.GetM());
trackingState->pose_d->SetM(view->calib.trafo_rgb_to_depth.calib * paraM);

trackingState->pose_d->Coerce();

Expand Down
30 changes: 18 additions & 12 deletions InfiniTAM/ORUtils/Matrix.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,12 @@ namespace ORUtils {
T m[s*s];
};

template<class T>
_CPU_AND_GPU_CODE_ inline bool HasNaN(const T &m)
{
return !(m == m);
}

//////////////////////////////////////////////////////////////////////////
// Matrix class with math operators
//////////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -157,16 +163,16 @@ namespace ORUtils {
_CPU_AND_GPU_CODE_ inline Matrix4 &operator -= (const Matrix4 &mat) { for (int i = 0; i < 16; ++i) this->m[i] -= mat.m[i]; return *this; }

_CPU_AND_GPU_CODE_ inline friend bool operator == (const Matrix4 &lhs, const Matrix4 &rhs) {
bool r = lhs[0] == rhs[0];
bool r = lhs.m[0] == rhs.m[0];
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Were these changes motivated by your compiler having trouble with anonymous structs?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is not about anonymous struct. The Matrix class simply doesn't provide a subscript operator. I think this part of codes were not tested before.

for (int i = 1; i < 16; i++)
r &= lhs[i] == rhs[i];
r &= lhs.m[i] == rhs.m[i];
return r;
}

_CPU_AND_GPU_CODE_ inline friend bool operator != (const Matrix4 &lhs, const Matrix4 &rhs) {
bool r = lhs[0] != rhs[0];
bool r = lhs.m[0] != rhs.m[0];
for (int i = 1; i < 16; i++)
r |= lhs[i] != rhs[i];
r |= lhs.m[i] != rhs.m[i];
return r;
}

Expand Down Expand Up @@ -327,16 +333,16 @@ namespace ORUtils {
_CPU_AND_GPU_CODE_ inline Matrix3& operator -= (const Matrix3 &mat) { for (int i = 0; i < 9; ++i) this->m[i] -= mat.m[i]; return *this; }

_CPU_AND_GPU_CODE_ inline friend bool operator == (const Matrix3 &lhs, const Matrix3 &rhs) {
bool r = lhs[0] == rhs[0];
bool r = lhs.m[0] == rhs.m[0];
for (int i = 1; i < 9; i++)
r &= lhs[i] == rhs[i];
r &= lhs.m[i] == rhs.m[i];
return r;
}

_CPU_AND_GPU_CODE_ inline friend bool operator != (const Matrix3 &lhs, const Matrix3 &rhs) {
bool r = lhs[0] != rhs[0];
bool r = lhs.m[0] != rhs.m[0];
for (int i = 1; i < 9; i++)
r |= lhs[i] != rhs[i];
r |= lhs.m[i] != rhs.m[i];
return r;
}

Expand Down Expand Up @@ -429,16 +435,16 @@ namespace ORUtils {
_CPU_AND_GPU_CODE_ inline MatrixSQX<T, s> &operator -= (const MatrixSQX<T, s> &mat) { for (int i = 0; i < s*s; ++i) this->m[i] -= mat.m[i]; return *this; }

_CPU_AND_GPU_CODE_ inline friend bool operator == (const MatrixSQX<T, s> &lhs, const MatrixSQX<T, s> &rhs) {
bool r = lhs[0] == rhs[0];
bool r = lhs.m[0] == rhs.m[0];
for (int i = 1; i < s*s; i++)
r &= lhs[i] == rhs[i];
r &= lhs.m[i] == rhs.m[i];
return r;
}

_CPU_AND_GPU_CODE_ inline friend bool operator != (const MatrixSQX<T, s> &lhs, const MatrixSQX<T, s> &rhs) {
bool r = lhs[0] != rhs[0];
bool r = lhs.m[0] != rhs.m[0];
for (int i = 1; i < s*s; i++)
r |= lhs[i] != rhs[i];
r |= lhs.m[i] != rhs.m[i];
return r;
}

Expand Down