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_);
}
Point3DVector LoadPoints3D(const std::string& filename)
{
std::cout << "LoadPoint3D " << filename << std::endl;
std::string line;
std::ifstream fin(filename.c_str());
Point3DVector points;
if(fin == NULL)
{
std::stringstream ss;
ss << "CameraCalibration:LoadPoints3D Cannot open file " << filename;
throw std::runtime_error(ss.str());
}
while(getline(fin, line))
{
std::stringstream ss;
ss << line;
double p[3];
ss >> p[0] >> p[1] >> p[2];
points.push_back(Eigen::Vector3d (p[0], p[1], p[2]));
}
return points;
}
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;
}
