int SlamSystem::findConstraintsForNewKeyFrames(Frame* newKeyFrame, bool forceParent, bool useFABMAP, float closeCandidatesTH)
{
if(!newKeyFrame->hasTrackingParent())
{
newConstraintMutex.lock();
keyFrameGraph->addKeyFrame(newKeyFrame);
newConstraintAdded = true;
newConstraintCreatedSignal.notify_all();
newConstraintMutex.unlock();
return 0;
}
if(!forceParent && (newKeyFrame->lastConstraintTrackedCamToWorld * newKeyFrame->getScaledCamToWorld().inverse()).log().norm() < 0.01)
return 0;
newKeyFrame->lastConstraintTrackedCamToWorld = newKeyFrame->getScaledCamToWorld();
// =============== get all potential candidates and their initial relative pose. =================
std::vector<KFConstraintStruct*, Eigen::aligned_allocator<KFConstraintStruct*> > constraints;
Frame* fabMapResult = 0;
std::unordered_set<Frame*, std::hash<Frame*>, std::equal_to<Frame*>,
Eigen::aligned_allocator< Frame* > > candidates = trackableKeyFrameSearch->findCandidates(newKeyFrame, fabMapResult, useFABMAP, closeCandidatesTH);
std::map< Frame*, Sim3, std::less<Frame*>, Eigen::aligned_allocator<std::pair<Frame*, Sim3> > > candidateToFrame_initialEstimateMap;
// erase the ones that are already neighbours.
for(std::unordered_set<Frame*>::iterator c = candidates.begin(); c != candidates.end();)
{
if(newKeyFrame->neighbors.find(*c) != newKeyFrame->neighbors.end())
{
if(enablePrintDebugInfo && printConstraintSearchInfo)
printf("SKIPPING %d on %d cause it already exists as constraint.\n", (*c)->id(), newKeyFrame->id());
c = candidates.erase(c);
}
else
++c;
}
poseConsistencyMutex.lock_shared();
for (Frame* candidate : candidates)
{
Sim3 candidateToFrame_initialEstimate = newKeyFrame->getScaledCamToWorld().inverse() * candidate->getScaledCamToWorld();
candidateToFrame_initialEstimateMap[candidate] = candidateToFrame_initialEstimate;
}
std::unordered_map<Frame*, int> distancesToNewKeyFrame;
if(newKeyFrame->hasTrackingParent())
keyFrameGraph->calculateGraphDistancesToFrame(newKeyFrame->getTrackingParent(), &distancesToNewKeyFrame);
poseConsistencyMutex.unlock_shared();
// =============== distinguish between close and "far" candidates in Graph =================
// Do a first check on trackability of close candidates.
std::unordered_set<Frame*, std::hash<Frame*>, std::equal_to<Frame*>,
Eigen::aligned_allocator< Frame* > > closeCandidates;
std::vector<Frame*, Eigen::aligned_allocator<Frame*> > farCandidates;
Frame* parent = newKeyFrame->hasTrackingParent() ? newKeyFrame->getTrackingParent() : 0;
int closeFailed = 0;
int closeInconsistent = 0;
SO3 disturbance = SO3::exp(Sophus::Vector3d(0.05,0,0));
for (Frame* candidate : candidates)
{
if (candidate->id() == newKeyFrame->id())
continue;
if(!candidate->pose->isInGraph)
continue;
if(newKeyFrame->hasTrackingParent() && candidate == newKeyFrame->getTrackingParent())
continue;
if(candidate->idxInKeyframes < INITIALIZATION_PHASE_COUNT)
continue;
SE3 c2f_init = se3FromSim3(candidateToFrame_initialEstimateMap[candidate].inverse()).inverse();
c2f_init.so3() = c2f_init.so3() * disturbance;
SE3 c2f = constraintSE3Tracker->trackFrameOnPermaref(candidate, newKeyFrame, c2f_init);
if(!constraintSE3Tracker->trackingWasGood) {closeFailed++; continue;}
SE3 f2c_init = se3FromSim3(candidateToFrame_initialEstimateMap[candidate]).inverse();
f2c_init.so3() = disturbance * f2c_init.so3();
SE3 f2c = constraintSE3Tracker->trackFrameOnPermaref(newKeyFrame, candidate, f2c_init);
if(!constraintSE3Tracker->trackingWasGood) {closeFailed++; continue;}
if((f2c.so3() * c2f.so3()).log().norm() >= 0.09) {closeInconsistent++; continue;}
closeCandidates.insert(candidate);
}
int farFailed = 0;
int farInconsistent = 0;
for (Frame* candidate : candidates)
{
if (candidate->id() == newKeyFrame->id())
continue;
if(!candidate->pose->isInGraph)
continue;
if(newKeyFrame->hasTrackingParent() && candidate == newKeyFrame->getTrackingParent())
continue;
if(candidate->idxInKeyframes < INITIALIZATION_PHASE_COUNT)
continue;
if(candidate == fabMapResult)
{
farCandidates.push_back(candidate);
continue;
}
if(distancesToNewKeyFrame.at(candidate) < 4)
continue;
farCandidates.push_back(candidate);
}
int closeAll = closeCandidates.size();
int farAll = farCandidates.size();
// erase the ones that we tried already before (close)
for(std::unordered_set<Frame*>::iterator c = closeCandidates.begin(); c != closeCandidates.end();)
{
if(newKeyFrame->trackingFailed.find(*c) == newKeyFrame->trackingFailed.end())
{
++c;
continue;
}
auto range = newKeyFrame->trackingFailed.equal_range(*c);
bool skip = false;
Sim3 f2c = candidateToFrame_initialEstimateMap[*c].inverse();
for (auto it = range.first; it != range.second; ++it)
{
if((f2c * it->second).log().norm() < 0.1)
{
skip=true;
break;
}
}
if(skip)
{
if(enablePrintDebugInfo && printConstraintSearchInfo)
printf("SKIPPING %d on %d (NEAR), cause we already have tried it.\n", (*c)->id(), newKeyFrame->id());
c = closeCandidates.erase(c);
}
else
++c;
}
// erase the ones that are already neighbours (far)
for(unsigned int i=0;i<farCandidates.size();i++)
{
if(newKeyFrame->trackingFailed.find(farCandidates[i]) == newKeyFrame->trackingFailed.end())
continue;
auto range = newKeyFrame->trackingFailed.equal_range(farCandidates[i]);
bool skip = false;
for (auto it = range.first; it != range.second; ++it)
{
if((it->second).log().norm() < 0.2)
{
skip=true;
break;
}
}
if(skip)
{
if(enablePrintDebugInfo && printConstraintSearchInfo)
printf("SKIPPING %d on %d (FAR), cause we already have tried it.\n", farCandidates[i]->id(), newKeyFrame->id());
farCandidates[i] = farCandidates.back();
farCandidates.pop_back();
i--;
}
}
if (enablePrintDebugInfo && printConstraintSearchInfo)
printf("Final Loop-Closure Candidates: %d / %d close (%d failed, %d inconsistent) + %d / %d far (%d failed, %d inconsistent) = %d\n",
(int)closeCandidates.size(),closeAll, closeFailed, closeInconsistent,
(int)farCandidates.size(), farAll, farFailed, farInconsistent,
(int)closeCandidates.size() + (int)farCandidates.size());
// =============== limit number of close candidates ===============
// while too many, remove the one with the highest connectivity.
while((int)closeCandidates.size() > maxLoopClosureCandidates)
{
Frame* worst = 0;
int worstNeighbours = 0;
for(Frame* f : closeCandidates)
{
int neightboursInCandidates = 0;
for(Frame* n : f->neighbors)
if(closeCandidates.find(n) != closeCandidates.end())
neightboursInCandidates++;
if(neightboursInCandidates > worstNeighbours || worst == 0)
{
worst = f;
worstNeighbours = neightboursInCandidates;
}
}
closeCandidates.erase(worst);
}
// =============== limit number of far candidates ===============
// delete randomly
int maxNumFarCandidates = (maxLoopClosureCandidates +1) / 2;
if(maxNumFarCandidates < 5) maxNumFarCandidates = 5;
while((int)farCandidates.size() > maxNumFarCandidates)
{
int toDelete = rand() % farCandidates.size();
if(farCandidates[toDelete] != fabMapResult)
{
farCandidates[toDelete] = farCandidates.back();
farCandidates.pop_back();
}
}
// =============== TRACK! ===============
// make tracking reference for newKeyFrame.
newKFTrackingReference->importFrame(newKeyFrame);
for (Frame* candidate : closeCandidates)
{
KFConstraintStruct* e1=0;
KFConstraintStruct* e2=0;
testConstraint(
candidate, e1, e2,
candidateToFrame_initialEstimateMap[candidate],
loopclosureStrictness);
if(enablePrintDebugInfo && printConstraintSearchInfo)
printf(" CLOSE (%d)\n", distancesToNewKeyFrame.at(candidate));
if(e1 != 0)
{
constraints.push_back(e1);
constraints.push_back(e2);
// delete from far candidates if it's in there.
for(unsigned int k=0;k<farCandidates.size();k++)
{
if(farCandidates[k] == candidate)
{
if(enablePrintDebugInfo && printConstraintSearchInfo)
printf(" DELETED %d from far, as close was successful!\n", candidate->id());
farCandidates[k] = farCandidates.back();
farCandidates.pop_back();
}
}
}
}
for (Frame* candidate : farCandidates)
{
KFConstraintStruct* e1=0;
KFConstraintStruct* e2=0;
testConstraint(
candidate, e1, e2,
Sim3(),
loopclosureStrictness);
if(enablePrintDebugInfo && printConstraintSearchInfo)
printf(" FAR (%d)\n", distancesToNewKeyFrame.at(candidate));
if(e1 != 0)
{
constraints.push_back(e1);
constraints.push_back(e2);
}
}
if(parent != 0 && forceParent)
{
KFConstraintStruct* e1=0;
KFConstraintStruct* e2=0;
testConstraint(
parent, e1, e2,
candidateToFrame_initialEstimateMap[parent],
100);
if(enablePrintDebugInfo && printConstraintSearchInfo)
printf(" PARENT (0)\n");
if(e1 != 0)
{
constraints.push_back(e1);
constraints.push_back(e2);
}
else
{
float downweightFac = 5;
const float kernelDelta = 5 * sqrt(6000*loopclosureStrictness) / downweightFac;
printf("warning: reciprocal tracking on new frame failed badly, added odometry edge (Hacky).\n");
poseConsistencyMutex.lock_shared();
constraints.push_back(new KFConstraintStruct());
constraints.back()->firstFrame = newKeyFrame;
constraints.back()->secondFrame = newKeyFrame->getTrackingParent();
constraints.back()->secondToFirst = constraints.back()->firstFrame->getScaledCamToWorld().inverse() * constraints.back()->secondFrame->getScaledCamToWorld();
constraints.back()->information <<
0.8098,-0.1507,-0.0557, 0.1211, 0.7657, 0.0120, 0,
-0.1507, 2.1724,-0.1103,-1.9279,-0.1182, 0.1943, 0,
-0.0557,-0.1103, 0.2643,-0.0021,-0.0657,-0.0028, 0.0304,
0.1211,-1.9279,-0.0021, 2.3110, 0.1039,-0.0934, 0.0005,
0.7657,-0.1182,-0.0657, 0.1039, 1.0545, 0.0743,-0.0028,
0.0120, 0.1943,-0.0028,-0.0934, 0.0743, 0.4511, 0,
0,0, 0.0304, 0.0005,-0.0028, 0, 0.0228;
constraints.back()->information *= (1e9/(downweightFac*downweightFac));
constraints.back()->robustKernel = new g2o::RobustKernelHuber();
constraints.back()->robustKernel->setDelta(kernelDelta);
constraints.back()->meanResidual = 10;
constraints.back()->meanResidualD = 10;
constraints.back()->meanResidualP = 10;
constraints.back()->usage = 0;
poseConsistencyMutex.unlock_shared();
}
}
newConstraintMutex.lock();
keyFrameGraph->addKeyFrame(newKeyFrame);
for(unsigned int i=0;i<constraints.size();i++)
keyFrameGraph->insertConstraint(constraints[i]);
newConstraintAdded = true;
newConstraintCreatedSignal.notify_all();
newConstraintMutex.unlock();
newKFTrackingReference->invalidate();
candidateTrackingReference->invalidate();
return constraints.size();
}
int main(int argc, char** argv)
{
////////////////////////////////////////////////////////////////////
// Create command line options. Check if we should print usage.
GetPot cl(argc,argv);
if(cl.search(3, "-help", "-h", "?") || argc < 2) {
std::cout << sUriInfo << std::endl;
return -1;
}
////////////////////////////////////////////////////////////////////
// Default configuration values
// Default grid printed on US Letter
int grid_preset = GridPresetGWUSmall;
double grid_spacing;
int grid_rows;
int grid_cols;
uint32_t grid_seed;
double grid_large_rad;
double grid_small_rad;
std::string save_grid;
// Use no input cameras by default
std::vector<calibu::CameraAndPose > input_cameras;
// Fix cameras intrinsic parameters during optimisation, changing
// only their relative poses.
bool fix_intrinsics = false;
// Require user to start playing the video
bool start_paused = false;
// Output file for camera rig
std::string output_filename = "cameras.xml";
////////////////////////////////////////////////////////////////////
// Parse command line
grid_preset = cl.follow(grid_preset, "-grid-preset");
switch(grid_preset)
{
case GridPresetGWUSmall:
grid_spacing = 0.254 / 18; // meters
grid_large_rad = 0.00423; // m
grid_small_rad = 0.00283; // m
grid_rows = 10; // grid dots
grid_cols = 19; // grid dots
grid_seed = 71;
break;
case GridPresetGoogleLarge:
grid_spacing = 0.03156; // meters
grid_large_rad = 0.00889; // m
grid_small_rad = 0.00635; // m
grid_rows = 36; // grid dots
grid_cols = 25; // grid dots
grid_seed = 71;
break;
}
grid_spacing = cl.follow(grid_spacing,"-grid-spacing");
grid_seed = cl.follow((int)grid_seed,"-grid-seed");
grid_cols = cl.follow((int)grid_cols,"-grid-cols");
grid_rows = cl.follow((int)grid_rows,"-grid-rows");
grid_large_rad = cl.follow(grid_large_rad,"-grid-large-rad");
grid_small_rad = cl.follow(grid_small_rad,"-grid-small-rad");
fix_intrinsics = cl.search(2, "-fix-intrinsics", "-f");
start_paused = cl.search(2, "-paused", "-p");
output_filename = cl.follow(output_filename.c_str(), 2, "-output", "-o");
save_grid = cl.follow("", "-save-grid");
const Eigen::Vector2i grid_size(grid_cols, grid_rows);
////////////////////////////////////////////////////////////////////
// Setup Grid pattern
ConicFinder conic_finder;
conic_finder.Params().conic_min_area = 4.0;
conic_finder.Params().conic_min_density = 0.6;
conic_finder.Params().conic_min_aspect = 0.2;
std::unique_ptr<TargetGridDot> target;
if(grid_preset == GridPresetGoogleLarge)
target.reset(new TargetGridDot(grid_spacing, GoogleLargeGrid()));
else if(grid_preset == GridPresetGWUSmall)
target.reset(new TargetGridDot(grid_spacing, GWUSmallGrid()));
else
target.reset(new TargetGridDot(grid_spacing, grid_size, grid_seed));
// Save grid and exit if required
if(!save_grid.empty()) {
saveGrid(*target, save_grid, grid_large_rad, grid_small_rad);
return 0;
}
////////////////////////////////////////////////////////////////////
// Setup Video Source
// Last argument or parameter - Video URI
std::string cam_param = cl.follow("", 2, "-video_url", "-cam");
std::string uri = cam_param.empty() ? argv[argc-1] : cam_param;
hal::Camera cam;
try {
cam = hal::Camera( uri );
} catch (...) {
if(!cam_param.empty())
std::cerr << "Could not create camera from URI: " << uri
<< std::endl;
return -1;
}
if(cam.Empty()) return -1;
// For the moment, assume all N cameras have same resolution
const size_t N = cam.NumChannels();
const size_t w = cam.Width();
const size_t h = cam.Height();
std::shared_ptr<pb::ImageArray> imageArray = pb::ImageArray::Create();
cam.Capture(*imageArray);
std::vector<cv::Mat> vImages;
std::vector<uint64_t> vSerialNos;
vImages.reserve(imageArray->Size());
vSerialNos.reserve(imageArray->Size());
for( int i = 0; i < imageArray->Size(); ++i ){
pb::Image& image = *(*imageArray)[i];
cv::Mat im = image.Mat();
if( im.type() != CV_8UC1 ){
std::cerr << "Input channels must be GRAY8 format. Use "
"Convert:[fmt=MONO8]// video scheme." << std::endl;
} else {
vImages.emplace_back(im);
vSerialNos.emplace_back(image.SerialNumber());
}
}
// Load camera hints from command line
cl.disable_loop();
cl.reset_cursor();
for(std::string filename = cl.follow("",2,"-cameras","-c");
!filename.empty(); filename = cl.follow("",2,"-cameras","-c") ) {
const size_t i = input_cameras.size();
if(i < N) {
if(filename == "fov") {
CameraModelT<Fov> starting_cam(w, h);
starting_cam.Params() << 300, 300, w/2.0, h/2.0, 0.2;
input_cameras.push_back( CameraAndPose(CameraModel(starting_cam), Sophus::SE3d() ) );
}else if(filename == "poly2") {
CameraModelT<Poly2> starting_cam(w, h);
starting_cam.Params() << 300, 300, w/2.0, h/2.0, 0.0, 0.0;
input_cameras.push_back( CameraAndPose(CameraModel(starting_cam), Sophus::SE3d() ) );
}else if(filename == "poly3" || filename =="poly") {
CameraModelT<Poly3> starting_cam(w, h);
starting_cam.Params() << 300, 300, w/2.0, h/2.0, 0.0, 0.0, 0.0;
input_cameras.push_back( CameraAndPose(CameraModel(starting_cam), Sophus::SE3d() ) );
}else if(filename == "kb4") {
CameraModelT<ProjectionKannalaBrandt> starting_cam(w, h);
starting_cam.Params() << 300, 300, w/2.0, h/2.0, 0.0, 0.0, 0.0, 0.0;
input_cameras.push_back( CameraAndPose(CameraModel(starting_cam), Sophus::SE3d() ) );
}else{
const CameraRig rig = ReadXmlRig(filename);
for(const CameraModelAndTransform& cop : rig.cameras ) {
input_cameras.push_back( CameraAndPose(cop.camera, cop.T_wc.inverse()) );
}
}
}else{
throw std::runtime_error("Too many camera files provided.");
}
}
if(input_cameras.size() > 0 && input_cameras.size() != N) {
std::cerr << "Number of cameras specified in files does not match video source" << std::endl;
return -1;
}
////////////////////////////////////////////////////////////////////
// Setup image processing pipeline
ImageProcessing image_processing(w,h);
image_processing.Params().black_on_white = true;
image_processing.Params().at_threshold = 0.9;
image_processing.Params().at_window_ratio = 30.0;
CVarUtils::AttachCVar("proc.adaptive.threshold", &image_processing.Params().at_threshold);
CVarUtils::AttachCVar("proc.adaptive.window_ratio", &image_processing.Params().at_window_ratio);
CVarUtils::AttachCVar("proc.black_on_white", &image_processing.Params().black_on_white);
////////////////////////////////////////////////////////////////////
// Initialize Calibration object and tracking params
Calibrator calibrator;
calibrator.FixCameraIntrinsics(fix_intrinsics);
int calib_cams[N];
bool tracking_good[N];
std::vector<Sophus::SE3d> T_hw;
T_hw.resize(N);
for(size_t i=0; i<N; ++i) {
const int w_i = cam.Width();
const int h_i = cam.Height();
if(i < input_cameras.size() ) {
input_cameras[i].camera.SetSerialNumber(vSerialNos[i]);
calib_cams[i] = calibrator.AddCamera(
input_cameras[i].camera, input_cameras[i].T_ck
);
}else{
// Generic starting set of parameters.
CameraModelT<Fov> starting_cam(w_i, h_i);
starting_cam.Params() << 300, 300, w_i/2.0, h_i/2.0, 0.2;
starting_cam.SetSerialNumber(vSerialNos[i]);
calib_cams[i] = calibrator.AddCamera(
CameraModel(starting_cam),
Sophus::SE3d()
);
}
}
////////////////////////////////////////////////////////////////////
// Setup GUI
const int PANEL_WIDTH = 150;
pangolin::CreateWindowAndBind("Main",(N+1)*w/2.0+PANEL_WIDTH,h/2.0);
// Make things look prettier...
glEnable(GL_LINE_SMOOTH);
glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST );
glHint( GL_LINE_SMOOTH_HINT, GL_NICEST );
glEnable (GL_BLEND);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glDepthFunc( GL_LEQUAL );
glEnable( GL_DEPTH_TEST );
glLineWidth(1.7);
// Pangolin 3D Render state
pangolin::OpenGlRenderState stacks;
stacks.SetProjectionMatrix(pangolin::ProjectionMatrixRDF_TopLeft(640,480,420,420,320,240,0.01,1E6));
stacks.SetModelViewMatrix(pangolin::ModelViewLookAtRDF(0,0,-0.5, 0,0,0, 0, -1, 0) );
// Create viewport for video with fixed aspect
pangolin::CreatePanel("ui").SetBounds(1.0,0.0,0,pangolin::Attach::Pix(PANEL_WIDTH));
pangolin::View& container = pangolin::CreateDisplay()
.SetBounds(1.0,0.0, pangolin::Attach::Pix(PANEL_WIDTH),1.0)
.SetLayout(pangolin::LayoutEqual);
// Add view for each camera stream
for(size_t c=0; c < N; ++c) {
container.AddDisplay( pangolin::CreateDisplay().SetAspect(w/(float)h) );
}
// Add 3d view, attach input handler
pangolin::Handler3D handler(stacks);
pangolin::View& v3D = pangolin::CreateDisplay().SetAspect((float)w/h).SetHandler(&handler);
container.AddDisplay(v3D);
// OpenGl Texture for video frame
pangolin::GlTexture tex(w,h,GL_LUMINANCE8);
////////////////////////////////////////////////////////////////////
// Display Variables
pangolin::Var<bool> run("ui.Play video", !start_paused, true);
pangolin::Var<double> disp_mse("ui.MSE");
pangolin::Var<int> disp_frame("ui.frame");
pangolin::Var<bool> add("ui.Add Frames", true, true);
pangolin::Var<bool> disp_thresh("ui.Display Thresh",false);
pangolin::Var<bool> disp_lines("ui.Display Lines",true);
pangolin::Var<bool> disp_cross("ui.Display crosses",true);
pangolin::Var<bool> disp_bbox("ui.Display bbox",true);
////////////////////////////////////////////////////////////////////
// Key shortcuts
// 1,2,3,... keys hide and show viewports
for(size_t i=0; i<container.NumChildren(); ++i) {
pangolin::RegisterKeyPressCallback('1'+i, [&container,i](){container[i].ToggleShow();} );
}
pangolin::RegisterKeyPressCallback('[', [&](){calibrator.Start();} );
pangolin::RegisterKeyPressCallback(']', [&](){calibrator.Stop();} );
bool step = false;
pangolin::RegisterKeyPressCallback(pangolin::PANGO_SPECIAL+ pangolin::PANGO_KEY_RIGHT, [&](){step = true;} );
pangolin::RegisterKeyPressCallback(' ', [&](){run = !run;} );
pangolin::RegisterKeyPressCallback('r', [&](){calibrator.PrintResults();} );
pangolin::RegisterKeyPressCallback('q', &pangolin::Quit);
////////////////////////////////////////////////////////////////////
// Main event loop
for(int frame=0; !pangolin::ShouldQuit();){
const bool go = (frame==0) || run || pangolin::Pushed(step);
int calib_frame = -1;
if( go ) {
if( cam.Capture( vImages ) ){
if(add) {
calib_frame = calibrator.AddFrame(Sophus::SE3d(Sophus::SO3d(), Eigen::Vector3d(0,0,1000)) );
}
++frame;
}else{
run = false;
}
}
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
for(size_t iI = 0; iI < N; ++iI)
{
if (vImages.size() != N) break;
image_processing.Process( vImages[iI].data, vImages[iI].cols, vImages[iI].rows, vImages[iI].cols );
conic_finder.Find( image_processing );
const std::vector<Conic, Eigen::aligned_allocator<Conic> >& conics =
conic_finder.Conics();
std::vector<int> ellipse_target_map;
tracking_good[iI] = target->FindTarget(
image_processing, conic_finder.Conics(),
ellipse_target_map
);
if(tracking_good[iI]) {
// Generate map and point structures
std::vector<Eigen::Vector2d,
Eigen::aligned_allocator<Eigen::Vector2d> > ellipses;
for( size_t i=0; i < conics.size(); ++i ) {
ellipses.push_back(conics[i].center);
}
// find camera pose given intrinsics
PosePnPRansac(
calibrator.GetCamera(iI).camera, ellipses, target->Circles3D(),
ellipse_target_map,
0, 0, &T_hw[iI]
);
if(calib_frame >= 0) {
if(iI==0 || !tracking_good[0]) {
// Initialize pose of frame for least squares optimisation
calibrator.GetFrame(calib_frame) = T_hw[iI];
}
for(size_t p=0; p < ellipses.size(); ++p) {
const Eigen::Vector2d pc = ellipses[p];
const Eigen::Vector2i pg = target->Map()[p].pg;
if( 0<= pg(0) && pg(0) < grid_size(0) && 0<= pg(1) && pg(1) < grid_size(1) )
{
const Eigen::Vector3d pg3d = grid_spacing * Eigen::Vector3d(pg(0), pg(1), 0);
// TODO: Add these correspondences in bulk to avoid
// hitting mutex each time.
calibrator.AddObservation(calib_frame, calib_cams[iI], pg3d, pc );
}
}
}
}
if(container[iI].IsShown()) {
container[iI].ActivateScissorAndClear();
glColor3f(1,1,1);
// Display camera image
if(!disp_thresh) {
tex.Upload(image_processing.Img(),GL_LUMINANCE,GL_UNSIGNED_BYTE);
tex.RenderToViewportFlipY();
}else{
tex.Upload(image_processing.ImgThresh(),GL_LUMINANCE,GL_UNSIGNED_BYTE);
tex.RenderToViewportFlipY();
}
// Setup orthographic pixel drawing
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(-0.5,w-0.5,h-0.5,-0.5,0,1.0);
glMatrixMode(GL_MODELVIEW);
if(disp_lines) {
for(std::list<LineGroup>::const_iterator i = target->LineGroups().begin(); i != target->LineGroups().end(); ++i)
{
glColor3f(0.5,0.5,0.5);
glBegin(GL_LINE_STRIP);
for(std::list<size_t>::const_iterator el = i->ops.begin(); el != i->ops.end(); ++el)
{
const Eigen::Vector2d p = conics[*el].center;
glVertex2d(p(0), p(1));
}
glEnd();
}
}
if(disp_cross) {
for( size_t i=0; i < conics.size(); ++i ) {
const Eigen::Vector2d pc = conics[i].center;
pangolin::glColorBin( target->Map()[i].value, 2);
pangolin::glDrawCross(pc, conics[i].bbox.Width()*0.75 );
}
}
if(disp_bbox) {
for( size_t i=0; i < conics.size(); ++i ) {
const Eigen::Vector2i pg = tracking_good[iI] ? target->Map()[i].pg : Eigen::Vector2i(0,0);
if( 0<= pg(0) && pg(0) < grid_size(0) && 0<= pg(1) && pg(1) < grid_size(1) ) {
pangolin::glColorBin(pg(1)*grid_size(0)+pg(0), grid_size(0)*grid_size(1));
glDrawRectPerimeter(conics[i].bbox);
}
}
}
}
}
if(v3D.IsShown()) {
v3D.ActivateScissorAndClear(stacks);
calibu::glDrawTarget(*target, Eigen::Vector2d(0,0), 1.0, 0.8, 1.0);
for(size_t c=0; c< calibrator.NumCameras(); ++c) {
const Eigen::Matrix3d Kinv = calibrator.GetCamera(c).camera.Kinv();
const CameraAndPose cap = calibrator.GetCamera(c);
const Sophus::SE3d T_ck = cap.T_ck;
// Draw keyframes
pangolin::glColorBin(c, 2, 0.2);
for(size_t k=0; k< calibrator.NumFrames(); ++k) {
pangolin::glDrawAxis((T_ck * calibrator.GetFrame(k)).inverse().matrix(), 0.01);
}
// Draw current camera
if(tracking_good[c]) {
pangolin::glColorBin(c, 2, 0.5);
pangolin::glDrawFrustrum(Kinv,w,h,T_hw[c].inverse().matrix(),0.05);
}
}
}
disp_mse = calibrator.MeanSquareError();
disp_frame = frame;
// Process window events via GLUT
pangolin::FinishFrame();
}
calibrator.Stop();
calibrator.PrintResults();
calibrator.WriteCameraModels(output_filename);
}
void GeometricConsistencyGrouping::recognize(
const sensor_msgs::PointCloud2::ConstPtr& scene_cloud_msg,
const sensor_msgs::PointCloud2::ConstPtr& scene_feature_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!reference_cloud_ || !reference_feature_) {
NODELET_ERROR_THROTTLE(1.0, "Not yet reference is available");
return;
}
pcl::PointCloud<pcl::SHOT352>::Ptr
scene_feature (new pcl::PointCloud<pcl::SHOT352>);
pcl::PointCloud<pcl::PointNormal>::Ptr
scene_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*scene_cloud_msg, *scene_cloud);
pcl::fromROSMsg(*scene_feature_msg, *scene_feature);
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<pcl::SHOT352> match_search;
match_search.setInputCloud (reference_feature_);
for (size_t i = 0; i < scene_feature->size(); ++i)
{
std::vector<int> neigh_indices(1);
std::vector<float> neigh_sqr_dists(1);
if (!pcl_isfinite (scene_feature->at(i).descriptor[0])) { //skipping NaNs
continue;
}
int found_neighs
= match_search.nearestKSearch(scene_feature->at(i), 1,
neigh_indices, neigh_sqr_dists);
// add match only if the squared descriptor distance is less than 0.25
// (SHOT descriptor distances are between 0 and 1 by design)
if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) {
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
}
}
pcl::GeometricConsistencyGrouping<pcl::PointNormal, pcl::PointNormal> gc_clusterer;
gc_clusterer.setGCSize(gc_size_);
gc_clusterer.setGCThreshold(gc_thresh_);
gc_clusterer.setInputCloud(reference_cloud_);
gc_clusterer.setSceneCloud(scene_cloud);
gc_clusterer.setModelSceneCorrespondences(model_scene_corrs);
//gc_clusterer.cluster (clustered_corrs);
std::vector<pcl::Correspondences> clustered_corrs;
std::vector<Eigen::Matrix4f,
Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
gc_clusterer.recognize(rototranslations, clustered_corrs);
if (rototranslations.size() > 0) {
NODELET_INFO("detected %lu objects", rototranslations.size());
Eigen::Matrix4f result_mat = rototranslations[0];
Eigen::Affine3f affine(result_mat);
geometry_msgs::PoseStamped ros_pose;
tf::poseEigenToMsg(affine, ros_pose.pose);
ros_pose.header = scene_cloud_msg->header;
pub_output_.publish(ros_pose);
}
else {
NODELET_WARN("Failed to find object");
}
}