Category: Algorithm

Use boost r-tree to find the nearest point

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/index/rtree.hpp>

using PointType = boost::geometry::model::point<float,3,boost::geometry::cs::cartesian>;

// Build the r-tree
boost::geometry::index::rtree< PointType,boost::geometry::index::quadratic<16> > rtree;
for(...) {
  rtree.insert( PointType( x, y, z ) );
}

// Find the nearest point
PointType roiPoint( x, y, z );
std::vector< PointType > nearestPoints;
rtree.query( boost::geometry::index::nearest( roiPoint, 1 ), std::back_inserter( nearestPoints ) );
const auto nearest = nearestPoints[0];
std::cout << nearest.get<0>() << ", "<< nearest.get<1>() << ", " << nearest.get<2>() << std::endl;

Check if a binary tree is a valid BST

Definition for a binary tree node:

struct TreeNode {
   int value;
   TreeNode *left;
   TreeNode *right;
   TreeNode( int x ) : value( x ), left( nullptr ), right( nullptr ) {}
}

A straightforward but incorrect solution:

bool validateBST( TreeNode *root ) {
   if( root == nullptr ) {
     return true;
   }
   
   if( root->left != nullptr && root->left->value > root->value ) {
     return false;
   }
   
   if( root->right != nullptr && root->right->value < root->value ) {
     return false;
   }
   
   if( !validateBST( root->left ) || !validateBST( root->right ) ) {
     return false;
   }
   
   return true;
}

Test it.

#include &lt;iostream>

int main (int argc, char** argv)
{
  {
    TreeNode *root    = new TreeNode( 3 ); 
    root->left        = new TreeNode( 2 ); 
    root->right       = new TreeNode( 5 ); 
    root->left->left  = new TreeNode( 1 ); 
    root->left->right = new TreeNode( 4 ); 

    std::cout&lt;&lt; validateBST( root ) &lt;&lt; std::endl;  // Expect 0
  }
  
  {
    TreeNode *root    = new TreeNode( 4 ); 
    root->left        = new TreeNode( 2 ); 
    root->right       = new TreeNode( 5 ); 
    root->left->left  = new TreeNode( 1 ); 
    root->left->right = new TreeNode( 3 ); 

    std::cout&lt;&lt; validateBST( root ) &lt;&lt; std::endl;  // Expect 1
  }

  return 0;
}

Build and run it.

g++ main.cpp --std=c++14
./a.out
1
1

A correct solution:

#include &lt;climits>

bool isValidBST( TreeNode *root, int min, int max ) {
  if( root == nullptr ) {
    return true;
  }
  
  if( root->value &lt; min || root->value > max ) {
    return false;
  }
  
  return isValidBST( root->left, min, root->value - 1 ) &amp;&amp; 
         isValidBST( root->right, root->value + 1, max );
}

bool validateBST( TreeNode *root ) {
  return isValidBST( root, INT_MIN, INT_MAX );
}

Use the same “main” to test it.

g++ main.cpp --std=c++14
./a.out
0
1

A better solution:

bool isValidBST( TreeNode *root, TreeNode *left, TreeNode *right ) {
  if( root == nullptr ) {
      return true;
  }

  if( left != nullptr &amp;&amp; left->value >= root->value ) {
      return false;
  }

  if( right != nullptr &amp;&amp; right->value &lt;= root->value ) {
      return false;
  }

  return isValidBST( root->left, left, root ) &amp;&amp; 
         isValidBST( root->right, root, right );
}

bool validateBST( TreeNode *root ) {
  return isValidBST( root, nullptr, nullptr );
}

Ceres Solver: A non-linear optimization library developed by Google

Ceres Solver is an open source C++ library for modeling and solving large, complicated optimization problems. Here is its tutorial about Non-linear Least Squares.

2,9,3 in the following codes represent the dimension of residual, the dimension of camera, the dimension of point.

// Factory to hide the construction of the CostFunction object from
// the client code.
static ceres::CostFunction* Create(const double observed_x,
                                   const double observed_y) {
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
                 new SnavelyReprojectionError(observed_x, observed_y)));
}

Their definitions can be found in the latest source code (autodiff_cost_function.h).

//   CostFunction* cost_function
//       = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
//            new MyScalarCostFunctor(1.0));             ^  ^  ^
//                                                       |  |  |
//                            Dimension of residual -----+  |  |
//                            Dimension of x ---------------+  |
//                            Dimension of y ------------------+

The latest code is using variadic template or template parameter pack which was introduced since C++11 to define the number of parameters.

template <typename CostFunctor,
          int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
          int... Ns>          // Number of parameters in each parameter block.
class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...>

Before that, in the version 1.14.0 released on March 24, 2018, it was using 10 parameters directly.

