void Tracker::spin()
{
ros::Rate loopRateTracking(100);
tf::Transform transform;
std_msgs::Header lastHeader;
while (!exiting())
{
// When a camera sequence is played several times,
// the seq id will decrease, in this case we want to
// continue the tracking.
if (header_.seq < lastHeader.seq)
lastTrackedImage_ = 0;
if (lastTrackedImage_ < header_.seq)
{
lastTrackedImage_ = header_.seq;
// If we can estimate the camera displacement using tf,
// we update the cMo to compensate for robot motion.
if (compensateRobotMotion_)
try
{
tf::StampedTransform stampedTransform;
listener_.lookupTransform
(header_.frame_id, // camera frame name
header_.stamp, // current image time
header_.frame_id, // camera frame name
lastHeader.stamp, // last processed image time
worldFrameId_, // frame attached to the environment
stampedTransform
);
vpHomogeneousMatrix newMold;
transformToVpHomogeneousMatrix (newMold, stampedTransform);
cMo_ = newMold * cMo_;
mutex_.lock();
tracker_->setPose(image_, cMo_);
mutex_.unlock();
}
catch(tf::TransformException& e)
{
mutex_.unlock();
}
// If we are lost but an estimation of the object position
// is provided, use it to try to reinitialize the system.
if (state_ == LOST)
{
// If the last received message is recent enough,
// use it otherwise do nothing.
if (ros::Time::now () - objectPositionHint_.header.stamp
< ros::Duration (1.))
transformToVpHomogeneousMatrix
(cMo_, objectPositionHint_.transform);
mutex_.lock();
tracker_->setPose(image_, cMo_);
mutex_.unlock();
}
// We try to track the image even if we are lost,
// in the case the tracker recovers...
if (state_ == TRACKING || state_ == LOST)
try
{
mutex_.lock();
// tracker_->setPose(image_, cMo_); // Removed as it is not necessary when the pose is not modified from outside.
tracker_->track(image_);
tracker_->getPose(cMo_);
mutex_.unlock();
}
catch(...)
{
mutex_.unlock();
ROS_WARN_THROTTLE(10, "tracking lost");
state_ = LOST;
}
// Publish the tracking result.
if (state_ == TRACKING)
{
geometry_msgs::Transform transformMsg;
vpHomogeneousMatrixToTransform(transformMsg, cMo_);
// Publish position.
if (transformationPublisher_.getNumSubscribers () > 0)
{
geometry_msgs::TransformStampedPtr objectPosition
(new geometry_msgs::TransformStamped);
objectPosition->header = header_;
objectPosition->child_frame_id = childFrameId_;
objectPosition->transform = transformMsg;
transformationPublisher_.publish(objectPosition);
}
// Publish result.
if (resultPublisher_.getNumSubscribers () > 0)
{
geometry_msgs::PoseWithCovarianceStampedPtr result
(new geometry_msgs::PoseWithCovarianceStamped);
result->header = header_;
result->pose.pose.position.x =
transformMsg.translation.x;
result->pose.pose.position.y =
transformMsg.translation.y;
result->pose.pose.position.z =
transformMsg.translation.z;
result->pose.pose.orientation.x =
transformMsg.rotation.x;
result->pose.pose.orientation.y =
transformMsg.rotation.y;
result->pose.pose.orientation.z =
transformMsg.rotation.z;
result->pose.pose.orientation.w =
transformMsg.rotation.w;
const vpMatrix& covariance =
tracker_->getCovarianceMatrix();
for (unsigned i = 0; i < covariance.getRows(); ++i)
for (unsigned j = 0; j < covariance.getCols(); ++j)
{
unsigned idx = i * covariance.getCols() + j;
if (idx >= 36)
continue;
result->pose.covariance[idx] = covariance[i][j];
}
resultPublisher_.publish(result);
}
// Publish moving edge sites.
if (movingEdgeSitesPublisher_.getNumSubscribers () > 0)
{
visp_tracker::MovingEdgeSitesPtr sites
(new visp_tracker::MovingEdgeSites);
updateMovingEdgeSites(sites);
sites->header = header_;
movingEdgeSitesPublisher_.publish(sites);
}
// Publish KLT points.
if (kltPointsPublisher_.getNumSubscribers () > 0)
{
visp_tracker::KltPointsPtr klt
(new visp_tracker::KltPoints);
updateKltPoints(klt);
klt->header = header_;
kltPointsPublisher_.publish(klt);
}
// Publish to tf.
transform.setOrigin
(tf::Vector3(transformMsg.translation.x,
transformMsg.translation.y,
transformMsg.translation.z));
transform.setRotation
(tf::Quaternion
(transformMsg.rotation.x,
transformMsg.rotation.y,
transformMsg.rotation.z,
transformMsg.rotation.w));
transformBroadcaster_.sendTransform
(tf::StampedTransform
(transform,
header_.stamp,
header_.frame_id,
childFrameId_));
}
}
lastHeader = header_;
spinOnce();
loopRateTracking.sleep();
}
}
int main(int argc, char** argv) {
if (argc < 3) {
printf("Usage: %s img1.png img2.png ... imgN.png\n", argv[0]);
exit(1);
}
const int imageCount = argc - 1;
CImg<float> initImg;
CImg<uint8_t> initImgGray;
CImg<float> curImg;
CImg<uint8_t> curImgGray;
// Load a grayscale image from RGB
initImg = CImg<float>::get_load(argv[1]).RGBtoLab();
initImgGray = initImg.get_shared_channel(0);
int originalWidth = initImg.width();
int originalHeight = initImg.height();
const int workingWidth = originalWidth;
const int workingHeight = originalHeight;
const Eigen::Vector2d imageCenter(workingWidth / 2.0, workingHeight / 2.0);
const double imageSize = max(workingWidth / 2.0, workingHeight / 2.0);
printf("Image size = %d x %d\n", workingWidth, workingHeight);
const int numPoints = 20000;
const int numMainPoints = 3000;
const int windowSize = 31;
CVOpticalFlow klt(windowSize, 15);
float minDistance = min(workingWidth, workingHeight) * 1.0 / sqrt((float) numPoints);
minDistance = max(5.0f, minDistance);
klt.init(initImgGray, numPoints, minDistance);
const int numGoodPoints = klt.sortFeatures(min(workingWidth, workingHeight) * 1.0 / sqrt(numMainPoints));
printf("Feature count = %d\n", klt.featureCount());
DepthReconstruction reconstruct;
CVFundamentalMatrixEstimator fundMatEst;
reconstruct.init(imageCount - 1, klt.featureCount(), numGoodPoints);
vector<Eigen::Vector2f> keypoints;
for (int pointI = 0; pointI < klt.featureCount(); pointI++) {
Eigen::Vector2f match0;
Eigen::Vector2f matchOther;
float error;
klt.getMatch(pointI, match0, matchOther, error);
keypoints.push_back(match0);
match0 -= imageCenter.cast<float>();
match0 /= imageSize;
reconstruct.setKeypoint(pointI, match0.cast<double>());
}
for (int imgI = 1; imgI < imageCount; imgI++) {
printf("Processing image #%d\n", imgI);
curImg = CImg<float>::get_load(argv[1 + imgI]).RGBtoLab();
curImgGray = curImg.get_shared_channel(0);
assert(curImg.width() == originalWidth);
assert(curImg.height() == originalHeight);
printf("Computing KLT\n");
klt.compute(curImgGray);
printf("Done\n");
for (int pointI = 0; pointI < klt.featureCount(); pointI++) {
Eigen::Vector2f match0;
Eigen::Vector2f matchOther;
float error;
klt.getMatch(pointI, match0, matchOther, error);
matchOther -= imageCenter.cast<float>();
matchOther /= imageSize;
reconstruct.addObservation(imgI - 1, pointI, matchOther.cast<double>());
}
}
reconstruct.solve();
// Visualize the result
if (true) {
CImg<float> depthVis(workingWidth * 0.25, workingHeight * 0.25);
reconstruct.visualize(depthVis, 1, 0.99, 1.0, false);
depthVis.normalize(0, 255).save_png("results/depth_estimate.png");
depthVis.display();
}
{
const vector<double> rDepths = reconstruct.getDepths();
// double minDepth = *(min_element(&(rDepths.front()), &(rDepths[numMainPoints])));
// double maxDepth = *(max_element(&(rDepths.front()), &(rDepths[numMainPoints])));
// printf("depth range = [%f, %f]\n", minDepth, maxDepth);
vector<double> depth;
TriQPBO qpbo(initImg, keypoints);
// qpbo.addCandidateVertexDepths(rDepths);
for (int camI = 0; camI < imageCount - 1; camI++) {
if (camI < 0 || reconstruct.isInlierCamera(camI)) {
depth.clear();
reconstruct.getAllDepthSamples(camI, depth);
qpbo.addCandidateVertexDepths(depth);
}
}
printf("initializing qpbo\n");
qpbo.init();
printf("done\n");
CImg<float> colorVis(workingWidth, workingHeight, 1, 3);
colorVis.fill(0);
qpbo.visualizeTriangulation(colorVis);
colorVis.display();
colorVis.normalize(0, 255).save_png("results/delaunay.png");
{
CImg<double> depthVis(workingWidth, workingHeight);
depthVis.fill(0.0);
qpbo.denseInterp(depthVis);
double medianDepth = depthVis.median();
depthVis.min(medianDepth * 30);
depthVis.max(0.0);
depthVis.normalize(0, 255).save_png("results/initial_triangle_depth.png");
// (initImg, depthVis).display();
}
printf("Enter unary cost factor...\n");
float unaryCostFactor = 0;
// cin >> unaryCostFactor;
// unaryCostFactor << cin;
// qpbo.solveAlphaExpansion(minDepth, maxDepth, 32, 2, unaryCostFactor);
qpbo.solve(2, unaryCostFactor);
{
CImg<double> depthVis(workingWidth, workingHeight);
depthVis.fill(0.0);
qpbo.denseInterp(depthVis);
double medianDepth = depthVis.median();
depthVis.min(medianDepth * 30);
depthVis.max(0.0);
depthVis.normalize(0, 255).save_png("results/qpbo_triangle_depth.png");
}
// (initImg, depthVis).display();
for (int i = 0; i < 1; i++) {
qpbo.smoothAvg();
}
{
CImg<double> depthVis(workingWidth, workingHeight);
depthVis.fill(0.0);
qpbo.denseInterp(depthVis);
double medianDepth = depthVis.median();
depthVis.min(medianDepth * 30);
depthVis.max(0.0);
depthVis.normalize(0, 255).save_png("results/qpbo_smoothed_triangle_depth.png");
}
qpbo.solveSmoothHack();
// Output wrl file
{
vector<array<Eigen::Vector3d, 3>> triangles;
qpbo.getSmoothTriangles(triangles);
fstream wrl;
wrl.open("results/model.wrl", std::ios_base::out);
vector<double> depths;
for (const auto& tri : triangles) {
for (const auto& v : tri) {
depths.push_back(v.z());
}
}
sort(depths.begin(), depths.end());
double maxDepth = depths[depths.size() * 0.75];
wrl << "#VRML V2.0 utf8" << endl;
wrl << "Transform { children [" << endl;
wrl << "WorldInfo {" << endl;
wrl << "info [\"Input Format: pol\"]" << endl;
wrl << "}" << endl;
wrl << "Shape {" << endl;
wrl << "\tappearance Appearance { material" << endl;
wrl << "Material {" << endl;
wrl << "diffuseColor 1.0 1.0 1.0" << endl;
wrl << "transparency 0" << endl;
wrl << "}" << endl;
wrl << "texture ImageTexture {" << endl;
wrl << "url [\"" << argv[1] << "\"]" << endl;
wrl << "}}" << endl;
wrl << "geometry IndexedFaceSet {" << endl;
wrl << "coord\nCoordinate {" << endl;
wrl << "point [" << endl;
size_t vertexIndex = 0;
for (const array<Eigen::Vector3d, 3>& tri : triangles) {
if (vertexIndex != 0) {
wrl << ",";
}
for (int i = 0; i < 3; i++) {
if (i != 0) {
wrl << ",";
}
double depth = -1.0 * (min(tri[i].z(), maxDepth) / maxDepth);
wrl <<
" " << (depth - 1) * ((tri[i].x() / initImg.width()) - 0.5) <<
" " << (depth - 1) * ((tri[i].y() / initImg.height()) - 0.5) <<
// " " << -1.0 * log(1.0 + tri[i].z()) << endl;
" " << depth << endl;
}
vertexIndex++;
}
wrl << "]" << endl;
wrl << "}" << endl;
wrl << "colorPerVertex FALSE" << endl;
wrl << "normal Normal { vector [" << endl;
vertexIndex = 0;
for (const array<Eigen::Vector3d, 3>& tri : triangles) {
if (vertexIndex != 0) {
wrl << ",";
}
for (int i = 0; i < 3; i++) {
if (i != 0) {
wrl << ",";
}
wrl <<
" " << 0 <<
" " << 0 <<
" " << 1 << endl;
}
vertexIndex++;
}
wrl << "]}" << endl;
wrl << "texCoord TextureCoordinate {" << endl;
wrl << "point [" << endl;
vertexIndex = 0;
for (const array<Eigen::Vector3d, 3>& tri : triangles) {
if (vertexIndex != 0) {
wrl << ",";
}
for (int i = 0; i < 3; i++) {
if (i != 0) {
wrl << ",";
}
wrl <<
" " << tri[i].x() / initImg.width() <<
" " << tri[i].y() / initImg.height() << endl;
}
vertexIndex++;
}
wrl << "]" << endl;
wrl << "}" << endl;
wrl << "coordIndex [" << endl;
vertexIndex = 0;
for (const array<Eigen::Vector3d, 3>& tri : triangles) {
if (vertexIndex != 0) {
wrl << ",";
}
for (int i = 0; i < 3; i++) {
if (i != 0) {
wrl << ",";
}
wrl << " " << vertexIndex;
vertexIndex++;
}
wrl << ", -1 " << endl;
}
wrl << "]" << endl;
wrl << "}}]}" << endl;
wrl.close();
}
/*
for (int camI = 0; camI < imageCount - 1; camI++) {
if (camI < 0 || reconstruct.isInlierCamera(camI)) {
vector<double> depth; // reconstruct.getDepths();
reconstruct.getAllDepthSamples(camI, depth);
printf("initializing qpbo\n");
TriQPBO qpbo(initImg, keypoints, depth);
printf("done\n");
// CImg<float> colorVis(workingWidth, workingHeight, 1, 3);
// colorVis.fill(0);
// qpbo.visualizeTriangulation(colorVis);
// colorVis.display();
qpbo.solve();
CImg<double> depthVis(workingWidth, workingHeight);
depthVis.fill(0.0);
qpbo.denseInterp(depthVis);
double medianDepth = depthVis.median();
depthVis.min(medianDepth * 10);
depthVis.max(0.0);
depthVis.display();
}
}
*/
/*
for (int camI = -1; camI < imageCount - 1; camI++) {
if (camI < 0 || reconstruct.isInlierCamera(camI)) {
TriQPBO qpbo(initImg, keypoints);
vector<double> depth(keypoints.size());
if (camI < 0) {
depth = reconstruct.getDepths();
} else {
reconstruct.getAllDepthSamples(camI, depth);
}
qpbo.addCandidateVertexDepths(depth, false);
qpbo.solve();
CImg<double> depthVis(workingWidth, workingHeight);
depthVis.fill(0.0);
qpbo.denseInterp(depthVis);
double medianDepth = depthVis.median();
depthVis.min(medianDepth * 10);
depthVis.display();
}
}
*/
}
#if false
const bool display_daisy_stereo = false;
if (display_daisy_stereo) {
SparseDaisyStereo daisyStereo;
daisyStereo.init(initImg.get_shared_channel(0));
vector<Eigen::Vector2f> pointSamples;
for (int y = 0; y < 128; y++) {
for (int x = 0; x < 128; x++) {
pointSamples.push_back(Eigen::Vector2f(
x * workingWidth / 128.0f,
y * workingHeight / 128.0f));
}
}
for (int imgI = 1; imgI < imageCount; imgI++) {
printf("Rectifying image #%d\n", imgI);
curImg = CImg<float>::get_load(argv[1 + imgI]).RGBtoLab();
PolarFundamentalMatrix polarF;
bool rectificationPossible = reconstruct.getPolarFundamentalMatrix(
imgI - 1,
Eigen::Vector2d(workingWidth / 2.0, workingHeight / 2.0),
max(workingWidth / 2.0, workingHeight / 2.0),
polarF);
if (!rectificationPossible) {
printf("Rectification not possible, epipoles at infinity.\n");
continue;
}
vector<Eigen::Vector2f> matches(pointSamples.size());
vector<float> matchDistances(pointSamples.size());
daisyStereo.match(curImg.get_shared_channel(0), polarF, pointSamples, matches,
matchDistances);
}
}
#endif
#if false
bool display_dense_interp = false;
if (display_dense_interp) {
float scaleFactor = 256.0f / max(originalWidth, originalHeight);
int denseInterpWidth = scaleFactor * originalWidth;
int denseInterpHeight = scaleFactor * originalHeight;
SparseInterp<double> interp(denseInterpWidth, denseInterpHeight);
interp.init(reconstruct.getPointCount(), 1.0);
const int minInlierCount = 1;
for (size_t i = 0; i < reconstruct.getPointCount(); i++) {
double depth;
size_t inlierC;
const Eigen::Vector2d& pt = reconstruct.getDepthSample(i, depth, inlierC);
if (depth > 0) {
Eigen::Vector2d ptImg = (pt * max(denseInterpWidth, denseInterpHeight) / 2.0);
ptImg.x() += denseInterpWidth / 2.0;
ptImg.y() += denseInterpHeight / 2.0;
interp.insertSample(i, ptImg.x() + 0.5, ptImg.y() + 0.5, depth);
}
}
interp.solve();
}
#endif
#if false
bool display_dense_flow = false;
if (display_dense_flow) {
CImg<uint8_t> initDown = initImgGray;
CImg<uint8_t> curDown;
float scaleFactor = 1024.0f / max(originalWidth, originalHeight);
int scaledWidth = scaleFactor * originalWidth;
int scaledHeight = scaleFactor * originalHeight;
// Resize with moving-average interpolation
initDown.resize(scaledWidth, scaledHeight, -100, -100, 2);
CVDenseOpticalFlow denseFlow;
for (int imgI = 1; imgI < imageCount; imgI++) {
printf("Computing dense flow for image #%d\n", imgI);
curDown = CImg<uint8_t>::get_load(argv[1 + imgI]).RGBtoLab().channel(0);
curDown.resize(scaledWidth, scaledHeight, -100, -100, 2);
denseFlow.compute(initDown, curDown);
CImg<float> flow(scaledWidth, scaledHeight, 2);
cimg_forXY(flow, x, y) {
denseFlow.getRelativeFlow(x, y, flow(x, y, 0), flow(x, y, 1));
}
(flow.get_shared_slice(0), flow.get_shared_slice(1)).display();
}
}
