26 #include "Geometry/ProjectionParametersPerspectiveDepth.hh"
27 #include "MathAlgo/PolynomialSolve.hh"
28 #include <Base/Image/ImageIO.hh>
31 #ifdef BIAS_HAVE_OPENMP
32 # if !defined(_OPENMP) && defined(WIN32)
33 # error Please check your OpenMP flags and defines - seem inconsistent.
44 const unsigned int height)
112 if(minDepth == maxDepth)
113 BIASERR(
"Min and Max Depth are the same! Check your parameters!");
118 if(params.size()!=6){
119 BIASWARNONCE(
"Polynom parameters have wrong size:"<<params.size()
120 <<
" Check your parameters!");
125 if(params.size()!=2){
126 BIASWARNONCE(
"Linear parameters have wrong size:"<<params.size()
127 <<
" Check your parameters!");
134 BIASWARNONCE(
"Spline parameters have wrong size:"<<params.size()
135 <<
" Check your parameters!");
148 for(
unsigned i=0;i<size;i++){
161 double(nrOfSplineParameters_-3));
174 const std::vector<double>& controls,
178 BIASASSERT(knots.size() == controls.size());
191 const std::vector<double>& minInt,
192 const std::vector<double>& maxInt,
193 const std::vector<double>& radiusValues,
194 const std::vector<double>& radiusIntensities){
196 std::vector<double> minIntEval;
197 std::vector<double> maxIntEval;
200 for(
unsigned i=0 ; i< depthValues.size(); i++){
201 if(minInt[i]<maxInt[i]){
203 minIntEval.push_back(minInt[i]);
204 maxIntEval.push_back(maxInt[i]);
210 if(ret < 0)
return ret;
212 if(ret < 0)
return ret;
226 std::vector<double> radiusEval;
228 for(
unsigned i=0 ; i< radiusValues.size(); i++){
229 if(radiusIntensities[i] != 0.0){
231 radiusEval.push_back(radiusIntensities[i]);
238 if(ret < 0)
return ret;
301 if(!bIsInCartesianCoords)
306 if(BIAS_ISNAN((
double)d) || BIAS_ISINF((
double)d)){
307 BIASWARN(
"Error:isnan or isinf failed for "<<d);
341 if(!bIsInCartesianCoords)
351 if (d == 0.0)
return;
353 std::vector<double> poly(4);
354 std::vector<double> sol;
358 if(!bIsInCartesianCoords)
371 solver.
Solve (poly, sol);
373 BIASASSERT(sol.size()>0);
375 for (
unsigned int i=1; i<sol.size(); i++) {
376 if (fabs(sol[i]-d)<fabs(dd-d))
400 if(!bIsInCartesianCoords)
410 unsigned int width = depthMap.
GetWidth();
411 unsigned int height = depthMap.
GetHeight();
418 if(!bIsInCartesianCoords){
423 tmpCartDepth = depthMap;
428 for (
unsigned int y=0;y<
height_;y++) {
429 for (
unsigned int x=0;x<
width_;x++) {
430 p[0] =
static_cast<float>(x);
431 p[1] =
static_cast<float>(y);
438 if(!bIsInCartesianCoords)
441 depthMap=tmpCartDepth;
450 unsigned int width = depthMap.
GetWidth();
451 unsigned int height = depthMap.
GetHeight();
457 if(!bIsInCartesianCoords){
462 tmpCartDepth = depthMap;
467 for (
unsigned int y=0;y<
height_;y++) {
468 for (
unsigned int x=0;x<
width_;x++) {
469 p[0] =
static_cast<float>(x);
470 p[1] =
static_cast<float>(y);
477 if(!bIsInCartesianCoords)
480 depthMap=tmpCartDepth;
488 float &reflectivity,
bool bIsInCartesianCoords){
491 float radius =sqrt(
float((pos[0]-px)*(pos[0]-px) +(pos[1]-py)*(pos[1]-py)));
492 if(!bIsInCartesianCoords)
496 double min ,max,radialOffsetOne,radialOffset;
502 min +=radialOffsetOne - radialOffset;
503 max +=radialOffsetOne - radialOffset;
504 double i_norm = reflectivity-min/(max-min);
507 if(!bIsInCartesianCoords)
514 float &reflectivity,
bool bIsInCartesianCoords){
517 float radius =sqrt(
float((pos[0]-px)*(pos[0]-px) +(pos[1]-py)*(pos[1]-py)));
518 if(!bIsInCartesianCoords)
521 double min ,max,radialOffsetOne,radialOffset;
527 min +=radialOffsetOne - radialOffset;
528 max +=radialOffsetOne - radialOffset;
529 double i_norm = reflectivity-min/(max-min);
532 if(!bIsInCartesianCoords)
540 bool bIsInCartesianCoords){
543 if(!bIsInCartesianCoords)
546 tmpCartDepth = depthMap;
548 int width = (int)tmpCartDepth.
GetWidth();
549 int height = (int)tmpCartDepth.
GetHeight();
556 for (
int y=0;y<height;y++) {
557 for (
int x=0;x<width;x++) {
562 if(!bIsInCartesianCoords)
565 depthMap=tmpCartDepth;
573 bool bIsInCartesianCoords){
576 if(!bIsInCartesianCoords)
579 tmpCartDepth = depthMap;
580 int width = (int)tmpCartDepth.
GetWidth();
581 int height = (int)tmpCartDepth.
GetHeight();
588 for (
int y=0;y<height;y++) {
589 for (
int x=0;x<width;x++) {
594 if(!bIsInCartesianCoords)
597 depthMap=tmpCartDepth;
605 bool hasNoDepthDistortion =
true;
609 return !hasNoDepthDistortion;
615 bool hasNoReflectivityDistortion =
true;
617 hasNoReflectivityDistortion=(hasNoReflectivityDistortion &&
621 hasNoReflectivityDistortion =
true;
622 return !hasNoReflectivityDistortion;
674 return string(
"None");
676 return string(
"Linear");
678 return string(
"Polynom");
680 return string(
"Spline");
682 return string(
"Undefined");
684 return string(
"Unknown");
688 #ifdef BIAS_HAVE_XML2
692 TopLevelTag =
"ProjectionParametersPerspectiveDepth";
700 xmlNodePtr childNode;
701 childNode = XMLObject.
addChildNode(Node,
"DepthCalibration");
704 XMLObject.
addAttribute(childNode,
"Model",
"DEPTH_MODEL_POLYNOM");
707 XMLObject.
addAttribute(childNode,
"Model",
"DEPTH_MODEL_LINEAR");
710 XMLObject.
addAttribute(childNode,
"Model",
"DEPTH_MODEL_SPLINE");
714 XMLObject.
addAttribute(childNode,
"Model",
"DEPTH_MODEL_NONE");
717 XMLObject.
addAttribute(childNode,
"Model",
"DEPTH_MODEL_UNDEFINED");
723 std::ostringstream o;
730 std::ostringstream o;
739 childNode = XMLObject.
addChildNode(Node,
"ReflectivityCalibration");
741 std::ostringstream o;
746 std::ostringstream o;
751 std::ostringstream o;
757 std::ostringstream o;
767 xmlNodePtr childNode;
769 if ((childNode = XMLObject.
getChild(Node,
"DepthCalibration"))!=NULL) {
770 xmlAttrPtr modelAttr = NULL;
776 if(model ==
"DEPTH_MODEL_POLYNOM"){
780 else if(model ==
"DEPTH_MODEL_LINEAR"){
784 else if(model ==
"DEPTH_MODEL_SPLINE"){
794 else if(model ==
"DEPTH_MODEL_NONE"){
797 else if(model ==
"DEPTH_MODEL_UNDEFINED"){
805 std::ostringstream o;
811 std::ostringstream o;
820 if(min!=max && min >=0.0 && max > 0.0)
823 if ((childNode = XMLObject.
getChild(Node,
"ReflectivityCalibration"))!=NULL) {
826 std::ostringstream o;
831 for (
unsigned int i=0; i<4; i++) {
832 std::ostringstream o,o2,o3;
846 #endif // BIAS_HAVE_XML2
static unsigned nrOfSplineParameters_
void Init()
Init function called by constructors.
void addAttribute(const xmlNodePtr Node, const std::string &AttributeName, bool AttributeValue)
Add an attribute to a node.
std::vector< double > radiusIntNormValues_
void InitSpline()
call this for restart at t= first knot point initiates recalculation of all polynom coefficients at f...
std::vector< double > depthIntNormValues_
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
static std::string GetDepthDistModelString(BIAS_TOF_DEPTH_ERROR_MODEL model)
BIAS_TOF_DEPTH_ERROR_MODEL
BIAS::Interpolator splineInterpolatorUndist_
camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
void UnDistortReflectivity(HomgPoint2D const &pos, float &depth, float &Reflectivity, bool bIsInCartesianCoords=false)
UnDistort Reflectivity value i at image position pos.
virtual int GetPrincipal(double &PrincipalX, double &PrincipalY) const
Get principal point (in pixels relative to top left corner).
xmlNodePtr getChild(const xmlNodePtr ParentNode, const std::string &ChildName)
Get a child of a Parent node by specifying the childs name, NULL is returned if the ParentNode has no...
void SetDepthCalibrationModel(BIAS_TOF_DEPTH_ERROR_MODEL model)
Set the depth calibration model, one of BIAS_TOF_DEPTH_ERROR_MODEL inits the vectors and reserves spa...
int FitPolynomial(const unsigned int degree, const std::vector< double > &x, const std::vector< double > &y, std::vector< double > &coefficients)
given locations x and measurements y=f(x) approximate f by a polynomial of arbitrary degree and retur...
std::vector< double > maxNormIntCoeffs_
void SetReflectivityDepthDistortionToZero()
Sets depth reflectivity distortion to 0.
static unsigned nrOfReflectivityParameters_
void DistortDepth(HomgPoint2D const &pos, float &d, bool bIsInCartesianCoords=false)
Distort depth value d at image position pos.
virtual int XMLOut(const xmlNodePtr Node, XMLIO &XMLObject) const
specialization of XML write function
virtual int XMLIn(const xmlNodePtr Node, XMLIO &XMLObject)
specialization of XML read function
void SetKnotPoints(const std::vector< double > &kPnt)
set the additional knot points, if you want nonuniform interpolation.
std::vector< double > DepthCalibrationParameters_
int DistortReflectivityDepthIP(BIAS::Image< float > &depthMap, BIAS::Image< float > &ReflectivityImage, bool bIsInCartesianCoords=false)
Distort Reflectivity error in depth map.
void Clear()
resets everything.
void UnDistortDepth(HomgPoint2D const &pos, float &d, bool bIsInCartesianCoords=false)
UnDistort depth value d at image position pos.
BIAS_TOF_DEPTH_ERROR_MODEL DepthCalibrationModel_
unsigned int GetWidth() const
void SetDepthCalibrationParametersSpline(const std::vector< double > &knots, const std::vector< double > &controls, float minDepth=0.0, float maxDepth=15000.0)
Set the depth calibration parameters for B-Spline model.
base class for solving polynomial equations
POLYNOMIALSOLVE_TYPE EvaluatePolynomial(const POLYNOMIALSOLVE_TYPE x, const std::vector< POLYNOMIALSOLVE_TYPE > &coeff) const
numerically robust way to evaluate a polynomial at position x, uses Horner scheme (same principle as ...
virtual int XMLOut(const xmlNodePtr Node, XMLIO &XMLObject) const
specialization of XML write function
int UnDistortReflectivityDepthIP(BIAS::Image< float > &depthMap, BIAS::Image< float > &ReflectivityImage, bool bIsInCartesianCoords=false)
Undistort Reflectivity error in depth map.
std::string getAttributeValueString(const xmlAttrPtr Attribute) const
std::vector< double > ReflectivityCalibrationParameters_
int getAttributeValueInt(const xmlAttrPtr Attribute) const
static unsigned GetNrOfDepthErrormodelParameters(BIAS_TOF_DEPTH_ERROR_MODEL model)
returns the number of depth error model parameters
Wrapper class for reading and writing XML files based on the XML library libxml2. ...
long int D_DEPTH_DISTORTION
void SetDepthDistortionToZero()
Sets depth distortion to 0.
virtual int XMLGetClassName(std::string &TopLevelTag, double &Version) const
specialization of XML block name function
xmlNodePtr addChildNode(const xmlNodePtr ParentNode, const std::string &NewNodeName)
Add a child node to an incoming node with the given name.
int SetReflectivityNormalizationParameters(const std::vector< double > &depthValues, const std::vector< double > &minInt, const std::vector< double > &maxInt, const std::vector< double > &radiusValues, const std::vector< double > &radiusIntensities)
sets the reflectivity calibration parameters and Fit polynomials to radial and reflectivity normaliza...
unsigned int GetChannelCount() const
returns the number of Color channels, e.g.
int Spline(double &res, double t, unsigned int k=3)
these functions do the Spline interpolation which reaches each control point.
unsigned int height_
height of image in pixels
int TransformCartesianToPolarCoordinates(const BIAS::Image< float > &cartesianDepth, BIAS::Image< float > &polarDepth)
transforms an image from cartesian coordinates to polar coordinates
unsigned int GetHeight() const
int DistortDepthMapIP(BIAS::Image< float > &depthMap, bool bIsInCartesianCoords=false)
Distorts the passed depth map in place.
std::vector< double > listSplineKnotPoints_
BIAS::Interpolator splineInterpolator_
long int NewDebugLevel(const std::string &name)
creates a new debuglevel
void SetControlPoints(const std::vector< double > &cPnt1)
set the control points, which control the interpolating curve
std::vector< double > minNormIntCoeffs_
void SetDepthCalibrationParameters(const std::vector< double > ¶ms, float minDepth=0.0, float maxDepth=15000.0)
Set the depth calibration parameters.
xmlAttrPtr getAttributeByName(const xmlNodePtr Node, const std::string &attribute_name)
search for a specific attribute
unsigned int width_
width of image in pixels
double getAttributeValueDouble(const xmlAttrPtr Attribute) const
void SetReflectivityCalibrationParameters(const std::vector< double > ¶ms)
Set the reflectivity calibration parameters.
int TransformPolarToCartesianCoordinates(const BIAS::Image< float > &polarDepth, BIAS::Image< float > &cartesianDepth)
transforms an image from polar coordinates to cartesian coordinates
int Solve(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff, std::vector< POLYNOMIALSOLVE_TYPE > &sol)
solve polynomial of arbitrary order n coeff[n]x^n+...+coeff[2]x^2+coeff[1]x+coeff[0]=0 coeff[n]!=0...
void DistortReflectivity(HomgPoint2D const &pos, float &depth, float &Reflectivity, bool bIsInCartesianCoords=false)
Distort Reflectivity value at depth at image offset form center radius.
int UnDistortDepthMapIP(BIAS::Image< float > &depthMap, bool bIsInCartesianCoords=false)
Undistorts the passed depth map in place.
ProjectionParametersPerspectiveDepth(const unsigned int width=0, const unsigned int height=0)
constructor with image width and height
static unsigned GetNrOfReflectivityParameters()
returns the number of depth reflectivity error model parameters
bool HasDepthDistortion() const
checks whether depth distortion is present
virtual int XMLIn(const xmlNodePtr Node, XMLIO &XMLObject)
specialization of XML read function
std::vector< double > radiusIntNormCoeffs_
bool HasReflectivityDistortion() const
checks whether depth reflectivity distortion is present
const StorageType ** GetImageDataArray() const
overloaded GetImageDataArray() from ImageBase