How to get texture coordinates from VTK IntersectWithLine? - c++

I've loaded a texture mapped OBJ via vtkOBJReader and loaded it into a vtkModifiedBSPTree:
auto readerOther(vtkSmartPointer<vtkOBJReader>::New());
auto rawOtherPath(modelPathOther.toLatin1());
readerOther->SetFileName(rawOtherPath.data());
readerOther->Update();
auto meshDataOther(readerOther->GetOutput());
auto bspTreeOther(vtkSmartPointer<vtkModifiedBSPTree>::New());
bspTreeOther->SetDataSet(meshDataOther);
bspTreeOther->BuildLocator();
I then compute my line segment start and end and feed that into
if (bspTreeOther->IntersectWithLine(p1, p2, tolerance, distanceAlongLine, intersectionCoords, pcoords, subId, cellId, cell))
With all the relevant predefined variables of course.
What I need is the texture's UV coordinates at the point of intersection.
I'm so very new to VTK that I've not yet caught the logic of how its put together yet; the abstraction layers are still losing me while I'm digging through the source.
I've hunted for this answer across SO and the VTK users archives and found vague hints given by those who understood VTK deeply to those who were nearly there themselves, and thus of little help to me thus far.
(Appended 11/9/2018)
To clarify, I'm working with non-degenerate triangulated meshes created by a single 3D scanner shot, so quads and other higher polygons are not going to be ever seen by my code. A general solution should account for such things, but that can be accomplished via triangulating the mesh first via a good application of handwavium.

