template <typename PointSource, typename PointTarget> void
pcl16::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src,
const std::vector<int> &indices_src,
const PointCloudTarget &cloud_tgt,
const std::vector<int> &indices_tgt,
Eigen::Matrix4f &transformation_matrix)
{
if (indices_src.size () < 4) // need at least 4 samples
{
PCL16_THROW_EXCEPTION (NotEnoughPointsException,
"[pcl16::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () << " points!");
return;
}
// Set the initial solution
Vector6d x = Vector6d::Zero ();
x[0] = transformation_matrix (0,3);
x[1] = transformation_matrix (1,3);
x[2] = transformation_matrix (2,3);
x[3] = atan2 (transformation_matrix (2,1), transformation_matrix (2,2));
x[4] = asin (-transformation_matrix (2,0));
x[5] = atan2 (transformation_matrix (1,0), transformation_matrix (0,0));
// Set temporary pointers
tmp_src_ = &cloud_src;
tmp_tgt_ = &cloud_tgt;
tmp_idx_src_ = &indices_src;
tmp_idx_tgt_ = &indices_tgt;
// Optimize using forward-difference approximation LM
const double gradient_tol = 1e-2;
OptimizationFunctorWithIndices functor(this);
BFGS<OptimizationFunctorWithIndices> bfgs (functor);
bfgs.parameters.sigma = 0.01;
bfgs.parameters.rho = 0.01;
bfgs.parameters.tau1 = 9;
bfgs.parameters.tau2 = 0.05;
bfgs.parameters.tau3 = 0.5;
bfgs.parameters.order = 3;
int inner_iterations_ = 0;
int result = bfgs.minimizeInit (x);
do
{
inner_iterations_++;
result = bfgs.minimizeOneStep (x);
if(result)
{
break;
}
result = bfgs.testGradient(gradient_tol);
} while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_)
{
PCL16_DEBUG ("[pcl16::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
PCL16_DEBUG ("BFGS solver finished with exit code %i \n", result);
transformation_matrix.setIdentity();
applyState(transformation_matrix, x);
}
else
PCL16_THROW_EXCEPTION(SolverDidntConvergeException,
"[pcl16::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
}
void MainController::run()
{
while(!pangolin::ShouldQuit() && !((!logReader->hasMore()) && quiet) && !(eFusion->getTick() == end && quiet))
{
if(!gui->pause->Get() || pangolin::Pushed(*gui->step))
{
if((logReader->hasMore() || rewind) && eFusion->getTick() < end)
{
TICK("LogRead");
if(rewind)
{
if(!logReader->hasMore())
{
logReader->getBack();
}
else
{
logReader->getNext();
}
if(logReader->rewound())
{
logReader->currentFrame = 0;
}
}
else
{
logReader->getNext();
}
TOCK("LogRead");
if(eFusion->getTick() < start)
{
eFusion->setTick(start);
logReader->fastForward(start);
}
float weightMultiplier = framesToSkip + 1;
if(framesToSkip > 0)
{
eFusion->setTick(eFusion->getTick() + framesToSkip);
logReader->fastForward(logReader->currentFrame + framesToSkip);
framesToSkip = 0;
}
Eigen::Matrix4f * currentPose = 0;
if(groundTruthOdometry)
{
currentPose = new Eigen::Matrix4f;
currentPose->setIdentity();
*currentPose = groundTruthOdometry->getIncrementalTransformation(logReader->timestamp);
}
eFusion->processFrame(logReader->rgb, logReader->depth, logReader->timestamp, currentPose, weightMultiplier);
if(currentPose)
{
delete currentPose;
}
if(frameskip && Stopwatch::getInstance().getTimings().at("Run") > 1000.f / 30.f)
{
framesToSkip = int(Stopwatch::getInstance().getTimings().at("Run") / (1000.f / 30.f));
}
}
}
else
{
eFusion->predict();
}
TICK("GUI");
if(gui->followPose->Get())
{
pangolin::OpenGlMatrix mv;
Eigen::Matrix4f currPose = eFusion->getCurrPose();
Eigen::Matrix3f currRot = currPose.topLeftCorner(3, 3);
Eigen::Quaternionf currQuat(currRot);
Eigen::Vector3f forwardVector(0, 0, 1);
Eigen::Vector3f upVector(0, iclnuim ? 1 : -1, 0);
Eigen::Vector3f forward = (currQuat * forwardVector).normalized();
Eigen::Vector3f up = (currQuat * upVector).normalized();
Eigen::Vector3f eye(currPose(0, 3), currPose(1, 3), currPose(2, 3));
eye -= forward;
Eigen::Vector3f at = eye + forward;
Eigen::Vector3f z = (eye - at).normalized(); // Forward
Eigen::Vector3f x = up.cross(z).normalized(); // Right
Eigen::Vector3f y = z.cross(x);
Eigen::Matrix4d m;
m << x(0), x(1), x(2), -(x.dot(eye)),
y(0), y(1), y(2), -(y.dot(eye)),
z(0), z(1), z(2), -(z.dot(eye)),
0, 0, 0, 1;
memcpy(&mv.m[0], m.data(), sizeof(Eigen::Matrix4d));
gui->s_cam.SetModelViewMatrix(mv);
}
gui->preCall();
std::stringstream stri;
stri << eFusion->getModelToModel().lastICPCount;
gui->trackInliers->Ref().Set(stri.str());
std::stringstream stre;
stre << (isnan(eFusion->getModelToModel().lastICPError) ? 0 : eFusion->getModelToModel().lastICPError);
gui->trackRes->Ref().Set(stre.str());
if(!gui->pause->Get())
{
gui->resLog.Log((isnan(eFusion->getModelToModel().lastICPError) ? std::numeric_limits<float>::max() : eFusion->getModelToModel().lastICPError), icpErrThresh);
gui->inLog.Log(eFusion->getModelToModel().lastICPCount, icpCountThresh);
}
Eigen::Matrix4f pose = eFusion->getCurrPose();
if(gui->drawRawCloud->Get() || gui->drawFilteredCloud->Get())
{
eFusion->computeFeedbackBuffers();
}
if(gui->drawRawCloud->Get())
{
eFusion->getFeedbackBuffers().at(FeedbackBuffer::RAW)->render(gui->s_cam.GetProjectionModelViewMatrix(), pose, gui->drawNormals->Get(), gui->drawColors->Get());
}
if(gui->drawFilteredCloud->Get())
{
eFusion->getFeedbackBuffers().at(FeedbackBuffer::FILTERED)->render(gui->s_cam.GetProjectionModelViewMatrix(), pose, gui->drawNormals->Get(), gui->drawColors->Get());
}
if(gui->drawGlobalModel->Get())
{
glFinish();
TICK("Global");
if(gui->drawFxaa->Get())
{
gui->drawFXAA(gui->s_cam.GetProjectionModelViewMatrix(),
gui->s_cam.GetModelViewMatrix(),
eFusion->getGlobalModel().model(),
eFusion->getConfidenceThreshold(),
eFusion->getTick(),
eFusion->getTimeDelta(),
iclnuim);
}
else
{
eFusion->getGlobalModel().renderPointCloud(gui->s_cam.GetProjectionModelViewMatrix(),
eFusion->getConfidenceThreshold(),
gui->drawUnstable->Get(),
gui->drawNormals->Get(),
gui->drawColors->Get(),
gui->drawPoints->Get(),
gui->drawWindow->Get(),
gui->drawTimes->Get(),
eFusion->getTick(),
eFusion->getTimeDelta());
}
glFinish();
TOCK("Global");
}
if(eFusion->getLost())
{
glColor3f(1, 1, 0);
}
else
{
glColor3f(1, 0, 1);
}
gui->drawFrustum(pose);
glColor3f(1, 1, 1);
if(gui->drawFerns->Get())
{
glColor3f(0, 0, 0);
for(size_t i = 0; i < eFusion->getFerns().frames.size(); i++)
{
if((int)i == eFusion->getFerns().lastClosest)
continue;
gui->drawFrustum(eFusion->getFerns().frames.at(i)->pose);
}
glColor3f(1, 1, 1);
}
if(gui->drawDefGraph->Get())
{
const std::vector<GraphNode*> & graph = eFusion->getLocalDeformation().getGraph();
for(size_t i = 0; i < graph.size(); i++)
{
pangolin::glDrawCross(graph.at(i)->position(0),
graph.at(i)->position(1),
graph.at(i)->position(2),
0.1);
for(size_t j = 0; j < graph.at(i)->neighbours.size(); j++)
{
pangolin::glDrawLine(graph.at(i)->position(0),
graph.at(i)->position(1),
graph.at(i)->position(2),
graph.at(graph.at(i)->neighbours.at(j))->position(0),
graph.at(graph.at(i)->neighbours.at(j))->position(1),
graph.at(graph.at(i)->neighbours.at(j))->position(2));
}
}
}
if(eFusion->getFerns().lastClosest != -1)
{
glColor3f(1, 0, 0);
gui->drawFrustum(eFusion->getFerns().frames.at(eFusion->getFerns().lastClosest)->pose);
glColor3f(1, 1, 1);
}
const std::vector<PoseMatch> & poseMatches = eFusion->getPoseMatches();
int maxDiff = 0;
for(size_t i = 0; i < poseMatches.size(); i++)
{
if(poseMatches.at(i).secondId - poseMatches.at(i).firstId > maxDiff)
{
maxDiff = poseMatches.at(i).secondId - poseMatches.at(i).firstId;
}
}
for(size_t i = 0; i < poseMatches.size(); i++)
{
if(gui->drawDeforms->Get())
{
if(poseMatches.at(i).fern)
{
glColor3f(1, 0, 0);
}
else
{
glColor3f(0, 1, 0);
}
for(size_t j = 0; j < poseMatches.at(i).constraints.size(); j++)
{
pangolin::glDrawLine(poseMatches.at(i).constraints.at(j).sourcePoint(0), poseMatches.at(i).constraints.at(j).sourcePoint(1), poseMatches.at(i).constraints.at(j).sourcePoint(2),
poseMatches.at(i).constraints.at(j).targetPoint(0), poseMatches.at(i).constraints.at(j).targetPoint(1), poseMatches.at(i).constraints.at(j).targetPoint(2));
}
}
}
glColor3f(1, 1, 1);
eFusion->normaliseDepth(0.3f, gui->depthCutoff->Get());
for(std::map<std::string, GPUTexture*>::const_iterator it = eFusion->getTextures().begin(); it != eFusion->getTextures().end(); ++it)
{
if(it->second->draw)
{
gui->displayImg(it->first, it->second);
}
}
eFusion->getIndexMap().renderDepth(gui->depthCutoff->Get());
gui->displayImg("ModelImg", eFusion->getIndexMap().imageTex());
gui->displayImg("Model", eFusion->getIndexMap().drawTex());
std::stringstream strs;
strs << eFusion->getGlobalModel().lastCount();
gui->totalPoints->operator=(strs.str());
std::stringstream strs2;
strs2 << eFusion->getLocalDeformation().getGraph().size();
gui->totalNodes->operator=(strs2.str());
std::stringstream strs3;
strs3 << eFusion->getFerns().frames.size();
gui->totalFerns->operator=(strs3.str());
std::stringstream strs4;
strs4 << eFusion->getDeforms();
gui->totalDefs->operator=(strs4.str());
std::stringstream strs5;
strs5 << eFusion->getTick() << "/" << logReader->getNumFrames();
gui->logProgress->operator=(strs5.str());
std::stringstream strs6;
strs6 << eFusion->getFernDeforms();
gui->totalFernDefs->operator=(strs6.str());
gui->postCall();
logReader->flipColors = gui->flipColors->Get();
eFusion->setRgbOnly(gui->rgbOnly->Get());
eFusion->setPyramid(gui->pyramid->Get());
eFusion->setFastOdom(gui->fastOdom->Get());
eFusion->setConfidenceThreshold(gui->confidenceThreshold->Get());
eFusion->setDepthCutoff(gui->depthCutoff->Get());
eFusion->setIcpWeight(gui->icpWeight->Get());
eFusion->setSo3(gui->so3->Get());
eFusion->setFrameToFrameRGB(gui->frameToFrameRGB->Get());
resetButton = pangolin::Pushed(*gui->reset);
if(gui->autoSettings)
{
static bool last = gui->autoSettings->Get();
if(gui->autoSettings->Get() != last)
{
last = gui->autoSettings->Get();
static_cast<LiveLogReader *>(logReader)->setAuto(last);
}
}
Stopwatch::getInstance().sendAll();
if(resetButton)
{
break;
}
if(pangolin::Pushed(*gui->save))
{
eFusion->savePly();
}
TOCK("GUI");
}
}
/* ---[ */
int
main (int argc, char** argv)
{
print_info ("Transform a cloud. For more information, use: %s -h\n", argv[0]);
bool help = false;
parse_argument (argc, argv, "-h", help);
if (argc < 3 || help)
{
printHelp (argc, argv);
return (-1);
}
// Parse the command line arguments for .pcd files
std::vector<int> p_file_indices;
p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
if (p_file_indices.size () != 2)
{
print_error ("Need one input PCD file and one output PCD file to continue.\n");
return (-1);
}
// Initialize the transformation matrix
Eigen::Matrix4f tform;
tform.setIdentity ();
// Command line parsing
float dx, dy, dz;
std::vector<float> values;
if (parse_3x_arguments (argc, argv, "-trans", dx, dy, dz) > -1)
{
tform (0, 3) = dx;
tform (1, 3) = dy;
tform (2, 3) = dz;
}
if (parse_x_arguments (argc, argv, "-quat", values) > -1)
{
if (values.size () == 4)
{
const float& x = values[0];
const float& y = values[1];
const float& z = values[2];
const float& w = values[3];
tform.topLeftCorner (3, 3) = Eigen::Matrix3f (Eigen::Quaternionf (w, x, y, z));
}
else
{
print_error ("Wrong number of values given (%zu): ", values.size ());
print_error ("The quaternion specified with -quat must contain 4 elements (w,x,y,z).\n");
}
}
if (parse_x_arguments (argc, argv, "-axisangle", values) > -1)
{
if (values.size () == 4)
{
const float& ax = values[0];
const float& ay = values[1];
const float& az = values[2];
const float& theta = values[3];
tform.topLeftCorner (3, 3) = Eigen::Matrix3f (Eigen::AngleAxisf (theta, Eigen::Vector3f (ax, ay, az)));
}
else
{
print_error ("Wrong number of values given (%zu): ", values.size ());
print_error ("The rotation specified with -axisangle must contain 4 elements (ax,ay,az,theta).\n");
}
}
if (parse_x_arguments (argc, argv, "-matrix", values) > -1)
{
if (values.size () == 9 || values.size () == 16)
{
int n = values.size () == 9 ? 3 : 4;
for (int r = 0; r < n; ++r)
for (int c = 0; c < n; ++c)
tform (r, c) = values[n*r+c];
}
else
{
print_error ("Wrong number of values given (%zu): ", values.size ());
print_error ("The transformation specified with -matrix must be 3x3 (9) or 4x4 (16).\n");
}
}
// Load the first file
sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2);
if (!loadCloud (argv[p_file_indices[0]], *cloud))
return (-1);
// Apply the transform
sensor_msgs::PointCloud2 output;
compute (cloud, output, tform);
// Check if a scaling parameter has been given
double divider[3];
if (parse_3x_arguments (argc, argv, "-scale", divider[0], divider[1], divider[2]) > -1)
{
print_highlight ("Scaling XYZ data with the following values: %f, %f, %f\n", divider[0], divider[1], divider[2]);
scaleInPlace (output, divider);
}
// Save into the second file
saveCloud (argv[p_file_indices[1]], output);
}
