static void mdlOutputs(SimStruct *S, int_T tid)
{
USING_NAMESPACE_QPOASES
int i;
int nV;
returnValue status;
int nWSR = MAXITER;
int nU = NCONTROLINPUTS;
InputRealPtrsType in_g, in_lb, in_ub;
QProblemB* problem;
real_t *H, *g, *lb, *ub;
real_t *xOpt;
real_T *out_uOpt, *out_objVal, *out_status, *out_nWSR;
/* get pointers to block inputs ... */
const mxArray* in_H = ssGetSFcnParam(S, 0);
in_g = ssGetInputPortRealSignalPtrs(S, 0);
in_lb = ssGetInputPortRealSignalPtrs(S, 1);
in_ub = ssGetInputPortRealSignalPtrs(S, 2);
/* ... and to the QP data */
problem = (QProblemB *) ssGetPWork(S)[0];
H = (real_t *) ssGetPWork(S)[1];
g = (real_t *) ssGetPWork(S)[2];
lb = (real_t *) ssGetPWork(S)[3];
ub = (real_t *) ssGetPWork(S)[4];
/* setup QP data */
nV = ssGetInputPortWidth(S, 1); /* nV = size_g */
if ( H != 0 )
{
/* no conversion from FORTRAN to C as Hessian is symmetric! */
for ( i=0; i<nV*nV; ++i )
H[i] = (mxGetPr(in_H))[i];
}
for ( i=0; i<nV; ++i )
g[i] = (*in_g)[i];
if ( lb != 0 )
{
for ( i=0; i<nV; ++i )
lb[i] = (*in_lb)[i];
}
if ( ub != 0 )
{
for ( i=0; i<nV; ++i )
ub[i] = (*in_ub)[i];
}
xOpt = new real_t[nV];
if ( problem->getCount() == 0 )
{
/* initialise and solve first QP */
status = problem->init( H,g,lb,ub, nWSR,0 );
problem->getPrimalSolution( xOpt );
}
else
{
/* solve neighbouring QP using hotstart technique */
status = problem->hotstart( g,lb,ub, nWSR,0 );
if ( ( status != SUCCESSFUL_RETURN ) && ( status != RET_MAX_NWSR_REACHED ) )
{
/* if an error occurs, reset problem data structures ... */
problem->reset( );
/* ... and initialise/solve again with remaining number of iterations. */
int nWSR_retry = MAXITER - nWSR;
status = problem->init( H,g,lb,ub, nWSR_retry,0 );
nWSR += nWSR_retry;
}
/* obtain optimal solution */
problem->getPrimalSolution( xOpt );
}
/* generate block output: status information ... */
out_uOpt = ssGetOutputPortRealSignal(S, 0);
out_objVal = ssGetOutputPortRealSignal(S, 1);
out_status = ssGetOutputPortRealSignal(S, 2);
out_nWSR = ssGetOutputPortRealSignal(S, 3);
for ( i=0; i<nU; ++i )
out_uOpt[i] = (real_T)(xOpt[i]);
out_objVal[0] = (real_T)(problem->getObjVal());
out_status[0] = (real_t)(getSimpleStatus( status ));
out_nWSR[0] = (real_T)(nWSR);
removeNaNs( out_uOpt,nU );
removeInfs( out_uOpt,nU );
removeNaNs( out_objVal,1 );
removeInfs( out_objVal,1 );
delete[] xOpt;
}
int main(int argc, char** argv) {
if (argc != 3) {
std::cerr << "please provide 2 point clouds as arguments: <source>.pcd <target>.pcd)" << std::endl;
exit(0);
}
PointCloud::Ptr cloudSource(new PointCloud);
PointCloud::Ptr cloudOut(new PointCloud);
PointCloud::Ptr cloudTarget(new PointCloud);
PointCloud::Ptr cloudSourceFiltered(new PointCloud);
PointCloud::Ptr cloudTargetFiltered(new PointCloud);
PointCloudNormal::Ptr cloudSourceNormal(new PointCloudNormal);
PointCloudNormal::Ptr cloudTargetNormal(new PointCloudNormal);
PointCloudNormal::Ptr cloudHandlesSource(new PointCloudNormal);
PointCloudNormal::Ptr cloudHandlesTarget(new PointCloudNormal);
PointCloudNormal::Ptr cloudSourceNormalNoNaNs(new PointCloudNormal);
PointCloudNormal::Ptr cloudTargetNormalNoNaNs(new PointCloudNormal);
pcl::PCDWriter writer;
std::vector<int> indicesSource, indicesTarget;
//Fill in the cloud data
ROS_INFO("Reading files....");
pcl::PCDReader reader;
reader.read(argv[1], *cloudSource);
reader.read(argv[2], *cloudTarget);
//pcl::copyPointCloud (*cloudSourceNormal, *cloudSource);
//pcl::copyPointCloud (*cloudTargetNormal, *cloudTarget);
normalEstimation(cloudSource, cloudSourceNormal, 0.03);
normalEstimation(cloudTarget, cloudTargetNormal, 0.03);
std::vector<int> sourceHandleClusters;
std::vector<int> targetHandleClusters;
ROS_INFO("Extracting handles....");
extractHandles(cloudSource, cloudSourceFiltered, cloudSourceNormal, sourceHandleClusters);
extractHandles(cloudTarget, cloudTargetFiltered, cloudTargetNormal, targetHandleClusters);
std::vector<int> removedPoints;
removeNaNs(cloudSourceNormal, cloudSourceNormalNoNaNs, removedPoints);
adjustIndicesFromRemovedPoints(sourceHandleClusters, removedPoints);
removeNaNs(cloudTargetNormal, cloudTargetNormalNoNaNs, removedPoints);
adjustIndicesFromRemovedPoints(targetHandleClusters, removedPoints);
writer.write("handlesSource.pcd", *cloudSourceNormalNoNaNs, sourceHandleClusters, true);
writer.write("handlesTarget.pcd", *cloudTargetNormalNoNaNs, targetHandleClusters, true);
ROS_INFO("Calculating inital transformation from ICP SVD on handles....");
Eigen::Matrix4f guess;
guess << 1, 0, 0, 0.0,
0, 1, 0, 0.5,
0, 0, 1, 0.33,
0, 0, 0, 1;
pcl::IterativeClosestPoint<PointNormal, PointNormal> icp;
// set source and target clouds from indices of pointclouds
pcl::ExtractIndices<PointNormal> handlefilter;
pcl::PointIndices::Ptr sourceHandleIndices (new pcl::PointIndices);
sourceHandleIndices->indices = sourceHandleClusters;
handlefilter.setIndices(sourceHandleIndices);
handlefilter.setInputCloud(cloudSourceNormalNoNaNs);
handlefilter.filter(*cloudHandlesSource);
icp.setInputCloud(cloudHandlesSource);
pcl::PointIndices::Ptr targetHandleIndices (new pcl::PointIndices);
targetHandleIndices->indices = targetHandleClusters;
handlefilter.setIndices(targetHandleIndices);
handlefilter.setInputCloud(cloudTargetNormalNoNaNs);
handlefilter.filter(*cloudHandlesTarget);
icp.setInputTarget(cloudHandlesTarget);
PointCloudNormal Final;
icp.align(Final, guess);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
ROS_INFO("Initialize transformation estimation object....");
boost::shared_ptr< TransformationEstimationJointOptimize<PointNormal, PointNormal > >
transformationEstimation_(new TransformationEstimationJointOptimize<PointNormal, PointNormal>());
float denseCloudWeight = 1.0;
float visualFeatureWeight = 0.0;
float handleFeatureWeight = 0.25;
transformationEstimation_->setWeights(denseCloudWeight, visualFeatureWeight, handleFeatureWeight);
transformationEstimation_->setCorrespondecesDFP(indicesSource, indicesTarget);
// Eigen::Matrix4f guess;
// guess << 0.999203, 0.0337772, -0.0213298, 0.0110106,
// -0.0349403, 0.99778, -0.0567293, -0.00746282,
// 0.0193665, 0.0574294, 0.998162, 0.0141431,
// 0, 0, 0, 1;
// guess << 1, 0.2, 0.3, -0.3,
// -0.2, 1, -0.2, 0.9,
// -0.3, 0.2, 1, 0.4,
// 0, 0, 0, 1;
// guess << 1, 0, 0, 0.0,
// 0, 1, 0, 0.5,
// 0, 0, 1, 0.33,
// 0, 0, 0, 1;
// icp svd for handles node_1/node_91
// guess << 0.993523, 0.0152363, -0.112636, 0.138385,
// -0.0264756, 0.994739, -0.0989777, 0.615225,
// 0.110535, 0.101318, 0.988696, 0.347863,
// 0, 0, 0, 1;
// custom icp
ROS_INFO("Initialize icp object....");
pcl::IterativeClosestPointJointOptimize<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> icpJointOptimize; //JointOptimize
icpJointOptimize.setMaximumIterations (20);
icpJointOptimize.setTransformationEpsilon (0);
icpJointOptimize.setMaxCorrespondenceDistance(0.1);
icpJointOptimize.setRANSACOutlierRejectionThreshold(0.03);
icpJointOptimize.setEuclideanFitnessEpsilon (0);
icpJointOptimize.setTransformationEstimation (transformationEstimation_);
icpJointOptimize.setHandleSourceIndices(sourceHandleClusters);
icpJointOptimize.setHandleTargetIndices(targetHandleClusters);
icpJointOptimize.setInputCloud(cloudSourceNormalNoNaNs);
icpJointOptimize.setInputTarget(cloudTargetNormalNoNaNs);
ROS_INFO("Running ICP....");
PointCloudNormal::Ptr cloud_transformed( new PointCloudNormal);
icpJointOptimize.align ( *cloud_transformed, icp.getFinalTransformation()); //init_tr );
std::cout << "[SIIMCloudMatch::runICPMatch] Has converged? = " << icpJointOptimize.hasConverged() << std::endl <<
" fitness score (SSD): " << icpJointOptimize.getFitnessScore (1000) << std::endl
<< icpJointOptimize.getFinalTransformation () << "\n";
transformPointCloud (*cloudSource, *cloudOut, icpJointOptimize.getFinalTransformation());
ROS_INFO("Writing output....");
writer.write("converged.pcd", *cloudOut, true);
}