Code
Note that if one vertex belongs to several polygons and has different texture coordinates, VTK will create duplicates of the vertex.
I don't use vtkCleanPolyData, because VTK will merge such "duplicates" and we will lose needed information, as far as I know.
I use vtkCellLocator instead of vtkModifiedBSPTree,
because in my case it was faster.
The main file main.cpp.
You can find magic numbers in start and end arrays — these are your p1 and p2.
I've set these values just for example
#include <vtkSmartPointer.h>
#include <vtkPointData.h>
#include <vtkCellLocator.h>
#include <vtkGenericCell.h>
#include <vtkOBJReader.h>
#include <vtkTriangleFilter.h>
#include <vtkMath.h>
#include <iostream>
int main(int argc, char * argv[])
{
if (argc < 2)
{
std::cerr << "Usage: " << argv[0] << " OBJ_file_name" << std::endl;
return EXIT_FAILURE;
}
auto reader{vtkSmartPointer<vtkOBJReader>::New()};
reader->SetFileName(argv[1]);
reader->Update();
// Triangulate the mesh if needed
auto triangleFilter{vtkSmartPointer<vtkTriangleFilter>::New()};
triangleFilter->SetInputConnection(reader->GetOutputPort());
triangleFilter->Update();
auto mesh{triangleFilter->GetOutput()};
// Use `auto mesh(reader->GetOutput());` instead if no triangulation needed
// Build a locator to find intersections
auto locator{vtkSmartPointer<vtkCellLocator>::New()};
locator->SetDataSet(mesh);
locator->BuildLocator();
// Initialize variables needed for intersection calculation
double start[3]{-1, 0, 0.5};
double end[3]{ 1, 0, 0.5};
double tolerance{1E-6};
double relativeDistanceAlongLine;
double intersectionCoordinates[3];
double parametricCoordinates[3];
int subId;
vtkIdType cellId;
auto cell{vtkSmartPointer<vtkGenericCell>::New()};
// Find intersection
int intersected = locator->IntersectWithLine(
start,
end,
tolerance,
relativeDistanceAlongLine,
intersectionCoordinates,
parametricCoordinates,
subId,
cellId,
cell.Get()
);
// Get points of intersection cell
auto pointsIds{vtkSmartPointer<vtkIdList>::New()};
mesh->GetCellPoints(cellId, pointsIds);
// Store coordinates and texture coordinates of vertices of the cell
double meshTrianglePoints[3][3];
double textureTrianglePoints[3][2];
auto textureCoordinates{mesh->GetPointData()->GetTCoords()};
for (unsigned pointNumber = 0; pointNumber < cell->GetNumberOfPoints(); ++pointNumber)
{
mesh->GetPoint(pointsIds->GetId(pointNumber), meshTrianglePoints[pointNumber]);
textureCoordinates->GetTuple(pointsIds->GetId(pointNumber), textureTrianglePoints[pointNumber]);
}
// Normalize the coordinates
double movedMeshTrianglePoints[3][3];
for (unsigned i = 0; i < 3; ++i)
{
movedMeshTrianglePoints[0][i] = 0;
movedMeshTrianglePoints[1][i] =
meshTrianglePoints[1][i] -
meshTrianglePoints[0][i];
movedMeshTrianglePoints[2][i] =
meshTrianglePoints[2][i] -
meshTrianglePoints[0][i];
}
// Normalize the texture coordinates
double movedTextureTrianglePoints[3][2];
for (unsigned i = 0; i < 2; ++i)
{
movedTextureTrianglePoints[0][i] = 0;
movedTextureTrianglePoints[1][i] =
textureTrianglePoints[1][i] -
textureTrianglePoints[0][i];
movedTextureTrianglePoints[2][i] =
textureTrianglePoints[2][i] -
textureTrianglePoints[0][i];
}
// Calculate SVD of a matrix consisting of normalized vertices
double U[3][3];
double w[3];
double VT[3][3];
vtkMath::SingularValueDecomposition3x3(movedMeshTrianglePoints, U, w, VT);
// Calculate pseudo inverse of a matrix consisting of normalized vertices
double pseudoInverse[3][3]{0};
for (unsigned i = 0; i < 3; ++i)
{
for (unsigned j = 0; j < 3; ++j)
{
for (unsigned k = 0; k < 3; ++k)
{
if (w[k] != 0)
{
pseudoInverse[i][j] += VT[k][i] * U[j][k] / w[k];
}
}
}
}
// Calculate interpolation matrix
double interpolationMatrix[3][2]{0};
for (unsigned i = 0; i < 3; ++i)
{
for (unsigned j = 0; j < 2; ++j)
{
for (unsigned k = 0; k < 3; ++k)
{
interpolationMatrix[i][j] += pseudoInverse[i][k] * movedTextureTrianglePoints[k][j];
}
}
}
// Calculate interpolated texture coordinates of the intersection point
double interpolatedTexturePoint[2]{textureTrianglePoints[0][0], textureTrianglePoints[0][1]};
for (unsigned i = 0; i < 2; ++i)
{
for (unsigned j = 0; j < 3; ++j)
{
interpolatedTexturePoint[i] += (intersectionCoordinates[j] - meshTrianglePoints[0][j]) * interpolationMatrix[j][i];
}
}
// Print the result
std::cout << "Interpolated texture coordinates";
for (unsigned i = 0; i < 2; ++i)
{
std::cout << " " << interpolatedTexturePoint[i];
}
std::cout << std::endl;
return EXIT_SUCCESS;
}
CMake project file CMakeLists.txt
cmake_minimum_required(VERSION 3.1)
PROJECT(IntersectInterpolate)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(VTK REQUIRED)
include(${VTK_USE_FILE})
add_executable(IntersectInterpolate MACOSX_BUNDLE main.cpp)
if(VTK_LIBRARIES)
target_link_libraries(IntersectInterpolate ${VTK_LIBRARIES})
else()
target_link_libraries(IntersectInterpolate vtkHybrid vtkWidgets)
endif()
Math
What we need
Suppose you have a mesh consisting of triangles and your vertices have texture coordinates.
Given vertices of a triangle A, B and C, corresponding texture coordinates A', B' and C', you want to find a mapping (to interpolate) from another inner and boundary points of the triangle to the texture.
Let's make some rational assumptions:
Points A, B, C should correspond to their texture coordinates A', B', C';
Each point X on the border, say AB, should correspond to the points of A'B' line in following way: |AX| / |AB| = |A'X'| / |A'B'| — half way on the original triangle should be a half way on the texture map;
Centroid of the triangle (A + B + C) / 3 should correspond to centroid of the texture triangle (A' + B' + C') / 3.
Equations to solve
Looks like we want to have affine mapping: coordinates of vertices of the original triangle should be multiplied by some coefficients and be added to some constants.
Let's construct the system of equations
Ax * Mxx + Ay * Myx + Az * Mzx + M0x = A'x
Ax * Mxy + Ay * Myy + Az * Mzy + M0y = A'y
Ax * Mxz + Ay * Myz + Az * Mzz + M0z = 0
and the same for B and C.
You can see that we have 9 equations and 12 unknowns.
Though, equations containing Miz (for i in {x, y, z}) have the solution 0 and don't play any role in further computations, so we can just set them equal to 0.
Thus, we have system with 6 equations and 8 unknowns
Ax * Mxx + Ay * Myx + Az * Mzx + M0x = A'x
Ax * Mxy + Ay * Myy + Az * Mzy + M0y = A'y
Let's write entire system in matrix view
-- -- -- -- -- --
| 1 Ax Ay Az | | M0x M0y | | A'x A'y |
| 1 Bx By Bz | x | Mxx Mxy | = | B'x B'y |
| 1 Cx Cy Cz | | Myx Myy | | C'x C'y |
-- -- | Mzx Mzy | -- --
-- --
I subtract coordinates of A vertex from B and C
and texture coordinates A' from B' and C',
and now we have the triangle with the first vertex located
in start of coordinate system as well as corresponding texture coordinates.
This means that now triangles are not translated (moved) one relative to another
and we don't need M0 part of interpolation matrix
-- -- -- -- -- --
| Bx By Bz | | Mxx Mxy | | B'x B'y |
| Cx Cy Cz | x | Myx Myy | = | C'x C'y |
-- -- | Mzx Mzy | -- --
-- --
Solution
Let's call the first matrix P, the second M and the last one T
P M = T
The matrix P is not square.
If we add zero row to it, the matrix becomes singular.
So, we have to calculate pseudo-inverse of it in order to solve the equation.
There's no function for calculating pseudo-inverse matrix in VTK.
We go to Moore–Penrose inverse article on Wikipedia and see that it can be calculated using SVD.
VTKMath::SingularValueDecomposition3x3 function allows us to do it.
The function gives us U, S and VT matrices.
I'll write pseudo-inverse of matrix P as P",
transposition of U as UT and transposition of VT as V.
Pseudo-inverse of diagonal matrix S is a matrix with 1 / Sii elements
where Sii is not a zero and 0 for zero elements
P = U S VT
P" = V S" UT
M = P" T
Usage
To apply interpolation matrix,
we need to not forget that we need to translate input and output vectors.
A' is a 2D vector of texture coordinates of the first vertex in the triangle,
A is a 3D vector of coordinates of the vertex,
M is the found interpolation matrix,
p is a 3D intersection point we want to get texture coordinates for,
t' is the resulting 2D vector with interpolated texture coordinates
t' = A' + (p - A) M

[Rewritten 2019/5/7 to reflect an updated understanding.]
After finding out that the parametric coordinates are inputs to a function from which one can get barycentric coordinates in the case of triangular cells, and then learning about what barycentric coordinates are, I was able to work out the following.
const auto readerOther(vtkSmartPointer<vtkOBJReader>::New());
const auto rawOtherPath(modelPathOther.toLatin1());
readerOther->SetFileName(rawOtherPath.data());
readerOther->Update();
const auto meshDataOther(readerOther->GetOutput());
const auto bspTreeOther(vtkSmartPointer<vtkModifiedBSPTree>::New());
bspTreeOther->SetDataSet(meshDataOther);
bspTreeOther->BuildLocator();
double point1[3]{0.0, 0.0, 0.0}; // start of line segment used to intersect the model.
double point2[3]{0.0, 0.0, 10.0}; // end of line segment
double distanceAlongLine;
double intersectionCoords[3]; // The coordinate of the intersection.
double parametricCoords[3]; // Parametric Coordinates of the intersection - see https://lorensen.github.io/VTKExamples/site/VTKBook/08Chapter8/#82-interpolation-functions
int subId; // ?
vtkIdType cellId;
double intersectedTextureCoords[2];
if (bspTreeOther->IntersectWithLine(point1, point2, TOLERANCE, distanceAlongLine, intersectionCoords, parametricCoords, subId, cellId))
{
const auto textureCoordsOther(meshDataOther->GetPointData()->GetTCoords());
const auto pointIds{meshDataOther->GetCell(cellId)->GetPointIds()};
const auto vertexIndex0{pointIds->GetId(0)};
const auto vertexIndex1{pointIds->GetId(1)};
const auto vertexIndex2{pointIds->GetId(2)};
double texCoord0[2];
double texCoord1[2];
double texCoord2[2];
textureCoordsOther->GetTuple(vertexIndex0, texCoord0);
textureCoordsOther->GetTuple(vertexIndex1, texCoord1);
textureCoordsOther->GetTuple(vertexIndex2, texCoord2);
const auto parametricR{parametricCoords[0]};
const auto parametricS{parametricCoords[1]};
const auto barycentricW0{1 - parametricR - parametricS};
const auto barycentricW1{parametricR};
const auto barycentricW2{parametricS};
intersectedTextureCoords[0] =
barycentricW0 * texCoord0[0] +
barycentricW1 * texCoord1[0] +
barycentricW2 * texCoord2[0];
intersectedTextureCoords[1] =
barycentricW0 * texCoord0[1] +
barycentricW1 * texCoord1[1] +
barycentricW2 * texCoord2[1];
}
Please note that this code is an interpretation of the actual code I'm using; I'm using Qt and its QVector2D and QVector3D classes along with some interpreter glue functions to go to and from arrays of doubles.
See https://lorensen.github.io/VTKExamples/site/VTKBook/08Chapter8 for details about the parametric coordinate systems of various cell types.

Related

How to determine the axis of rotation from a set of point clouds

I have a number of point clouds taken from an kinect-like instrument that is mounted on a tripod and then rotated. How do I determine the rotation axis accurately? I'm using c++ PCL and Eigen.
I can match the point clouds together with ICP and run a global registration (SLAM or ELCH) to get combined point cloud but for a number of reasons I would like to be able to determine the axis accurately and force the registrations to respect this rotation.
One issue that is related to this problem is the origin of my instrument. I can measure the distance to the rotation axis from the external dimensions of the device fairly accurately but I don't know exactly where is the origin in relation the extremities of the device. Solving this issue could help me to locate the origin too.
There are two methods that I'm considering.
One is to take the transformation matrices of the registered point clouds and extract the translation vectors that represent the locations where the transformation would project the internal origin in the current position. To the set of points acquired this way I could try to fit a circle and the center point would represent a vector from the origin to the rotation axis and the normal direction of the circle would be the direction of the axis.
The other option is to determine the rotation axis directly from any single rotation matrix, but the vector to the rotation axis seems volatile.
Any better solutions or insights on the issue?
You need to calculate the major axis oft the tensor of inertia. https://en.m.wikipedia.org/wiki/Moment_of_inertia.
All points can be considered to have the same mass. Then you can use the Steiner approach.
THIS IS NOT AN ANSWER TO THE ORIGINAL QUESTION BUT CLARIFICATION TO A DISCUSSION ABOUT DEFINING THE ROTATION AXIS BETWEEN TWO POSES
Deepfreeze. Here is an Octave script to demonstrate what we discussed in the chat. I know it might be bad practice to post an answer when not presenting one, but I hope this will give you an insight on what I was trying to explain about the relationship between the translation vector and the point on the rotation axis (t_i in your notation).
rotation_axis_unit = [0,0,1]' % parallel to z axis
angle = 1 /180 *pi() % one degree
% a rotaion matrix of one degree rotation
R = [cos(angle) -sin(angle) 0 ; sin(angle) cos(angle) 0 ; 0 0 1 ];
% the point around wich to rotate
axis_point = [-10,0,0]' % a point
% just a point used to demonstrate that all points form a circular path
test_point = [10,5,0]';
%containers for plotting
path_origin = zeros(360,3);
path_test_point = zeros(360,3);
path_v = zeros(360,3);
origin = [0,0,0]';
% updating rotation matrix
R_i = R;
M1 = [R,R*-axis_point.+axis_point;[0,0,0,1]];
% go around a full circle
for i=1:360
% v = the last column of M. Created from axis_point.
% -axis_point is the vector from axis_point to origin which is being rotated
% then a correction is applied to center it around the axis point
v = R_i * -axis_point .+ axis_point;
% building 4x4 transformation matrix
M = [R_i, v;[0,0,0,1]];
% M could also be built M_i = M1 * M_i, rotating the previous M by one degree around axis_point
% rotatin testing point and saving it
test_point_i = M * [test_point;1];
path_test_point(i,:) = test_point_i(1:3,1)';
% saving the translation part of M
path_v(i,:) = v';
% rotating origin point and saving it
origin_i = test_point_i = M * [origin;1];
path_origin(i,:) = origin_i(1:3,1)';
R_i = R * R_i ;
end
figure(1)
% plot test point path, just to show it forms a circular path, center and axis_point
scatter3(path_test_point(:,1), path_test_point(:,2), path_test_point(:,3), 5,'r')
hold on
% plotting origin path, circular, center at axis_point
scatter3(path_origin(:,1), path_origin(:,2), path_origin(:,3), 7,'r')
hold on
% plotting translation vectors, identical to origin path, (if invisible rotate so that you are watching it from z axis direction)
scatter3(path_v(:,1), path_v(:,2), path_v(:,3), 1, 'black');
hold on
% plots for visual analysis
scatter3(0,0,0,5,'b') % origin
hold on
scatter3(axis_point(1), axis_point(2), axis_point(3), 5, 'g') % axis point, center of the circles
hold on
scatter3(test_point(1), test_point(2), test_point(3), 5, 'black') % test point origin
hold off
% what does this demonstrate?
% it shows that that the ralationship between a 4x4
% transformation matrix and axis_angle notation plus point on the rotation axis
% M = [ R, v,; [0,0,0,1]] = [ R_i , R_i * -c + c; [ 0, 0, 0, 1] ]
%
% where c equals axis_point ( = perpendicular vector from origin to the axis of rotation )
% pay attention to path_v and path_origin
% they are identical
% path_v was extracted from the 4x4 transformation matrix M
% and path_origin was created by rotating the origin point by M
%--> v = R_i * -.c +.c
% Notice that since M describes a rotation of alpha angles around an
% axis that goes through c
% and its translation vector lies on a circle whose center
% is at the rotation_axis and radius is the distance from that
% point to origin ->
%
% M * M will describe a rotation of 2 x alpha angles around the same axis
% Therefore you can easily create more points that lay on that circle
% by multiplying M by itself and extracting the translation vector
%
% c can then be solved by normal circle fit algorithms.
%------------------------------------------------------------
% CAUTION !!!
% this applies perfectly when the transformation matrices have been created so
% that the translation is perfectly orthogonal to the rotation axis
% in real world matrices the translation will not be orthogonal
% therefore the points will not travel on a circular path but on a helix and this needs to be
% dealt with when solving the center of rotation.
An option is to place a chessboard at ~1 [m]. Use the kinect camera to make images for different rotations, where the hole chessboard is still visible. Fit the chessboard using OpenCV.
The goal is to find the xyz coordinates of the chessboard for the different orientations. Use your camera api functions to determine the xyz coordinates of the chessboard or do the following:
Determine camera intrinsics of camera 1 and 2. (use both color and IR images for kinect).
Determine camera extrinsics (camera 2 [R,t] wrt camera 1)
Use the values to calculate projection matrices
Use the projection matrices to triangulate the points of the chessboard to get coordinates in [X,Y,Z] wrt camera1 coordinate system.
Each group of chessboard points is called [x_i]. Now we can write the equation:
Update:
This equation can be solved with a non-linear solver, I used ceres-solver.
#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include "glog/logging.h"
#include "opencv2/opencv.hpp"
#include "csv.h"
#include "Eigen/Eigen"
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
struct AxisRotationError {
AxisRotationError(double observed_x0, double observed_y0, double observed_z0, double observed_x1, double observed_y1, double observed_z1)
: observed_x0(observed_x0), observed_y0(observed_y0), observed_z0(observed_z0), observed_x1(observed_x1), observed_y1(observed_y1), observed_z1(observed_z1) {}
template <typename T>
bool operator()(const T* const axis, const T* const angle, const T* const trans, T* residuals) const {
//bool operator()(const T* const axis, const T* const trans, T* residuals) const {
// Normalize axis
T a[3];
T k = axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2];
a[0] = axis[0] / sqrt(k);
a[1] = axis[1] / sqrt(k);
a[2] = axis[2] / sqrt(k);
// Define quaternion from axis and angle. Convert angle to radians
T pi = T(3.14159265359);
//T angle[1] = {T(10.0)};
T quaternion[4] = { cos((angle[0]*pi / 180.0) / 2.0),
a[0] * sin((angle[0] * pi / 180.0) / 2.0),
a[1] * sin((angle[0] * pi / 180.0) / 2.0),
a[2] * sin((angle[0] * pi / 180.0) / 2.0) };
// Define transformation
T t[3] = { trans[0], trans[1], trans[2] };
// Calculate predicted positions
T observedPoint0[3] = { T(observed_x0), T(observed_y0), T(observed_z0)};
T point[3]; point[0] = observedPoint0[0] - t[0]; point[1] = observedPoint0[1] - t[1]; point[2] = observedPoint0[2] - t[2];
T rotatedPoint[3];
ceres::QuaternionRotatePoint(quaternion, point, rotatedPoint);
T predicted_x = rotatedPoint[0] + t[0];
T predicted_y = rotatedPoint[1] + t[1];
T predicted_z = rotatedPoint[2] + t[2];
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - T(observed_x1);
residuals[1] = predicted_y - T(observed_y1);
residuals[2] = predicted_z - T(observed_z1);
return true;
}
// Factory to hide the construction of the CostFunction object from
// the client code.
static ceres::CostFunction* Create(const double observed_x0, const double observed_y0, const double observed_z0,
const double observed_x1, const double observed_y1, const double observed_z1) {
// Define AutoDiffCostFunction. <AxisRotationError, #residuals, #dim axis, #dim angle, #dim trans
return (new ceres::AutoDiffCostFunction<AxisRotationError, 3, 3, 1,3>(
new AxisRotationError(observed_x0, observed_y0, observed_z0, observed_x1, observed_y1, observed_z1)));
}
double observed_x0;
double observed_y0;
double observed_z0;
double observed_x1;
double observed_y1;
double observed_z1;
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
// Load points.csv into cv::Mat's
// 216 rows with (x0, y0, z0, x1, y1, z1)
// [x1,y1,z1] = R* [x0-tx,y0-ty,z0-tz] + [tx,ty,tz]
// The xyz coordinates are points on a chessboard, where the chessboard
// is rotated for 4x. Each chessboard has 54 xyz points. So 4x 54,
// gives the 216 rows of observations.
// The chessboard is located at [0,0,1], as the camera_0 is located
// at [-0.1,0,0], the t should become [0.1,0,1.0].
// The chessboard is rotated around axis [0.0,1.0,0.0]
io::CSVReader<6> in("points.csv");
float x0, y0, z0, x1, y1, z1;
// The observations
cv::Mat x_0(216, 3, CV_32F);
cv::Mat x_1(216, 3, CV_32F);
for (int rowNr = 0; rowNr < 216; rowNr++){
if (in.read_row(x0, y0, z0, x1, y1, z1))
{
x_0.at<float>(rowNr, 0) = x0;
x_0.at<float>(rowNr, 1) = y0;
x_0.at<float>(rowNr, 2) = z0;
x_1.at<float>(rowNr, 0) = x1;
x_1.at<float>(rowNr, 1) = y1;
x_1.at<float>(rowNr, 2) = z1;
}
}
std::cout << x_0(cv::Rect(0, 0, 2, 5)) << std::endl;
// The variable to solve for with its initial value. It will be
// mutated in place by the solver.
int numObservations = 216;
double axis[3] = { 0.0, 1.0, 0.0 };
double* pAxis; pAxis = axis;
double angles[4] = { 10.0, 10.0, 10.0, 10.0 };
double* pAngles; pAngles = angles;
double t[3] = { 0.0, 0.0, 1.0,};
double* pT; pT = t;
bool FLAGS_robustify = true;
// Build the problem.
Problem problem;
// Set up the only cost function (also known as residual). This uses
// auto-differentiation to obtain the derivative (jacobian).
for (int i = 0; i < numObservations; ++i) {
ceres::CostFunction* cost_function =
AxisRotationError::Create(
x_0.at<float>(i, 0), x_0.at<float>(i, 1), x_0.at<float>(i, 2),
x_1.at<float>(i, 0), x_1.at<float>(i, 1), x_1.at<float>(i, 2));
//std::cout << "pAngles: " << pAngles[i / 54] << ", " << i / 54 << std::endl;
ceres::LossFunction* loss_function = FLAGS_robustify ? new ceres::HuberLoss(0.001) : NULL;
//ceres::LossFunction* loss_function = FLAGS_robustify ? new ceres::CauchyLoss(0.002) : NULL;
problem.AddResidualBlock(cost_function, loss_function, pAxis, &pAngles[i/54], pT);
//problem.AddResidualBlock(cost_function, loss_function, pAxis, pT);
}
// Run the solver!
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
options.num_threads = 4;
options.use_nonmonotonic_steps = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
//std::cout << summary.FullReport() << "\n";
std::cout << summary.BriefReport() << "\n";
// Normalize axis
double k = axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2];
axis[0] = axis[0] / sqrt(k);
axis[1] = axis[1] / sqrt(k);
axis[2] = axis[2] / sqrt(k);
// Plot results
std::cout << "axis: [ " << axis[0] << "," << axis[1] << "," << axis[2] << " ]" << std::endl;
std::cout << "t: [ " << t[0] << "," << t[1] << "," << t[2] << " ]" << std::endl;
std::cout << "angles: [ " << angles[0] << "," << angles[1] << "," << angles[2] << "," << angles[3] << " ]" << std::endl;
return 0;
}
The result I've got:
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 3.632073e-003 0.00e+000 3.76e-002 0.00e+000 0.00e+000 1.00e+004 0 4.30e-004 7.57e-004
1 3.787837e-005 3.59e-003 2.10e-003 1.17e-001 1.92e+000 3.00e+004 1 7.43e-004 8.55e-003
2 3.756202e-005 3.16e-007 1.73e-003 5.49e-001 1.61e-001 2.29e+004 1 5.35e-004 1.13e-002
3 3.589147e-005 1.67e-006 2.90e-004 9.19e-002 9.77e-001 6.87e+004 1 5.96e-004 1.46e-002
4 3.584281e-005 4.87e-008 1.38e-005 2.70e-002 1.00e+000 2.06e+005 1 4.99e-004 1.73e-002
5 3.584268e-005 1.35e-010 1.02e-007 1.63e-003 1.01e+000 6.18e+005 1 6.32e-004 2.01e-002
Ceres Solver Report: Iterations: 6, Initial cost: 3.632073e-003, Final cost: 3.584268e-005, Termination: CONVERGENCE
axis: [ 0.00119037,0.999908,-0.0134817 ]
t: [ 0.0993185,-0.0080394,1.00236 ]
angles: [ 9.90614,9.94415,9.93216,10.1119 ]
The angles result is quite nice 10 degrees. These can even be fixed for my case, as I know the rotation very accurately from my rotation stage. There is a small difference in the t and axis. This is cause by inaccuracies in my virtual stereoCamera simulation. My chessboard squares are not exactly square and the dimensions are also a little off....
My simulation scripts, images, results: blender_simulation.zip

