void Select2DTool::end (int x, int y, BitMask modifiers, BitMask) { if (!cloud_ptr_) return; final_x_ = x; final_y_ = y; display_box_ = false; // don't select anything if we don't have a selection box. if ((final_x_ == origin_x_) || (final_y_ == origin_y_)) return; GLint viewport[4]; glGetIntegerv(GL_VIEWPORT,viewport); IndexVector indices; GLfloat project[16]; glGetFloatv(GL_PROJECTION_MATRIX, project); Point3DVector ptsvec; cloud_ptr_->getDisplaySpacePoints(ptsvec); for(size_t i = 0; i < ptsvec.size(); ++i) { Point3D pt = ptsvec[i]; if (isInSelectBox(pt, project, viewport)) indices.push_back(i); } if (modifiers & SHFT) { selection_ptr_->addIndex(indices); } else if (modifiers & CTRL) { selection_ptr_->removeIndex(indices); } else { selection_ptr_->clear(); selection_ptr_->addIndex(indices); } cloud_ptr_->setSelection(selection_ptr_); }
Eigen::MatrixXd ComputeP_NormalizedDLT(const Point2DVector& points2D, const Point3DVector& points3D) { unsigned int numberOfPoints = points2D.size(); if(points3D.size() != numberOfPoints) { std::stringstream ss; ss << "ComputeP_NormalizedDLT: The number of 2D points (" << points2D.size() << ") must match the number of 3D points (" << points3D.size() << ")!" << std::endl; throw std::runtime_error(ss.str()); } // std::cout << "ComputeP_NormalizedDLT: 2D points: " << std::endl; // for(Point2DVector::const_iterator iter = points2D.begin(); iter != points2D.end(); ++iter) // { // Point2DVector::value_type p = *iter; // std::cout << p[0] << " " << p[1] << std::endl; // } // std::cout << "ComputeP_NormalizedDLT: 3D points: " << std::endl; // for(Point3DVector::const_iterator iter = points3D.begin(); iter != points3D.end(); ++iter) // { // Point3DVector::value_type p = *iter; // std::cout << p[0] << " " << p[1] << " " << p[2] << std::endl; // } Eigen::MatrixXd similarityTransform2D = ComputeNormalizationTransform<Eigen::Vector2d>(points2D); Eigen::MatrixXd similarityTransform3D = ComputeNormalizationTransform<Eigen::Vector3d>(points3D); // std::cout << "Computed similarity transforms:" << std::endl; // std::cout << "similarityTransform2D: " << similarityTransform2D << std::endl; // std::cout << "similarityTransform3D: " << similarityTransform3D << std::endl; // The (, Eigen::VectorXd()) below are only required when using gnu++0x, it seems to be a bug in Eigen Point2DVector transformed2DPoints(numberOfPoints, Eigen::Vector2d()); Point3DVector transformed3DPoints(numberOfPoints, Eigen::Vector3d()); for(unsigned int i = 0; i < numberOfPoints; ++i) { Eigen::VectorXd point2Dhomogeneous = points2D[i].homogeneous(); Eigen::VectorXd point2Dtransformed = similarityTransform2D * point2Dhomogeneous; transformed2DPoints[i] = point2Dtransformed.hnormalized(); Eigen::VectorXd point3Dhomogeneous = points3D[i].homogeneous(); Eigen::VectorXd point3Dtransformed = similarityTransform3D * point3Dhomogeneous; transformed3DPoints[i] = point3Dtransformed.hnormalized(); //transformed2DPoints[i] = (similarityTransform2D * points2D[i].homogeneous()).hnormalized(); //transformed3DPoints[i] = (similarityTransform3D * points3D[i].homogeneous()).hnormalized(); } // std::cout << "Transformed points." << std::endl; // Compute the Camera Projection Matrix Eigen::MatrixXd A(2*numberOfPoints,12); for(unsigned int i = 0; i < numberOfPoints; ++i) { // First row/equation from the ith correspondence unsigned int row = 2*i; A(row, 0) = 0; A(row, 1) = 0; A(row, 2) = 0; A(row, 3) = 0; A(row, 4) = transformed3DPoints[i](0); A(row, 5) = transformed3DPoints[i](1); A(row, 6) = transformed3DPoints[i](2); A(row, 7) = 1; A(row, 8) = -transformed2DPoints[i](1) * transformed3DPoints[i](0); A(row, 9) = -transformed2DPoints[i](1) * transformed3DPoints[i](1); A(row, 10) = -transformed2DPoints[i](1) * transformed3DPoints[i](2); A(row, 11) = -transformed2DPoints[i](1); // Second row/equation from the ith correspondence row = 2*i+1; A(row, 0) = transformed3DPoints[i](0); A(row, 1) = transformed3DPoints[i](1); A(row, 2) = transformed3DPoints[i](2); A(row, 3) = 1; A(row, 4) = 0; A(row, 5) = 0; A(row, 6) = 0; A(row, 7) = 0; A(row, 8) = -transformed2DPoints[i](0) * transformed3DPoints[i](0); A(row, 9) = -transformed2DPoints[i](0) * transformed3DPoints[i](1); A(row, 10) = -transformed2DPoints[i](0) * transformed3DPoints[i](2); A(row, 11) = -transformed2DPoints[i](0); } // std::cout << "A: " << A << std::endl; Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::MatrixXd V = svd.matrixV(); Eigen::MatrixXd lastColumnOfV = V.col(11); Eigen::MatrixXd P = Reshape(lastColumnOfV, 3, 4); // Denormalization P = similarityTransform2D.inverse()*P*similarityTransform3D; // 3x3 * 3x4 * 4x4 = 4x4 return P; }