void BezierBoundarySolver::_IterateCurvatureReduction(BezierBoundaryProblem* pProblem,Eigen::Vector4d& params) { double epsilon = 0.0001; //create a jacobian for the parameters by perturbing them Eigen::Vector4d Jt; //transpose of the jacobian BezierBoundaryProblem origProblem = *pProblem; double maxK = _GetMaximumCurvature(pProblem); for(int ii = 0; ii < 4 ; ii++){ Eigen::Vector4d epsilonParams = params; epsilonParams[ii] += epsilon; _Get5thOrderBezier(pProblem,epsilonParams); _Sample5thOrderBezier(pProblem); double kPlus = _GetMaximumCurvature(pProblem); epsilonParams[ii] -= 2*epsilon; _Get5thOrderBezier(pProblem,epsilonParams); _Sample5thOrderBezier(pProblem); double kMinus = _GetMaximumCurvature(pProblem); Jt[ii] = (kPlus-kMinus)/(2*epsilon); } //now that we have Jt, we can calculate JtJ Eigen::Matrix4d JtJ = Jt*Jt.transpose(); //thikonov regularization JtJ += Eigen::Matrix4d::Identity(); Eigen::Vector4d deltaParams = JtJ.inverse() * Jt*maxK; params -= deltaParams; _Get5thOrderBezier(pProblem,params); _Sample5thOrderBezier(pProblem); //double finalMaxK = _GetMaximumCurvature(pProblem); //dout("2D Iteration took k from " << maxK << " to " << finalMaxK); }
void cloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud) { if( ( !cloud_updated ) && ( goal_completion_time + ros::Duration(2.0) < cloud->header.stamp ) ) { try { // update the pose listener.waitForTransform( "/arm_kinect_frame", "/map", cloud->header.stamp, ros::Duration(5.0)); listener.lookupTransform( "/map", "/arm_kinect_frame", cloud->header.stamp, kinect2map); listener.waitForTransform( "/arm_kinect_optical_frame", "/map", cloud->header.stamp, ros::Duration(5.0)); listener.lookupTransform( "/map", "/arm_kinect_optical_frame", cloud->header.stamp, optical2map); tf::Vector3 position_( kinect2map.getOrigin() ); position.x() = position_.x(); position.y() = position_.y(); position.z() = position_.z(); tf::Quaternion orientation_( kinect2map.getRotation() ); orientation.x() = orientation_.x(); orientation.y() = orientation_.y(); orientation.z() = orientation_.z(); orientation.w() = orientation_.w(); ROS_INFO_STREAM("position = " << position.transpose() ); ROS_INFO_STREAM("orientation = " << orientation.transpose() ); // update the cloud pcl::copyPointCloud(*cloud, *xyz_cld_ptr); // Do I need to copy it? //xyz_cld_ptr = cloud; cloud_updated = true; } catch (tf::TransformException ex) { ROS_ERROR("%s", ex.what()); } } }
const CPoint3DWORLD CLandmark::_getOptimizedLandmarkSTEREOUV( const UIDFrame& p_uFrame, const CPoint3DWORLD& p_vecInitialGuess ) { //ds initial values Eigen::Matrix4d matH( Eigen::Matrix4d::Zero( ) ); Eigen::Vector4d vecB( Eigen::Vector4d::Zero( ) ); CPoint3DHomogenized vecX( CMiniVisionToolbox::getHomogeneous( p_vecInitialGuess ) ); //Eigen::Matrix2d matOmega( Eigen::Matrix2d::Identity( ) ); double dErrorSquaredTotalPixelsPREVIOUS = 0.0; //ds iterations (break-out if convergence reached early) for( uint32_t uIteration = 0; uIteration < CLandmark::uCapIterations; ++uIteration ) { //ds counts double dErrorSquaredTotalPixels = 0.0; uint32_t uInliers = 0; //ds initialize setup matH.setZero( ); vecB.setZero( ); //ds do calibration over all recorded values for( const CMeasurementLandmark* pMeasurement: m_vecMeasurements ) { //ds apply the projection to the transformed point const Eigen::Vector3d vecABCLEFT = pMeasurement->matProjectionWORLDtoLEFT*vecX; const Eigen::Vector3d vecABCRIGHT = pMeasurement->matProjectionWORLDtoRIGHT*vecX; //ds buffer c value const double dCLEFT = vecABCLEFT.z( ); const double dCRIGHT = vecABCRIGHT.z( ); //ds compute error const Eigen::Vector2d vecUVLEFT( vecABCLEFT.x( )/dCLEFT, vecABCLEFT.y( )/dCLEFT ); const Eigen::Vector2d vecUVRIGHT( vecABCRIGHT.x( )/dCRIGHT, vecABCRIGHT.y( )/dCRIGHT ); const Eigen::Vector4d vecError( vecUVLEFT.x( )-pMeasurement->ptUVLEFT.x, vecUVLEFT.y( )-pMeasurement->ptUVLEFT.y, vecUVRIGHT.x( )-pMeasurement->ptUVRIGHT.x, vecUVRIGHT.y( )-pMeasurement->ptUVRIGHT.y ); //ds current error const double dErrorSquaredPixels = vecError.transpose( )*vecError; //std::printf( "[%06lu][%04u] error: %4.2f %4.2f %4.2f %4.2f (squared: %4.2f)\n", uID, uIteration, vecError(0), vecError(1), vecError(2), vecError(3) , dErrorSquaredPixels ); //ds check if outlier double dWeight = 1.0; if( dKernelMaximumErrorSquaredPixels < dErrorSquaredPixels ) { dWeight = dKernelMaximumErrorSquaredPixels/dErrorSquaredPixels; } else { ++uInliers; } dErrorSquaredTotalPixels += dWeight*dErrorSquaredPixels; //ds jacobian of the homogeneous division Eigen::Matrix< double, 2, 3 > matJacobianLEFT; matJacobianLEFT << 1/dCLEFT, 0, -vecABCLEFT.x( )/( dCLEFT*dCLEFT ), 0, 1/dCLEFT, -vecABCLEFT.y( )/( dCLEFT*dCLEFT ); Eigen::Matrix< double, 2, 3 > matJacobianRIGHT; matJacobianRIGHT << 1/dCRIGHT, 0, -vecABCRIGHT.x( )/( dCRIGHT*dCRIGHT ), 0, 1/dCRIGHT, -vecABCRIGHT.y( )/( dCRIGHT*dCRIGHT ); //ds final jacobian Eigen::Matrix< double, 4, 4 > matJacobian; matJacobian.setZero( ); matJacobian.block< 2,4 >(0,0) = matJacobianLEFT*pMeasurement->matProjectionWORLDtoLEFT; matJacobian.block< 2,4 >(2,0) = matJacobianRIGHT*pMeasurement->matProjectionWORLDtoRIGHT; //ds precompute transposed const Eigen::Matrix< double, 4, 4 > matJacobianTransposed( matJacobian.transpose( ) ); //ds accumulate matH += dWeight*matJacobianTransposed*matJacobian; vecB += dWeight*matJacobianTransposed*vecError; } //ds solve constrained system (since dx(3) = 0.0) and update x solution vecX.block< 3,1 >(0,0) += matH.block< 4, 3 >(0,0).householderQr( ).solve( -vecB ); //std::printf( "[%06lu][%04u]: %6.2f %6.2f %6.2f %6.2f (delta 2norm: %f inliers: %u)\n", uID, uIteration, vecX.x( ), vecX.y( ), vecX.z( ), vecX(3), vecDeltaX.squaredNorm( ), uInliers ); //std::fprintf( m_pFilePosition, "%04lu %06lu %03u %03lu %03u %6.2f\n", p_uFrame, uID, uIteration, m_vecMeasurements.size( ), uInliers, dRSSCurrent ); //ds check if we have converged if( CLandmark::dConvergenceDelta > std::fabs( dErrorSquaredTotalPixelsPREVIOUS-dErrorSquaredTotalPixels ) ) { //ds compute average error const double dErrorSquaredAveragePixels = dErrorSquaredTotalPixels/m_vecMeasurements.size( ); //ds if acceptable inlier/outlier ratio if( CLandmark::dMinimumRatioInliersToOutliers < static_cast< double >( uInliers )/m_vecMeasurements.size( ) ) { //ds success ++uOptimizationsSuccessful; //ds update average dCurrentAverageSquaredError = dErrorSquaredAveragePixels; //ds check if optimal if( dMaximumErrorSquaredAveragePixels > dErrorSquaredAveragePixels ) { bIsOptimal = true; } //std::printf( "<CLandmark>(_getOptimizedLandmarkSTEREOUV) [%06lu] converged (%2u) in %3u iterations to (%6.2f %6.2f %6.2f) from (%6.2f %6.2f %6.2f) ARSS: %6.2f (inliers: %u/%lu)\n", // uID, uOptimizationsSuccessful, uIteration, vecX(0), vecX(1), vecX(2), p_vecInitialGuess(0), p_vecInitialGuess(1), p_vecInitialGuess(2), dCurrentAverageSquaredError, uInliers, m_vecMeasurements.size( ) ); //ds update the estimate return vecX.block< 3,1 >(0,0); } else { ++uOptimizationsFailed; //std::printf( "<CLandmark>(_getOptimizedLandmarkSTEREOUV) landmark [%06lu] optimization failed - solution unacceptable (average error: %f, inliers: %u, iteration: %u)\n", uID, dErrorSquaredAveragePixels, uInliers, uIteration ); //ds if still here the calibration did not converge - keep the initial estimate return p_vecInitialGuess; } } else { //ds update error dErrorSquaredTotalPixelsPREVIOUS = dErrorSquaredTotalPixels; } } ++uOptimizationsFailed; //std::printf( "<CLandmark>(_getOptimizedLandmarkSTEREOUV) landmark [%06lu] optimization failed - system did not converge\n", uID ); //ds if still here the calibration did not converge - keep the initial estimate return p_vecInitialGuess; }