Rotate points openGL

I've trying to generate figures by revolution reading the profile of figure from a ply. I follow the steps from here and other similar questioons, but my problem persist. When I try to rotate a Ply with only 2 points 36 stesps I get this if I puth the camara on the top the cilinder:
My code after a revision of the method rotate is:
void Figura::rotateY(int ngiros){
//Variables de rotacion.
//double alfa = 2*M_PI/ngiros;
int long_perfil = vertices.size();
vector<_vertex3f> new_vertices;
cout << long_perfil << " vertices" << endl;
_vertex3f aux1, aux2;
for(int i=0; i < ngiros; i++){
double alfa = (2*M_PI/ngiros)*i;
for(int j=0; j < long_perfil; j++){
aux1 = vertices.at(j);
aux1._0 = (cos(alfa) * aux1._0) + (sin(alfa) * aux1._2);
aux1._2 = (cos(alfa) * aux1._2) - (sin(alfa) * aux1._0);
vertices.push_back(aux1);
}
}
//vertices.clear();
//vertices = new_vertices;
//caras
for(int i=0; i < vertices.size(); i++){
_vertex3i aux(i, i+1, i+long_perfil);
_vertex3i aux2(i, i+long_perfil+1, i+1);
caras.push_back(aux);
caras.push_back(aux2);
}
}
}
I can't find my error. Some help will be welcome.
It looks like you're unclear over what coordinate system your original curve is in, and how you're applying a rotation to it. With your current code, you're just rotating the points by a variable amount, but keep them all within the same plane. You can tell from just looking at the code superficially: You never set a value for the y-coordinate of any of the points, so the whole result is not a 3D shape, but entirely in the y=0 plane. As flat as a pancake...
Another thing you need to be careful about is that you don't modify values while you're still using the old value:
aux1._0 = (cos(alfa) * aux1._0) + (sin(alfa) * aux1._2);
aux1._2 = (cos(alfa) * aux1._2) - (sin(alfa) * aux1._0);
Here, you're modifying the value of aux1._0 in the first statement, while the second statement should in fact still use the old value.
Let's say your original curve is in the x/y-plane, and you want to rotate around the y-axis. For this to result in a nice 3D shape, all the x-coordinates of the curve should be positive:
^ y
|
|--
| \
| \_
| | x
--------------->
| |
| /
| /
| _/
|/
Picture the z-axis pointing out of the screen.
Now, to rotate this curve by a given angle alpha around the y-axis, we leave the y-coordinate unchanged, and rotate the point (x, 0) by alpha within the xz-plane to get the new values for x and z. The new coordinates (x', y', z') for input point (x, y) of the shape are then:
x' = x * cos(alpha)
y' = y
z' = x * sin(alpha)
As a modified version of your code:
for(int i=0; i < ngiros; i++){
double alfa = (2*M_PI/ngiros)*i;
for(int j=0; j < long_perfil; j++){
aux1 = vertices.at(j);
aux2._0 = cos(alfa) * aux1._0;
aux2._1 = aux1._1;
aux2._2 = sin(alfa) * aux1._0;
vertices.push_back(aux2);
}
}

Suggestions to Compute the Intersetions of Multiple Convex 2D Polygons

I am writing this question fishing for any state-of-the-art software or methods that can quickly compute the intersection of N 2D polygons (the convex hulls of projected convex polyhedrons), and M 2D polygons where typically N >> M. N may be in the order or at least 1M polygons and N in the order 50k. I've searched for some time now, but I keep coming up with the same answer shown below.
Use boost and a loop to
compute the projection of the polyhedron (not the bottleneck)
compute the convex hull of said polyhedron (bottleneck)
compute the intersection of the projected polyhedron and existing 2D polygon (major bottleneck).
This loop is repeated NK times where typically K << M, and K is the average number of 2D polygons intersecting a single projected polyhedron. This is done to reduce the number of computations.
The problem with this is that if I have N=262144 and M=19456 it takes about 129 seconds (when multithreaded by polyhedron), and this must be done about 300 times. Ideally, I would like to reduce the computation time to about 1 second for the above sizes, so I was wondering if someone could help point to some software or literature that could improve efficiency.
[EDIT]
#sehe's request I'm posting the most relevant parts of the code. I haven't compiled it, so this is just to get the gist... this code assumes, there are voxels and pixels, but the shapes can be anything. The order of the points in the grid can be any, but the indices of where the points reside in the grid are the same.
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/ring.hpp>
const std::size_t Dimension = 2;
typedef boost::geometry::model::point<float, Dimension, boost::geometry::cs::cartesian> point_2d;
typedef boost::geometry::model::polygon<point_2d, false /* is cw */, true /* closed */> polygon_2d;
typedef boost::geometry::model::box<point_2d> box_2d;
std::vector<float> getOverlaps(std::vector<float> & projected_grid_vx, // projected voxels
std::vector<float> & pixel_grid_vx, // pixels
std::vector<int> & projected_grid_m, // number of voxels in each dimension
std::vector<int> & pixel_grid_m, // number of pixels in each dimension
std::vector<float> & pixel_grid_omega, // size of the pixel grid in cm
int projected_grid_size, // total number of voxels
int pixel_grid_size) { // total number of pixels
std::vector<float> overlaps(projected_grid_size * pixel_grid_size);
std::vector<float> h(pixel_grid_m.size());
for(int d=0; d < pixel_grid_m.size(); d++) {
h[d] = (pixel_grid_omega[2*d+1] - pixel_grid_omega[2*d]) / pixel_grid_m[d];
}
for(int i=0; i < projected_grid_size; i++){
std::vector<float> point_indices(8);
point_indices[0] = i;
point_indices[1] = i + 1;
point_indices[2] = i + projected_grid_m[0];
point_indices[3] = i + projected_grid_m[0] + 1;
point_indices[4] = i + projected_grid_m[0] * projected_grid_m[1];
point_indices[5] = i + projected_grid_m[0] * projected_grid_m[1] + 1;
point_indices[6] = i + (projected_grid_m[1] + 1) * projected_grid_m[0];
point_indices[7] = i + (projected_grid_m[1] + 1) * projected_grid_m[0] + 1;
std::vector<float> vx_corners(8 * projected_grid_m.size());
for(int vn = 0; vn < 8; vn++) {
for(int d = 0; d < projected_grid_m.size(); d++) {
vx_corners[vn + d * 8] = projected_grid_vx[point_indices[vn] + d * projeted_grid_size];
}
}
polygon_2d proj_voxel;
for(int vn = 0; vn < 8; vn++) {
point_2d poly_pt(vx_corners[2 * vn], vx_corners[2 * vn + 1]);
boost::geometry::append(proj_voxel, poly_pt);
}
boost::geometry::correct(proj_voxel);
polygon_2d proj_voxel_hull;
boost::geometry::convex_hull(proj_voxel, proj_voxel_hull);
box_2d bb_proj_vox;
boost::geometry::envelope(proj_voxel_hull, bb_proj_vox);
point_2d min_pt = bb_proj_vox.min_corner();
point_2d max_pt = bb_proj_vox.max_corner();
// then get min and max indices of intersecting bins
std::vector<float> min_idx(projected_grid_m.size() - 1),
max_idx(projected_grid_m.size() - 1);
// compute min and max indices of incidence on the pixel grid
// this is easy assuming you have a regular grid of pixels
min_idx[0] = std::min( (float) std::max( std::floor((min_pt.get<0>() - pixel_grid_omega[0]) / h[0] - 0.5 ), 0.), pixel_grid_m[0]-1);
min_idx[1] = std::min( (float) std::max( std::floor((min_pt.get<1>() - pixel_grid_omega[2]) / h[1] - 0.5 ), 0.), pixel_grid_m[1]-1);
max_idx[0] = std::min( (float) std::max( std::floor((max_pt.get<0>() - pixel_grid_omega[0]) / h[0] + 0.5 ), 0.), pixel_grid__m[0]-1);
max_idx[1] = std::min( (float) std::max( std::floor((max_pt.get<1>() - pixel_grid_omega[2]) / h[1] + 0.5 ), 0.), pixel_grid_m[1]-1);
// iterate only over pixels which intersect the projected voxel
for(int iy = min_idx[1]; iy <= max_idx[1]; iy++) {
for(int ix = min_idx[0]; ix <= max_idx[0]; ix++) {
int idx = ix + iy * pixel_grid_size[0]; // `first' index of pixel corner point
polygon_2d pix_poly;
for(int pn = 0; pn < 4; pn++) {
point_2d pix_corner_pt(
pixel_grid_vx[idx + pn % 2 + (pn / 2) * pixel_grid_m[0]],
pixel_grid_vx[idx + pn % 2 + (pn / 2) * pixel_grid_m[0] + pixel_grid_size]
);
boost::geometry::append(pix_poly, pix_corner_pt);
}
boost::geometry::correct( pix_poly );
//make this into a convex hull since the order of the point may be any
polygon_2d pix_hull;
boost::geometry::convex_hull(pix_poly, pix_hull);
// on to perform intersection
std::vector<polygon_2d> vox_pix_ints;
polygon_2d vox_pix_int;
try {
boost::geometry::intersection(proj_voxel_hull, pix_hull, vox_pix_ints);
} catch ( std::exception e ) {
// skip since these may coincide at a point or line
continue;
}
// both are convex so only one intersection expected
vox_pix_int = vox_pix_ints[0];
overlaps[i + idx * projected_grid_size] = boost::geometry::area(vox_pix_int);
}
} // end intersection for
} //end projected_voxel for
return overlaps;
}
You could create the ratio of polygon to bounding box:
This could be done computationally once to arrive at an avgerage poly area to BB ratio R constant.
Or you could do it with geometry using a circle bounded by its BB Since your using only projected polyhedron:
R = 0.0;
count = 0;
for (each poly) {
count++;
R += polyArea / itsBoundingBoxArea;
}
R = R/count;
Then calculate the summation of intersection of bounding boxes.
Sbb = 0.0;
for (box1, box2 where box1.isIntersecting(box2)) {
Sbb += box1.intersect(box2);
}
Then:
Approximation = R * Sbb
All of this would not work if concave polys were allowed. Because a concave poly can occupy less than 1% of it's bounding box. You will still have to find the convex hull.
Alternatively, If you can find the polygons area quicker than its hull, you could use the actual computed average poly area. This would give you a decent approximation as well while avoiding both poly intersection and wrapping.
Hm, the problem seems similar to doing "collision-detection" i game-engines. Or "potentially visible sets".
While I don't know much about the current state-of-the-art, i remember an optimization was to enclose objects in spheres, since checking overlaps between spheres (or circles in 2D) is really cheap.
In order to speed-up checks for collisions, objects were often put into search-structures (e.g. a sphere-tree (circle-tree in 2D case)). Basically organizing the space into a hierarchical structure, to make queries for overlaps fast.
So basically my suggestion boils down to: Try looking at algorithms for collision-detection i game-engines.
Assumption
I'm assuming that you mean "intersections" and not intersection. Moreover, It is not the expected use case that most of the individual polys from M and N will overlap at the same time. If this assumption is true then:
Answer
The way this is done with 2D game engines is by having a scene graph where every object has a bounding box. Then place all the the polygons into a node in an quadtree according to their location determined by bounding box. Then the task becomes parallel because each node can be processed separately for intersection.
Here is the wiki for quadtree:
Quadtree Wiki
An octree could be used when in 3D.
It actually doesn't even have to be a octree. You could get the same results with any space partition. You could find the maximum separation of polys (lets call it S). And create say S/10 space partitions. Then you would have 10 separate spaces to execute in parallel. Not only would it be concurrent, but It would no longer be M * N time since not every poly must be compared against every other poly.

Nested loop summation in 2D using OpenCL

I have recently started working with OpenCL in C++ and I'm trying to fully understand how to use 2D and 3D NDRange. I'm currently implementing Inverse Distance Weighting in OpenCL, but my problem is general.
Below is the serial function to compute the weights and it consists of a nested loop.
void computeWeights(int nGrids, int nPoints, double *distances, double *weightSum, const double p) {
for (int i = 0; i < nGrids; ++i) {
double sum = 0;
for (int j = 0; j < nPoints; ++j) {
double weight = 1 / pow(distances[i * nPoints + j], p);
distances[i * nPoints + j] = weight;
sum += weight;
}
weightSum[i] = sum;
}
}
What I would want is to implement the above function using a 2D NDRange, the first being over nGrids and the second over nPoints. What I don't understand, though, is how to handle the summation of the weights into weightSum[i]. I understand that I may have to use parallel sum reduction, somehow.
When dispatching a kernel with a 2D global workspace, OpenCL creates a grid of work-items. Each work-item executes the kernel and gets unique ids in both those dimensions.
(x,y)|________________________
| (0,0) (0,1) (0,2) ...
| (1,0) (1,1) (1,2)
| (2,0) (2,1) (2,2)
| ...
The work-items are also divided into groups and get unique ids within those work-groups. E.g. for work-groups of size (2,2):
(x,y)|________________________
| (0,0) (0,1) (0,0) ...
| (1,0) (1,1) (1,0)
| (0,0) (0,1) (0,0)
| ...
You can arrange the work-groups, so that each one of them performs a reduction.
Your SDK probably has samples, and a parallel reduction will be one of them.
To get you started, here is a kernel that solves your problem. It's in its simplest form, and works for a single work-group per row.
// cl::NDRange global(nPoints, nGrids);
// cl::NDRange local(nPoints, 1);
// cl::Local data(nPoints * sizeof (double));
kernel
void computeWeights(global double *distances, global double *weightSum, local double *data, double p)
{
uint nPoints = get_global_size(0);
uint j = get_global_id(0);
uint i = get_global_id(1);
uint lX = get_local_id(0);
double weight = 1.0 / pow(distances[i * nPoints + j], p);
distances[i * nPoints + j] = weight;
data[lX] = weight;
for (uint d = get_local_size(0) >> 1; d > 0; d >>= 1)
{
barrier(CLK_LOCAL_MEM_FENCE);
if (lX < d)
data[lX] += data[lX + d];
}
if (lX == 0)
weightSum[i] = data[0];
}
Each row of work-items (i.e. each work-group) computes the weights (and their sum) for grid i. Each work-item computes a weight, stores it back to distances, and loads it onto local memory. Then each work-group performs a reduction in local memory, and finally the result gets stored in weightSum.

How do I draw a cylinder in OpenTK(.Glu.Cylinder)?

How do I draw a cylinder with OpenGL in OpenTK?
Sample code from an older project of mine. This creates an "uncapped" cylinder (top and bottom are empty).
int segments = 10; // Higher numbers improve quality
int radius = 3; // The radius (width) of the cylinder
int height = 10; // The height of the cylinder
var vertices = new List<Vector3>();
for (double y = 0; y < 2; y++)
{
for (double x = 0; x < segments; x++)
{
double theta = (x / (segments - 1)) * 2 * Math.PI;
vertices.Add(new Vector3()
{
X = (float)(radius * Math.Cos(theta)),
Y = (float)(height * y),
Z = (float)(radius * Math.Sin(theta)),
});
}
}
var indices = new List<int>();
for (int x = 0; x < segments - 1; x++)
{
indices.Add(x);
indices.Add(x + segments);
indices.Add(X + segments + 1);
indices.Add(x + segments + 1);
indices.Add(x + 1);
indices.Add(x);
}
You can now render the cylinder like this:
GL.Begin(BeginMode.Triangles);
foreach (int index in indices)
GL.Vertex3(vertices[index]);
GL.End();
You can also upload vertices and indices into a vertex buffer object to improve performance.
Generating the geometry for a cylinder is quite simple (let's consider a Z-aligned cylinder). Let me use pseudocode:
points = list of (x,y,z)
where x = sin(a)*RADIUS, y = cos(a)*RADIUS, z = b,
for each a in [0..2*PI) with step StepA,
for each b in [0..HEIGHT] with step StepB
About the indices: Let us assume N equal to the number of "levels" or "slices" of the cylinder (which depends on HEIGHT and StepB) and M equal to the number of points on every "slice" (which depends on StepA).
The cylinder contains some quads, each spanning 2 neighbouring slices, so the indices would look like:
indices = list of (a,b,c,d)
where a = M * slice + point,
b = M * slice + (point+1) % M,
c = (M+1) * slice + (point+1) % M,
d = (M+1) * slice + point
for each slice in [0..N-2]
for each point in [0..M-1]
If you need normals for the cylinder, they are simple to generate:
normals = (x/RADIUS,y/RADIUS,0)
for each (x,y,z) in points
That's it for the round part of the cylinder, you might also want the "caps" but I believe they are easy to do.
I'll leave the fun part of translating my pseudocode into your language of choice for you. :)
The rest is to create/bind the VBO, load up the geometry, set pointers, use your shader of choice and call glDrawArrays(...) - any OpenGL 3 tutorial should cover this; are you familiar with that part?