Added estimation code to TrajectoryEstimator and started camera fast 3D point estimation