Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

fixed problems in refineGraphSE3Segment

(though it's not used now)
  • Loading branch information...
commit a9a74f37a1157bb51e78dce63b6221669d1b2bbf 1 parent 75ae21b
@mdim mdim authored
Showing with 13 additions and 10 deletions.
  1. +13 −10 src/reconst3d/graph_se3.cpp
View
23 src/reconst3d/graph_se3.cpp
@@ -160,19 +160,20 @@ void refineGraphSE3Segment(const vector<Mat>& odometryPoses,
int startIndex = sortedRefinedFrameIndices[ki-1];
int endIndex = sortedRefinedFrameIndices[ki];
- Mat basePose = partiallyRefinedPoses[startIndex].clone();
-
- int lastValidIndex = startIndex + 1;
+ int basePoseIndex = startIndex;
+ Mat basePose = partiallyRefinedPoses[basePoseIndex];
for(int fi = startIndex + 1; fi <= endIndex; fi++)
{
- if(odometryPoses[fi].empty() || odometryPoses[lastValidIndex].empty())
+ if(odometryPoses[fi].empty())
continue;
- Mat deltaRt = odometryPoses[lastValidIndex].inv() * odometryPoses[fi];
+ CV_Assert(odometryPoses[basePoseIndex].empty());
+
+ Mat deltaRt = odometryPoses[basePoseIndex].inv() * odometryPoses[fi];
rebasedPoses[fi] = basePose * deltaRt;
- basePose = rebasedPoses[fi].clone();
- lastValidIndex = fi;
+ basePoseIndex = fi;
+ basePose = rebasedPoses[basePoseIndex];
}
}
@@ -206,7 +207,8 @@ void refineGraphSE3Segment(const vector<Mat>& odometryPoses,
CV_Assert(addGraphVertex(optimizer, fi, rebasedPoses[fi]));
}
- Mat prevPose = partiallyRefinedPoses[startIndex];
+ int prevPoseIndex = startIndex;
+ Mat prevPose = partiallyRefinedPoses[prevPoseIndex];
int edgesCount = 0;
for(int fi = startIndex + 1; fi <= endIndex; fi++)
{
@@ -214,9 +216,10 @@ void refineGraphSE3Segment(const vector<Mat>& odometryPoses,
continue;
// Add edge with previous frame
- optimizer->addEdge(createEdgeSE3(optimizer->vertex(fi-1), optimizer->vertex(fi),
+ optimizer->addEdge(createEdgeSE3(optimizer->vertex(prevPoseIndex), optimizer->vertex(fi),
prevPose.inv(DECOMP_SVD) * rebasedPoses[fi]));
- prevPose = rebasedPoses[fi];
+ prevPoseIndex = fi;
+ prevPose = rebasedPoses[prevPoseIndex];
edgesCount++;
}
Please sign in to comment.
Something went wrong with that request. Please try again.