/*******************************+++*******************************/ unsigned MinAnyX(real (*ObjFunc)(real *x, size_t nDims), real AbsTol, real RelTol, unsigned MaxFuncs, const Matrix *XReg, size_t nDims, int MinAlg, real *x, real *Obj) /*****************************************************************/ /* Purpose: Optimize any objective function of x, where x may */ /* include fixed, discrete, or continuous */ /* variables. */ /* */ /* Args: ObjFunc The objective function. */ /* AbsTol Absolute tolerance on function value */ /* for convergence. */ /* RelTol Relative tolerance on function value */ /* for convergence. */ /* MaxFuncs Maximum function evaluations. */ /* XReg X-variable feasibility region. */ /* nDims Number of dimensions. */ /* MinAlg The minimizer called for unconstrained */ /* minimization. */ /* x Input: Starting point (including any */ /* fixed variables); */ /* Output: "Optimal" point. */ /* Obj Input: Objective at x; */ /* Output: "Optimal" objective. */ /* */ /* Returns: Total number of function evaluations. */ /* */ /* 96.01.18: CodeBug parameters changed and CodeBug replaced */ /* by CodeCheck. */ /* 96.01.18: CodeBug parameters changed. */ /* 96.03.07: Extrapolation at end of each iteration. */ /* 96.03.08: MinConverged replaced by ApproxEq. */ /* Extrapolation removed. */ /* 96.03.09: Extrapolation at end of each iteration. */ /* */ /* Version: 1996.03.09 */ /*****************************************************************/ { real ObjOld; real *ContMax, *ContMin, *xCont, *xOld; real *xExtCopy; real (*ObjFuncExtCopy)(real *x, size_t nDims); size_t i, ThisGroup; size_t *ContDistrib, **GroupIndex, *Group; size_t *GroupSize, *UngroupedIndex; size_t *IndexContCopy, nDimsExtCopy; size_t j, nContVars, nGroups, nUngroupedVars; unsigned nEvals, NumOpts; /* Save statics to local variables, */ /* to enable recursive calling. */ ObjFuncExtCopy = ObjFuncExt; xExtCopy = xExt; IndexContCopy = IndexCont; nDimsExtCopy = nDimsExt; CodeCheck(RegNumVars(XReg) == nDims); ContDistrib = AllocSize_t(nDims, NULL); IndexCont = AllocSize_t(nDims, NULL); ContMax = AllocReal(nDims, NULL); ContMin = AllocReal(nDims, NULL); xCont = AllocReal(nDims, NULL); xOld = AllocReal(nDims, NULL); UngroupedIndex = AllocSize_t(nDims, NULL); GroupIndex = AllocPtrSize_t(nDims, NULL); Group = AllocSize_t(nDims, NULL); GroupSize = AllocSize_t(nDims, NULL); /* Number of continuous variables. */ nContVars = 0; /* Number of GRID or ungrouped DISCRETE variables. */ nUngroupedVars = 0; /* Number of grouped DISCRETE variables. */ nGroups = 0; /* Count the continuous variables, etc. */ for (j = 0; j < nDims; j++) switch (RegSupport(XReg, j)) { case FIXED: break; case CONTINUOUS: IndexCont[nContVars] = j; ContMin[nContVars] = RegMin(XReg, j); ContMax[nContVars] = RegMax(XReg, j); ContDistrib[nContVars++] = RegDistrib(XReg, j); break; case GRID: UngroupedIndex[nUngroupedVars++] = j; break; case DISCRETE: ThisGroup = RegCandGroup(XReg, j); if (ThisGroup == 0) /* Ungrouped. */ UngroupedIndex[nUngroupedVars++] = j; else { for (i = 0; i < nGroups; i++) if (ThisGroup == Group[i]) break; if (i == nGroups) { /* New candidate group. */ Group[i] = ThisGroup; GroupSize[i] = 1; GroupIndex[i] = AllocSize_t(nDims, NULL); GroupIndex[i][0] = j; nGroups++; } else /* Existing candidate group. */ GroupIndex[i][GroupSize[i]++] = j; } break; default: CodeBug("Illegal support"); } /* External equivalents. */ ObjFuncExt = ObjFunc; nDimsExt = nDims; xExt = x; /* Iterate until converged. */ nEvals = 0; do { /* Used in test for convergence. */ ObjOld = *Obj; NumOpts = 0; VecCopy(x, nDims, xOld); if (nContVars > 0) { /* Load continuous x's into xCont. */ VecCopyIndex(nContVars, IndexCont, x, NULL, xCont); nEvals += MinCont(ObjCont, AbsTol, RelTol, MaxFuncs, ContMin, ContMax, ContDistrib, nContVars, MinAlg, xCont, Obj); NumOpts++; /* Put best continuous levels back in x. */ VecCopyIndex(nContVars, NULL, xCont, IndexCont, x); } /* Ungrouped variables. */ for (j = 0; j < nUngroupedVars; j++) { /* Optimize ungrouped-variable j. */ nEvals += MinDisc(1, &UngroupedIndex[j], XReg, x, Obj); NumOpts++; } /* Grouped variables. */ for (j = 0; j < nGroups; j++) { /* Optimize group j. */ nEvals += MinDisc(GroupSize[j], GroupIndex[j], XReg, x, Obj); NumOpts++; } /* Try extrapolating. */ nEvals += MinExtrap(ObjFunc, XReg, nDims, xOld, x, Obj); } while (NumOpts > 1 && !ApproxEq(ObjOld, *Obj, AbsTol, RelTol)); AllocFree(ContDistrib); AllocFree(IndexCont); AllocFree(ContMax); AllocFree(ContMin); AllocFree(xCont); AllocFree(xOld); AllocFree(UngroupedIndex); for (j = 0; j < nGroups; j++) AllocFree(GroupIndex[j]); AllocFree(GroupIndex); AllocFree(Group); AllocFree(GroupSize); /* Restore statics. */ ObjFuncExt = ObjFuncExtCopy; xExt = xExtCopy; IndexCont = IndexContCopy; nDimsExt = nDimsExtCopy; return nEvals; }
bool Depth::processCameraInfo( const sensor_msgs::CameraInfoConstPtr& first_camera_info, const sensor_msgs::CameraInfoConstPtr& second_camera_info, double* baseline, double* focal_length, bool* first_is_left, int* cx, int* cy) { if (first_camera_info->height != second_camera_info->height) { ROS_ERROR("Image heights do not match"); return false; } if (first_camera_info->width != second_camera_info->width) { ROS_ERROR("Image widths do not match"); return false; } for (double d : first_camera_info->D) { if (!ApproxEq(d, 0)) { ROS_ERROR("First image has non-zero distortion"); return false; } } for (double d : second_camera_info->D) { if (!ApproxEq(d, 0)) { ROS_ERROR("Second image has non-zero distortion"); return false; } } for (size_t i = 0; i < 12; ++i) { if ((i != 3) && !ApproxEq(first_camera_info->P[i], second_camera_info->P[i])) { ROS_ERROR("Image P matrices must match (excluding x offset)"); return false; } } if (!ApproxEq(first_camera_info->P[1], 0) || !ApproxEq(first_camera_info->P[4], 0)) { ROS_ERROR("Image P matrix contains skew"); return false; } if (!ApproxEq(first_camera_info->P[0], first_camera_info->P[5])) { ROS_ERROR("Image P matrix has different values for Fx and Fy"); return false; } if (first_camera_info->P[0] <= 0) { ROS_ERROR("Focal length must be greater than 0"); return false; } // downgraded to warning so that the color images of the KITTI dataset can be // processed if (!ApproxEq(first_camera_info->P[8], 0) || !ApproxEq(first_camera_info->P[9], 0) || !ApproxEq(first_camera_info->P[10], 1) || !ApproxEq(first_camera_info->P[11], 0)) { ROS_WARN_ONCE( "Image P matrix does not end in [0,0,1,0], these values will be " "ignored"); } // again downgraded to warning because KITTI has ugly matrices if (!ApproxEq(first_camera_info->P[7], 0)) { ROS_WARN_ONCE("P contains Y offset, this value will be ignored"); } *focal_length = first_camera_info->P[0]; *baseline = (second_camera_info->P[3] - first_camera_info->P[3]) / first_camera_info->P[0]; if (*baseline > 0) { *first_is_left = false; } else { *first_is_left = true; *baseline *= -1; } *cx = first_camera_info->P[2]; *cy = first_camera_info->P[6]; return true; }