Added few utility functions required to check if Marker is facing the Camera

cpp/Point3.cpp

@@ -71,6 +71,16 @@ Point3 cross(const Point3 &p, const Point3 &q)
 		 p.x_*q.y_ - p.y_*q.x_ );
 }
 /* ************************************************************************* */
+double dot(const Point3 &p, const Point3 &q)
+{
+  return ( p.x_*q.x_ + p.y_*q.y_ + p.z_*q.z_ );
+}
+/* ************************************************************************* */
+double norm(const Point3 &p)
+{
+  return sqrt( p.x_*p.x_ + p.y_*p.y_ + p.z_*p.z_ );
+}
+/* ************************************************************************* */
 bool assert_equal(const Point3& p, const Point3& q, double tol) {
   if(p.equals(q,tol)) return true;
   printf("not equal:\n");

cpp/Point3.h

@@ -68,6 +68,8 @@ namespace gtsam {
 
     /** friends */
     friend Point3 cross(const Point3 &p1, const Point3 &p2);
+    friend double dot(const Point3 &p1, const Point3 &p2);
+    friend double norm(const Point3 &p1);
 
   private:
     /** Serialization function */
@@ -96,6 +98,12 @@ namespace gtsam {
   /** cross product */
   Point3 cross(const Point3 &p, const Point3 &q); 
 
+  /** dot product */
+  double dot(const Point3 &p, const Point3 &q);
+
+  /** dot product */
+  double norm(const Point3 &p);
+
   /** equals with an tolerance, prints out message if unequal */
   bool assert_equal(const Point3& p, const Point3& q, double tol = 1e-9);
 }

cpp/Rot3.h

@@ -86,6 +86,16 @@ namespace gtsam {
       return Matrix_(3,3, r);  
     }
 
+    /** returns column vector specified by index */
+    Point3 column(int index) const{
+    	if(index == 3)
+    		return r3_;
+    	else if (index == 2)
+    		return r2_;
+    	else
+    		return r1_; // default returns r1
+    }
+
     /** inverse transformation  */
     Rot3 inverse() const { return transpose();}
 		

cpp/testPoint3.cpp

@@ -24,6 +24,13 @@ TEST( Point3, equals)
   CHECK(!P.equals(Q));
 }
 
+/* ************************************************************************* */
+TEST( Point3, dot)
+{
+	CHECK(dot(Point3(0,0,0),Point3(1,1,0)) == 0);
+	CHECK(dot(Point3(1,1,1),Point3(1,1,0)) == 2);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */