From 70f490b2980cd95719c0a1ce81f1f8fb81575f36 Mon Sep 17 00:00:00 2001
From: Nikhil Khedekar <nikhilvkhedekar@gmail.com>
Date: Thu, 17 Nov 2022 17:47:26 +0100
Subject: [PATCH] Exposed normalize for Point3_

---
 gtsam/slam/expressions.h | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h
index 3b8ea86d3..24476cb5e 100644
--- a/gtsam/slam/expressions.h
+++ b/gtsam/slam/expressions.h
@@ -48,6 +48,11 @@ inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) {
   return Line3_(f, wTc, wL);
 }
 
+inline Point3_ normalize(const Point3_& a){
+  Point3 (*f)(const Point3 &, OptionalJacobian<3, 3>) = &normalize;
+  return Point3_(f, a);
+}
+
 inline Point3_ cross(const Point3_& a, const Point3_& b) {
   Point3 (*f)(const Point3 &, const Point3 &,
              OptionalJacobian<3, 3>, OptionalJacobian<3, 3>) = &cross;