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;
}