33 #include <visp3/ustk_core/usGeometryDisplayTools.h>
35 #include <visp3/core/vpColVector.h>
36 #include <visp3/core/vpDisplay.h>
37 #include <visp3/core/vpMatrix.h>
38 #include <visp3/core/vpRGBa.h>
43 template <
class ImageDataType>
44 void displayFrame(
const vpImage<ImageDataType> &I,
const vpHomogeneousMatrix &imageMframe,
double Xscale,
double Yscale)
46 double origin_x = Xscale * imageMframe[0][3];
47 double origin_y = Yscale * imageMframe[1][3];
54 for (
int j = 0; j < 3; j++) {
55 double z = imageMframe[2][j];
66 order[0] = zmax_index;
67 order[1] = 3 - zmin_index - zmax_index;
68 order[2] = zmin_index;
71 vpColor color[3] = {vpColor::red, vpColor::green, vpColor::blue};
72 for (
unsigned int i = 0; i < 3; i++) {
73 unsigned int j = (
unsigned int)order[i];
74 vpDisplay::displayArrow(I, (
int)origin_y, (
int)origin_x, (
int)(origin_y + 20 * imageMframe[1][j]), (
int)(origin_x + 20 * imageMframe[0][j]),
80 template VISP_EXPORT
void displayFrame<vpRGBa>(
const vpImage<vpRGBa> &,
const vpHomogeneousMatrix &,
double,
double);
84 template <
class ImageDataType>
86 double Xscale,
double Yscale,
const vpColor &color)
88 vpMatrix R(imageMworld.getRotationMatrix());
89 vpColVector T(imageMworld.getTranslationVector());
91 vpColVector imagePoint(R * plane.
getPosition() + T);
94 double x = Xscale * imagePoint[0];
95 double y = Yscale * imagePoint[1];
97 double dx = 0.1 * Xscale * imageDirection[0];
98 double dy = 0.1 * Yscale * imageDirection[1];
100 vpDisplay::displayCross(I, (
int)y, (
int)x, 7, color);
101 vpDisplay::displayLine(I, (
int)(y - dx), (
int)(x + dy), (
int)(y + dx), (
int)(x - dy), color);
104 const vpHomogeneousMatrix &,
double,
double,
const vpColor &);
106 double,
double,
const vpColor &);
110 template <
class ImageDataType>
112 const vpColor &color,
int nbRenderingLines)
116 std::vector<double> params(nbRenderingLines + 1);
118 for (
int i = 1; i < nbRenderingLines + 1; i++)
119 params.at(i) = params.at(i - 1) + step;
121 vpMatrix imagePoints = curve.
getPoints(params);
123 double x0 = Xscale * imagePoints[0][0];
124 double y0 = Yscale * imagePoints[1][0];
128 for (
int i = 0; i < nbRenderingLines; i++) {
132 x1 = Xscale * imagePoints[0][i + 1];
133 y1 = Yscale * imagePoints[1][i + 1];
135 vpDisplay::displayLine(I, (
int)y0, (
int)x0, (
int)y1, (
int)x1, color);
139 double,
const vpColor &,
int);
141 const vpColor &,
int);
145 template <
class ImageDataType>
147 double Xscale,
double Yscale,
const vpColor &color,
int nbRenderingLines,
double visibilityDistance)
149 unsigned int nbPoints = nbRenderingLines + 1;
152 std::vector<double> params(nbPoints);
154 for (
unsigned int i = 1; i < nbPoints; i++)
155 params.at(i) = params.at(i - 1) + step;
157 vpMatrix points = curve.
getPoints(params);
159 const vpMatrix R(imageMworld.getRotationMatrix());
160 const vpMatrix T(imageMworld.getTranslationVector());
161 const vpMatrix ones(1, nbPoints, 1);
162 vpMatrix imagePoints(R * points + vpMatrix::kron(T, ones));
164 double x0 = Xscale * imagePoints[0][0];
165 double y0 = Yscale * imagePoints[1][0];
166 double z0 = imagePoints[2][0];
171 for (
int i = 0; i < nbRenderingLines; i++) {
176 x1 = Xscale * imagePoints[0][i + 1];
177 y1 = Yscale * imagePoints[1][i + 1];
178 z1 = imagePoints[2][i + 1];
180 if ((vpMath::sign(z0) != vpMath::sign(z1)) || ((fabs(z0) < visibilityDistance) && (fabs(z1) < visibilityDistance)))
181 vpDisplay::displayLine(I, (
int)y0, (
int)x0, (
int)y1, (
int)x1, color);
186 const vpHomogeneousMatrix &,
double,
double,
const vpColor &,
int,
189 const vpHomogeneousMatrix &,
double,
double,
const vpColor &,
int,
double);
191 template <
class ImageDataType>
193 const vpHomogeneousMatrix &imageMworld,
double Xscale,
double Yscale,
194 const vpColor &color)
209 vpColVector Dxy(3, 0);
211 vpColVector Axy = vpColVector::cross(D, Dxy);
212 vpColVector Bxy = vpColVector::cross(D, Axy).normalize();
214 double thetaxy = atan2(Axy[1], Axy[0]);
216 double bxy = r * vpColVector::cross(Bxy, Dxy).frobeniusNorm();
217 if (Bxy.frobeniusNorm() < 0.5)
221 vpImagePoint centerXY(Yscale * C[1], Xscale * C[0]);
222 double A = sqrt(pow(Xscale * cos(thetaxy), 2) + pow(Yscale * sin(thetaxy), 2));
225 thetaxy = atan2(axy * sin(thetaxy), bxy * cos(thetaxy));
226 vpDisplay::displayEllipse(I, centerXY, axy, bxy, thetaxy,
false, color);
229 const vpImage<unsigned char> &,
230 const vpHomogeneousMatrix &,
double,
double,
233 const vpHomogeneousMatrix &,
double,
double,
238 template <
class ImageDataType>
240 double Xscale,
double Yscale,
const vpColor &color,
int nbRenderingLinesPerSegment,
241 double visibilityDistance)
245 nbRenderingLinesPerSegment, visibilityDistance);
248 const vpHomogeneousMatrix &,
double,
double,
const vpColor &,
int,
251 double,
double,
const vpColor &,
int,
double);
253 template <
class ImageDataType>
255 const vpHomogeneousMatrix &imageMworld,
double Xscale,
double Yscale,
const vpColor &color,
256 double visibilityDistance)
262 vpMatrix points(3, nbPoints);
268 const vpMatrix R(imageMworld.getRotationMatrix());
269 const vpMatrix T(imageMworld.getTranslationVector());
270 const vpMatrix ones(1, nbPoints, 1);
271 vpMatrix imagePoints(R * points + vpMatrix::kron(T, ones));
273 for (
unsigned int i = 0; i < nbPoints; i++) {
274 double x = Xscale * imagePoints[0][i];
275 double y = Yscale * imagePoints[1][i];
276 double z = imagePoints[2][i];
277 if (fabs(z) < visibilityDistance)
278 vpDisplay::displayCross(I, (
int)y, (
int)x, 7, color);
282 const vpHomogeneousMatrix &,
double,
double,
283 const vpColor &,
double);
285 const vpHomogeneousMatrix &,
double,
double,
const vpColor &,
288 template <
class ImageDataType>
289 void display(
const usBSpline3D &spline,
const vpImage<ImageDataType> &I,
const vpHomogeneousMatrix &imageMworld,
290 double Xscale,
double Yscale,
const vpColor &color,
int nbRenderingLines,
double visibilityDistance)
292 displayLine(spline, I, imageMworld, Xscale, Yscale, color, nbRenderingLines, visibilityDistance);
293 displayExtremities(spline, I, imageMworld, Xscale, Yscale, color, visibilityDistance);
296 const vpHomogeneousMatrix &,
double,
double,
const vpColor &,
int,
299 double,
double,
const vpColor &,
int,
double);
301 template <
class ImageDataType>
303 const vpHomogeneousMatrix &imageMworld,
double Xscale,
double Yscale,
304 const vpColor &color)
319 vpColVector Dxy(3, 0);
321 vpColVector Axy = vpColVector::cross(D, Dxy);
322 vpColVector Bxy = vpColVector::cross(D, Axy).normalize();
324 double thetaxy = atan2(Axy[1], Axy[0]);
326 double bxy = r * vpColVector::cross(Bxy, Dxy).frobeniusNorm();
327 if (Bxy.frobeniusNorm() < 0.5)
331 vpImagePoint centerXY(Yscale * C[1], Xscale * C[0]);
332 double A = sqrt(pow(Xscale * cos(thetaxy), 2) + pow(Yscale * sin(thetaxy), 2));
335 thetaxy = atan2(axy * sin(thetaxy), bxy * cos(thetaxy));
336 vpDisplay::displayEllipse(I, centerXY, axy, bxy, thetaxy,
false, color);
339 const vpHomogeneousMatrix &,
double,
double,
342 const vpHomogeneousMatrix &,
double,
double,
const usPolynomialCurve3D & accessSegment(int i) const
int getNbSegments() const
Parameters setters and getters.
const usPolynomialCurve3D & accessLastSegment() const
double getCurvatureFromShape(double start, double end, vpColVector ¢er3D, vpColVector &direction3D) const
Curvature.
vpColVector getDirection() const
vpColVector getPosition() const
double getEndParameter() const
double getStartParameter() const
vpMatrix getPoints(vpColVector parameters) const
double getStartParameter() const
vpMatrix getPoints(const vpColVector ¶meters) const
vpColVector getEndPoint() const
double getCurvatureFromShape(double start, double end, vpColVector ¢er3D, vpColVector &direction3D) const
double getEndParameter() const
vpColVector getStartPoint() const