Subversion Repositories seema-scanner

Rev

Rev 44 | Rev 70 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 44 Rev 45
Line 59... Line 59...
59 
    std::vector<cv::Point3f> Q;
 59 
    std::vector<cv::Point3f> Q;
60 
    std::vector<cv::Vec3b> color;
 60 
    std::vector<cv::Vec3b> color;
61 
    algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);
 61 
    algorithm->get3DPoints(calibration, frameSequence.frames0, frameSequence.frames1, Q, color);
62 
 
 62 
 
63 
    // Convert point cloud to PCL format
 63 
    // Convert point cloud to PCL format
64 
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 64 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudPCL(new pcl::PointCloud<pcl::PointXYZRGB>);
65 
 
 65 
 
66 
    pointCloudPCL->width = Q.size();
 66 
    pointCloudPCL->width = Q.size();
67 
    pointCloudPCL->height = 1;
 67 
    pointCloudPCL->height = 1;
68 
    pointCloudPCL->is_dense = false;
 68 
    pointCloudPCL->is_dense = false;
69 
 
 69 
 
70 
    pointCloudPCL->points.resize(Q.size());
 70 
    pointCloudPCL->points.resize(Q.size());
71 
 
 71 
 
72 
    for(int i=0; i<Q.size(); i++){
 72 
    for(int i=0; i<Q.size(); i++){
73 
        pcl::PointXYZRGBNormal point;
 73 
        pcl::PointXYZRGB point;
74 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
 74 
        point.x = Q[i].x; point.y = Q[i].y; point.z = Q[i].z;
75 
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
 75 
        point.r = color[i][0]; point.g = color[i][1]; point.b = color[i][2];
76 
        pointCloudPCL->points[i] = point;
 76 
        pointCloudPCL->points[i] = point;
77 
    }
 77 
    }
78 
 
 78 
 
79 
    // Estimate surface normals
 79 
//    // Estimate surface normals
80 
    pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> ne;
 80 
//    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne;
81 
    pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBNormal>());
 81 
//    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
82 
    ne.setSearchMethod(tree);
 82 
//    ne.setSearchMethod(tree);
83 
    ne.setRadiusSearch(3);
 83 
//    ne.setRadiusSearch(3);
84 
    ne.setViewPoint(0.0, 0.0, 0.0);
 84 
//    ne.setViewPoint(0.0, 0.0, 0.0);
85 
    ne.setInputCloud(pointCloudPCL);
 85 
//    ne.setInputCloud(pointCloudPCL);
86 
    ne.compute(*pointCloudPCL);
 86 
//    ne.compute(*pointCloudPCL);
87 
 
 87 
 
88 
    // Assemble SMPointCloud data structure
 88 
    // Assemble SMPointCloud data structure
89 
    SMPointCloud smPointCloud;
 89 
    SMPointCloud smPointCloud;
- 
 
 90 
    smPointCloud.id = frameSequence.id;
90 
    smPointCloud.pointCloud = pointCloudPCL;
 91 
    smPointCloud.pointCloud = pointCloudPCL;
91 
    smPointCloud.rotationAngle = frameSequence.rotationAngle;
 92 
    smPointCloud.rotationAngle = frameSequence.rotationAngle;
92 
 
 93 
 
93 
    // Determine transform in world (camera0) coordinate system
 94 
    // Determine transform in world (camera0) coordinate system
94 
    float angleRadians = frameSequence.rotationAngle/180.0*M_PI;
 95 
    float angleRadians = frameSequence.rotationAngle/180.0*M_PI;
Line 129... Line 130...
129 
    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
 130 
    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
130 
    cvtools::matToPoints3f(QMat, Q);
 131 
    cvtools::matToPoints3f(QMat, Q);
131 
 
 132 
 
132 
 
 133 
 
133 
}
 134 
}
134 
 
 - 
 
135 
//void SMReconstructionWorker::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
 - 
 
136 
 
 - 
 
137 
//    std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
 - 
 
138 
//    int N = up.rows * up.cols;
 - 
 
139 
 
 - 
 
140 
//    cv::Mat projPointsCam(2, N, CV_32F);
 - 
 
141 
//    uc.reshape(0,1).copyTo(projPointsCam.row(0));
 - 
 
142 
//    vc.reshape(0,1).copyTo(projPointsCam.row(1));
 - 
 
143 
 
 - 
 
144 
//    cv::Mat projPointsProj(2, N, CV_32F);
 - 
 
145 
//    up.reshape(0,1).copyTo(projPointsProj.row(0));
 - 
 
146 
//    vp.reshape(0,1).copyTo(projPointsProj.row(1));
 - 
 
147 
 
 - 
 
148 
//    cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
 - 
 
149 
//    cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
 - 
 
150 
 
 - 
 
151 
//    cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
 - 
 
152 
//    cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
 - 
 
153 
//    cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
 - 
 
154 
//    Pp = cv::Mat(calibration.Kp) * temp;
 - 
 
155 
 
 - 
 
156 
//    cv::Mat xyzw;
 - 
 
157 
//    cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);
 - 
 
158 
 
 - 
 
159 
//    xyz.create(3, N, CV_32F);
 - 
 
160 
//    for(int i=0; i<N; i++){
 - 
 
161 
//        xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);
 - 
 
162 
//        xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);
 - 
 
163 
//        xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);
 - 
 
164 
//    }
 - 
 
165 
 
 - 
 
166 
//    xyz = xyz.t();
 - 
 
167 
//    xyz = xyz.reshape(3, up.rows);
 - 
 
168 
//}
 - 
 
169 
 
 -