template <typename CostFunctor,
          int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
          int N0,       // Number of parameters in block 0.
          int N1 = 0,   // Number of parameters in block 1.
          int N2 = 0,   // Number of parameters in block 2.
          int N3 = 0,   // Number of parameters in block 3.
          int N4 = 0,   // Number of parameters in block 4.
          int N5 = 0,   // Number of parameters in block 5.
          int N6 = 0,   // Number of parameters in block 6.
          int N7 = 0,   // Number of parameters in block 7.
          int N8 = 0,   // Number of parameters in block 8.
          int N9 = 0>   // Number of parameters in block 9.
class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals,
                                                      N0, N1, N2, N3, N4,
                                                      N5, N6, N7, N8, N9> {

Here are their differences.

BFS (Breadth-First Search) and DFS (Depth-First Search)

Some essential definitions:

typedef char Element;

struct Node 
{
  Node( Element inputData ) : data( inputData ) { 
    cout << "Construct " << data << endl; 
  }
  
  ~Node() { 
    cout << "Destruct " << data << endl;
    
    if( lchild != NULL ) {
      delete lchild;
      lchild = NULL;
    }
    
    if( rchild != NULL ) {
      delete rchild;
      rchild = NULL;
    }
  }

  Element data;
  Node *lchild;
  Node *rchild;
};

typedef Node* Tree;

BFS: Breadth-First Search

// BFS
void breadthFirstSearch(Tree root)
{
  queue<Node *> nodeQueue;
  nodeQueue.push (root);
  Node *node;
  while (!nodeQueue.empty()) {
   node = nodeQueue.front();
   nodeQueue.pop ();
   cout << node->data;

   if (node->lchild) {
     nodeQueue.push(node->lchild);
   }

   if (node->rchild) {
     nodeQueue.push(node->rchild);
   }
  }
}

DFS: Depth-First Search

// DFS (iterative)
void depthFirstSearch(Tree root)
{
  stack<Node *> nodeStack;
  nodeStack.push (root);
  Node *node;
  while (!nodeStack.empty ()) {
   node = nodeStack.top();
   nodeStack.pop ();
   cout << node->data;

   if (node->rchild) {
     nodeStack.push(node->rchild);
   }

   if (node->lchild) {
     nodeStack.push(node->lchild);
   }
  }
}

// DFS (recursive)
void depthFirstSearchRecursive(Tree root)
{
  if (root) {
    cout << root->data;

    if (root->lchild) {
     depthFirstSearchRecursive(root->lchild);
    }

    if (root->rchild) {
     depthFirstSearchRecursive(root->rchild);
    }
  }
}

Test

Define a class TreeBuilder to build a tree and destroy it.

class TreeBuilder
{
public:
  TreeBuilder( Element data[] ) : index_( 0 ) {
   constructTree( root_, data );  
  }

  ~TreeBuilder() {
   destructTree( root_ );
  }

  Tree tree() const { return root_ ;}
 
private:
  void constructTree(Tree &root, Element data[])
  {
    Element e = data[index_++];
    if (e == '#') {
     root = NULL;
    }
    else {
     root = new Node (e);
     constructTree(root->lchild, data);
     constructTree(root->rchild, data);
    }
  }

  void destructTree(Tree root) {
    if( root != NULL ) {
      delete root;
      root = NULL;
    }
  }

private:
  int index_;
  Tree root_;
};

Test BFS and DFS.

#include <iostream>
#include <stack>
#include <queue>

using namespace std;

...

int main(int argc, char** argv) 
{
  Element data[15] = { 'A', 'B', 'D', '#', '#', 'E', '#', '#', 'C', 'F','#', '#', 'G', '#', '#' };	
  
  TreeBuilder builder( data );
  
  auto tree = builder.tree();

  cout << endl;

  cout << "BFS result：";
  breadthFirstSearch(tree);

  cout << endl;
  
  cout << "DFS (iterative) result：";
  depthFirstSearch(tree);

  cout << endl;

  cout << "DFS (recursive) result：";
  depthFirstSearchRecursive(tree);

  cout << endl;
  
  return 0;
}

Build and run it.

g++ main.cpp --std=c++14
./a.out
Construct A
Construct B
Construct D
Construct E
Construct C
Construct F
Construct G

BFS result：ABCDEFG
DFS (iterative) result：ABDECFG
DFS (recursive) result：ABDECFG
Destruct A
Destruct B
Destruct D
Destruct E
Destruct C
Destruct F
Destruct G

Use valgrind to check if there is any memory leak.

valgrind --leak-check=yes ./a.out
...
==30852== HEAP SUMMARY:
==30852==     in use at exit: 0 bytes in 0 blocks
==30852==   total heap usage: 12 allocs, 12 frees, 74,024 bytes allocated
==30852== 
==30852== All heap blocks were freed -- no leaks are possible
==30852== 
==30852== For counts of detected and suppressed errors, rerun with: -v
==30852== ERROR SUMMARY: 0 errors from 0 contexts (suppressed: 0 from 0)