Permalink
Browse files

fixed transformation of normals in reconst3d

  • Loading branch information...
1 parent a4ec410 commit bf93c17cf60b4365b57a69e85243b460aac515f3 @mdim mdim committed Mar 18, 2013
@@ -12,6 +12,39 @@ void loadFrameData(const std::string& dirname, const std::string& frameIndex, cv
void loadTODLikeBase(const std::string& dirname, std::vector<cv::Mat>& bgrImages,
std::vector<cv::Mat>& depthes32F, std::vector<std::string>* imageFilenames=0);
+// TODO remove the following functions from reconst3d API
+inline
+cv::Point3f rotatePoint(const cv::Point3f& point, const cv::Mat& Rt)
+{
+ CV_Assert(Rt.type() == CV_64FC1);
+ const double * Rt_ptr = Rt.ptr<double>();
+
+ cv::Point3f rotatedPoint;
+ rotatedPoint.x = point.x * Rt_ptr[0] + point.y * Rt_ptr[1] + point.z * Rt_ptr[2];
+ rotatedPoint.y = point.x * Rt_ptr[4] + point.y * Rt_ptr[5] + point.z * Rt_ptr[6];
+ rotatedPoint.z = point.x * Rt_ptr[8] + point.y * Rt_ptr[9] + point.z * Rt_ptr[10];
+ return rotatedPoint;
+}
+
+inline
+cv::Point3f translatePoint(const cv::Point3f& point, const cv::Mat& Rt)
+{
+ CV_Assert(Rt.type() == CV_64FC1);
+ const double * Rt_ptr = Rt.ptr<double>();
+
+ cv::Point3f translatedPoint;
+ translatedPoint.x = point.x + Rt_ptr[3];
+ translatedPoint.y = point.y + Rt_ptr[7];
+ translatedPoint.z = point.z + Rt_ptr[11];
+ return translatedPoint;
+}
+
+inline
+cv::Point3f transformPoint(const cv::Point3f& point, const cv::Mat& Rt)
+{
+ return translatePoint(rotatePoint(point, Rt), Rt);
+}
+
// Find a table mask
class TableMasker: public cv::Algorithm
{
@@ -351,7 +351,6 @@ int computeCorrespsFiltered(const Mat& K, const Mat& K_inv, const Mat& Rt,
if(c != -1)
{
Point3f n0 = normals0.at<Point3f>(v0,u0);
- n0 *= 1./cv::norm(n0);
if(n0.ddot(Oz_inv) < cosMaxNormalsDiff)
{
corresps.at<int>(v0, u0) = -1;
@@ -250,81 +250,58 @@ void refineGraphSE3RgbdICPModel(std::vector<Ptr<RgbdFrame> >& _frames,
if(correspsCounts.at<int>(v0,u0) < minCorrespCount)
continue;
- int u1_, v1_;
- get2shorts(c, u1_, v1_);
+ int u1, v1;
+ get2shorts(c, u1, v1);
const Rect rect(0,0,curCloud.cols, curCloud.rows);
- const int Rad = 0;
- for(int v1 = v1_ - Rad; v1 <= v1_ + Rad; v1++)
+ CV_Assert(rect.contains(Point(u1, v1)) && !cvIsNaN(prevCloud.at<Point3f>(v1,u1).x));
+ Eigen::Vector3d pt_prev, pt_cur, norm_prev, norm_cur, global_norm_prev;
{
- for(int u1 = u1_ - Rad; u1 <= u1_ + Rad; u1++)
- {
- if(rect.contains(Point(u1, v1)) && !cvIsNaN(prevCloud.at<Point3f>(v1,u1).x) &&
- std::abs(prevCloud.at<Point3f>(v1,u1).z - prevCloud.at<Point3f>(v1_,u1_).z) < maxDepthDiff)
- {
- Eigen::Vector3d pt_prev, pt_cur, norm_prev, norm_cur, global_norm_prev;
- {
- pt_prev = cvtPoint_ocv2egn(prevCloud.at<Point3f>(v1,u1));
- norm_prev = cvtPoint_ocv2egn(prevNormals.at<Point3f>(v1,u1));
-
- vector<Point3f> tp;
- perspectiveTransform(vector<Point3f>(1, prevNormals.at<Point3f>(v1,u1)),
- tp, refinedPoses[prevFrameIdx]);
- CV_Assert(isValidDepth(tp[0].z));
- tp[0] *= 1./cv::norm(tp[0]);
- global_norm_prev = cvtPoint_ocv2egn(tp[0]);
-
- perspectiveTransform(vector<Point3f>(1, curCloud.at<Point3f>(v0,u0)),
- tp, refinedPoses[currFrameIdx]);
- CV_Assert(isValidDepth(tp[0].z));
- pt_cur = cvtPoint_ocv2egn(tp[0]);
- perspectiveTransform(vector<Point3f>(1, curNormals.at<Point3f>(v0,u0)),
- tp, refinedPoses[currFrameIdx]);
- tp[0] *= 1./cv::norm(tp[0]);
- CV_Assert(isValidDepth(tp[0].z));
- norm_cur = cvtPoint_ocv2egn(tp[0]);
- }
-
- // add new pose
- if(curVertexIndices.at<int>(v0,u0) == -1)
- {
- g2o::VertexPointXYZ* modelPoint = new g2o::VertexPointXYZ;
- modelPoint->setId(vertexIdx);
- modelPoint->setEstimate(pt_cur);
- modelPoint->setMarginalized(true);
- optimizer->addVertex(modelPoint);
-
- curVertexIndices.at<int>(v0,u0) = vertexIdx;
- vertexIdx++;
- }
-
- int vidx = curVertexIndices.at<int>(v0,u0);
-
- g2o::Edge_V_V_GICPLandmark * e = new g2o::Edge_V_V_GICPLandmark();
- e->setVertex(0, optimizer->vertex(vidx));
- e->setVertex(1, optimizer->vertex(prevFrameIdx));
-
- g2o::EdgeGICP meas;
- meas.pos0 = pt_cur;
- meas.pos1 = pt_prev;
- meas.normal0 = norm_cur;
- meas.normal1 = norm_prev;
-
- e->setMeasurement(meas);
- meas = e->measurement();
+ pt_prev = cvtPoint_ocv2egn(prevCloud.at<Point3f>(v1,u1));
+ norm_prev = cvtPoint_ocv2egn(prevNormals.at<Point3f>(v1,u1));
+
+ global_norm_prev = cvtPoint_ocv2egn(rotatePoint(prevNormals.at<Point3f>(v1,u1), refinedPoses[prevFrameIdx]));
+ pt_cur = cvtPoint_ocv2egn(transformPoint(curCloud.at<Point3f>(v0,u0), refinedPoses[currFrameIdx]));
+ norm_cur = cvtPoint_ocv2egn(rotatePoint(curNormals.at<Point3f>(v0,u0), refinedPoses[currFrameIdx]));
+ }
+
+ // add new pose
+ if(curVertexIndices.at<int>(v0,u0) == -1)
+ {
+ g2o::VertexPointXYZ* modelPoint = new g2o::VertexPointXYZ;
+ modelPoint->setId(vertexIdx);
+ modelPoint->setEstimate(pt_cur);
+ modelPoint->setMarginalized(true);
+ optimizer->addVertex(modelPoint);
+
+ curVertexIndices.at<int>(v0,u0) = vertexIdx;
+ vertexIdx++;
+ }
+
+ int vidx = curVertexIndices.at<int>(v0,u0);
+
+ g2o::Edge_V_V_GICPLandmark * e = new g2o::Edge_V_V_GICPLandmark();
+ e->setVertex(0, optimizer->vertex(vidx));
+ e->setVertex(1, optimizer->vertex(prevFrameIdx));
+
+ g2o::EdgeGICP meas;
+ meas.pos0 = pt_cur;
+ meas.pos1 = pt_prev;
+ meas.normal0 = norm_cur;
+ meas.normal1 = norm_prev;
+
+ e->setMeasurement(meas);
+ meas = e->measurement();
// e->information() = meas.prec0(0.01);
- meas.normal1 = global_norm_prev; // to get global covariation
- e->information() = 0.001 * (meas.cov0(1.).inverse() + meas.cov1(1.).inverse());
- meas.normal1 = norm_prev; // set local normal
+ meas.normal1 = global_norm_prev; // to get global covariation
+ e->information() = 0.001 * (meas.cov0(1.).inverse() + meas.cov1(1.).inverse());
+ meas.normal1 = norm_prev; // set local normal
- g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
- e->setRobustKernel(rk);
+ g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
+ e->setRobustKernel(rk);
- optimizer->addEdge(e);
- }
- }
- }
+ optimizer->addEdge(e);
}
}
}
@@ -365,10 +342,8 @@ void refineGraphSE3RgbdICPModel(std::vector<Ptr<RgbdFrame> >& _frames,
Eigen::Vector3d ep = v->estimate();
p.x = ep[0]; p.y = ep[1]; p.z = ep[2];
}
- vector<Point3f> tp;
- perspectiveTransform(vector<Point3f>(1, p), tp, refinedPoses[frameIdx].inv(DECOMP_SVD));
- CV_Assert(isValidDepth(tp[0].z));
- depth.at<float>(y,x) = tp[0].z;
+ // TODO use new (x,y) using point projection?
+ depth.at<float>(y,x) = transformPoint(p, refinedPoses[frameIdx].inv(DECOMP_SVD)).z;
}
}
View
@@ -135,7 +135,8 @@ void ObjectModel::create(const vector<Ptr<RgbdFrame> >& frames, const vector<Mat
Mat transfNormals;
if(!frame->normals.empty())
{
- perspectiveTransform(frame->normals.reshape(3,1), transfNormals, poses[frameIndex]);
+ const Mat R = poses[frameIndex](Rect(0,0,3,3));
+ transform(frame->normals.reshape(3,1), transfNormals, R);
transfNormals = transfNormals.reshape(3, frame->normals.rows);
}
@@ -151,7 +152,6 @@ void ObjectModel::create(const vector<Ptr<RgbdFrame> >& frames, const vector<Mat
if(!frame->normals.empty())
{
Point3f n = transfNormals.at<Point3f>(y,x);
- n *= 1./cv::norm(n);
normals.push_back(n);
}
}
@@ -49,7 +49,8 @@ void filterFramesByViewHist(vector<Ptr<RgbdFrame> >& frames,
Mat tcloud, tnormals;
perspectiveTransform(cloud.reshape(3,1), tcloud, poses[frameIndex]);
- perspectiveTransform(frames[frameIndex]->normals.reshape(3,1), tnormals, poses[frameIndex]);
+ Mat R = poses[frameIndex](Rect(0,0,3,3));
+ transform(frames[frameIndex]->normals.reshape(3,1), tnormals, R);
cloud = tcloud.reshape(3, frames[frameIndex]->depth.rows);
normals = tnormals.reshape(3, frames[frameIndex]->depth.rows);
@@ -67,12 +68,9 @@ void filterFramesByViewHist(vector<Ptr<RgbdFrame> >& frames,
const Point3f& p = cloud.at<Point3f>(y,x);
CV_Assert(normals.type() == CV_32FC3);
Point3f n = normals.at<Point3f>(y,x);
- n *= 1./cv::norm(n);
if(isValidDepth(p.z) && mask.at<uchar>(y,x))
{
- //CV_Assert(cv::norm(n) > 0.98);
-
transformedPoints3d.push_back(p);
transformedNormals.push_back(n);
pointFrameIndices.push_back(Vec3i(frameIndex, -1, -1));

0 comments on commit bf93c17

Please sign in to comment.