diff --git a/include/opendrive_geometry.h b/include/opendrive_geometry.h
index dc264cfd483e15481f1caf3167bc8acdb1e004c4..f5a3caa736263fb086f58b740240e224bd06d751 100644
--- a/include/opendrive_geometry.h
+++ b/include/opendrive_geometry.h
@@ -25,6 +25,9 @@ class COpendriveGeometry
     virtual void load_params(const planView::geometry_type &geometry_info) = 0;
     virtual std::string get_name(void)=0;
     void set_scale_factor(double scale);
+    void set_start_point(TOpendriveWorldPoint &point);
+    void set_max_s(double s);
+    void set_min_s(double s);
   public:
     virtual COpendriveGeometry *clone(void) = 0;
     bool get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
@@ -32,6 +35,9 @@ class COpendriveGeometry
     bool in_range(double s) const;
     double get_length(void) const;
     virtual void get_curvature(double &start,double &end)=0;
+    double get_max_s(void) const;
+    double get_min_s(void) const;
+    TOpendriveWorldPoint get_start_point(void) const;
     void operator=(const COpendriveGeometry &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom);
     virtual ~COpendriveGeometry();
diff --git a/include/opendrive_lane.h b/include/opendrive_lane.h
index 988dc2338c62e28bbdd180480eb5f38f78396ca4..4aa9964a5f7794e1b32782d8f4c2363028b91de2 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -34,15 +34,18 @@ class COpendriveLane
   protected:
     COpendriveLane();
     COpendriveLane(const COpendriveLane &object);
-    void load(const right::lane_type &lane_info,COpendriveRoadSegment *segment);
+    void load(const ::lane &lane_info,COpendriveRoadSegment *segment);
     void link_neightbour_lane(COpendriveLane *lane);
     void link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start,bool to_start);
     void add_next_lane(COpendriveLane *lane);
     void add_prev_lane(COpendriveLane *lane);
+    void remove_lane(COpendriveLane *lane);
     void add_node(COpendriveRoadNode *node);
     bool has_node(COpendriveRoadNode *node);
-    bool has_node(unsigned int index);
+    bool has_node_with_index(unsigned int index);
     void set_parent_segment(COpendriveRoadSegment *segment);
+    void set_left_lane(COpendriveLane *lane,opendrive_mark_t mark);
+    void set_right_lane(COpendriveLane *lane,opendrive_mark_t mark);
     void set_resolution(double res);
     void set_scale_factor(double scale);
     void set_width(double width);
@@ -75,7 +78,7 @@ class COpendriveLane
     double get_length(void) const;
     TOpendriveLocalPoint transform_to_local_point(TOpendriveTrackPoint &track);
     TOpendriveWorldPoint transform_to_world_point(TOpendriveTrackPoint &track);
-    unsigned int get_closest_node(TOpendriveWorldPoint &point,double angle_tol=0.1);
+    unsigned int get_closest_node_index(TOpendriveWorldPoint &point,double angle_tol=0.1);
     friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
     ~COpendriveLane();
 };
diff --git a/include/opendrive_link.h b/include/opendrive_link.h
index 0ea31cc51ddeab6aa6ec858c09297a8376ff9430..25c291696468fe8142a87b3428efc65b5a45a60f 100644
--- a/include/opendrive_link.h
+++ b/include/opendrive_link.h
@@ -46,9 +46,7 @@ class COpendriveLink
     const COpendriveLane &get_parent_lane(void) const;
     double get_resolution(void) const;
     double get_scale_factor(void) const;
-    double find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point);
-    double get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world);
-    double get_track_point_local(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
+    double find_closest_world_point(TOpendriveWorldPoint &world,TOpendriveWorldPoint &point);
     void get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length=0.0, double end_length=-1.0) const;
     void get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &yaw,double start_length=0.0, double end_length=-1.0) const;
     double get_length(void) const;
diff --git a/include/opendrive_road.h b/include/opendrive_road.h
index 763ef4a5d7f8d944f019556f9e22e8156adbf1b1..4a75fa55d21647b99d75a0b2b55530e81c5a9f02 100644
--- a/include/opendrive_road.h
+++ b/include/opendrive_road.h
@@ -23,13 +23,13 @@ class COpendriveRoad
     double scale_factor;
     double min_road_length;
   protected:
-    COpendriveRoadSegment &operator[](std::string &key);
+    COpendriveRoadSegment *get_segment_by_id(std::string &id);
     void free(void);
     void link_segments(OpenDRIVE &open_drive);
     unsigned int add_node(COpendriveRoadNode *node);
-    void remove_node(COpendriveRoadNode *node);
+    bool remove_node(COpendriveRoadNode *node);
     unsigned int add_lane(COpendriveLane *lane);
-    void remove_lane(COpendriveLane *lane);
+    bool remove_lane(COpendriveLane *lane);
     void complete_open_lanes(void);
     void add_segment(COpendriveRoadSegment *segment,std::vector<unsigned int> &old_path,std::vector<unsigned int> &new_path);
     bool has_segment(COpendriveRoadSegment *segment);
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index 6e919e2b163f7a8c93def8a4477bd3c5bc3c37e8..e0ca5f91ed103468c57f97f10a685d729e5fb7bb 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -7,6 +7,13 @@
 #include "opendrive_lane.h"
 #include "opendrive_road_segment.h"
 
+typedef struct
+{
+  COpendriveLane * lane;
+  COpendriveGeometry *geometry;
+  COpendriveRoadSegment *segment;
+}TOpendriveRoadNodeParent;
+
 class COpendriveRoadNode
 {
   friend class COpendriveLane;
@@ -17,44 +24,46 @@ class COpendriveRoadNode
     std::vector<COpendriveLink *> links;
     double resolution;
     double scale_factor;
-    TOpendriveWorldPoint start_point;
-    std::vector<COpendriveLane *> lanes;
-    std::vector<COpendriveGeometry *> geometries;
-    std::vector<COpendriveRoadSegment *>parents;
+    TOpendriveWorldPoint point;
+    std::vector<TOpendriveRoadNodeParent> parents;
     unsigned int index;
   protected:
     COpendriveRoadNode();
     COpendriveRoadNode(const COpendriveRoadNode &object);
-    void load(const planView::geometry_type &geom_info,COpendriveLane *lane);
-    void add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
+    void free(void);
+    void load(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment);
+    bool add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
     void add_link(COpendriveLink *link);
     void remove_link(COpendriveLink *link);
+    bool has_link(COpendriveLink *link);
+    bool has_link_with(COpendriveRoadNode *node);
+    COpendriveLink *get_link_with(COpendriveRoadNode *node);
     void set_resolution(double res);
     void set_scale_factor(double scale);
     void set_index(unsigned int index);
-    void set_start_point(TOpendriveWorldPoint &start);
-    void add_parent_segment(COpendriveRoadSegment *segment);
-    void add_lane(const planView::geometry_type &geom_info,COpendriveLane *lane);
+    void set_point(TOpendriveWorldPoint &start);
+    void add_parent(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment);
+    void add_parent(COpendriveGeometry *geometry,COpendriveLane *lane,COpendriveRoadSegment *segment);
+    TOpendriveRoadNodeParent *get_parent_with_lane(const COpendriveLane *lane);
+    TOpendriveRoadNodeParent *get_parent_with_segment(const COpendriveRoadSegment *segment);
     bool updated(node_up_ref_t &refs);
+    COpendriveRoadNode *get_original_node(node_up_ref_t &node_refs);
     void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
     void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
     void update_start_pose(COpendriveLane *lane,TOpendriveWorldPoint *start=NULL);
     void update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end=NULL);
-    COpendriveLink *get_link_with(COpendriveRoadNode *end);
   public:
     double get_resolution(void) const;
     double get_scale_factor(void) const;
     unsigned int get_index(void) const;
-    unsigned int get_num_parent_segments(void) const;
+    unsigned int get_num_parents(void) const;
     const COpendriveRoadSegment& get_parent_segment(unsigned int index) const;
-    TOpendriveWorldPoint get_start_pose(void) const;
+    const COpendriveLane &get_parent_lane(unsigned int index) const;
+    const COpendriveGeometry &get_geometry(unsigned int index) const;
+    TOpendriveWorldPoint get_point(void) const;
     unsigned int get_num_links(void) const;
     const COpendriveLink &get_link(unsigned int index) const;
     const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
-    unsigned int get_num_lanes(void) const; 
-    const COpendriveLane &get_lane(unsigned int index) const;
-    unsigned int get_num_geometries(void) const;
-    const COpendriveGeometry &get_geometry(unsigned int index) const;
     unsigned int get_closest_link(TOpendriveWorldPoint &point,double angle_tol=0.1) const;
     double get_closest_distance(TOpendriveWorldPoint &point,double angle_tol=0.1) const;
     double get_closest_point(TOpendriveWorldPoint &point,TOpendriveWorldPoint &closest_point,double angle_tol=0.1) const;
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index cb8f8beaab8617fa8c331b476ff6f8f00dbc0315..d06dbccd44971c64be715c96ee09ccef06d4ffe1 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -49,15 +49,18 @@ class COpendriveRoadSegment
     void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
     void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
     void add_lanes(lanes::laneSection_type &lane_section);
+    void add_lane(const ::lane &lane_info);
     void remove_lane(COpendriveLane *lane);
     void add_nodes(OpenDRIVE::road_type &road_info);
-    void add_node(COpendriveRoadNode *node);
+    void add_node(const planView::geometry_type &geom_info,COpendriveLane *lane);
+    void add_empty_node(TOpendriveWorldPoint &point,COpendriveLane *lane);
     void remove_node(COpendriveRoadNode *node);
     bool has_node(COpendriveRoadNode *node);
     int get_node_side(COpendriveRoadNode *node);
     void link_neightbour_lanes(lanes::laneSection_type &lane_section);
-    void link_segment(COpendriveRoadSegment &segment);
-    void link_segment(COpendriveRoadSegment &segment,int from,bool from_start, int to,bool to_start);
+    void link_neightbour_lanes(void);
+    void link_segment(COpendriveRoadSegment *segment);
+    void link_segment(COpendriveRoadSegment *segment,int from,bool from_start, int to,bool to_start);
     bool connects_to(COpendriveRoadSegment *segment);
     bool connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2);
     COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPoint *start=NULL,TOpendriveWorldPoint *end=NULL);
diff --git a/src/opendrive_geometry.cpp b/src/opendrive_geometry.cpp
index 84ad25bab5831bf44a84f293dbc5887235a6b07e..a22bda53052a424f5efbc9d7174f8d8d8cd67902 100644
--- a/src/opendrive_geometry.cpp
+++ b/src/opendrive_geometry.cpp
@@ -23,9 +23,11 @@ COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object)
 
 void COpendriveGeometry::print(std::ostream& out)
 {
+  TOpendriveWorldPoint tmp_point=this->get_start_point();
+
   out << "        Geometry " << this->get_name() << std::endl; 
   out << "          lenght: " << this->get_length() << std::endl;
-  out << "          pose: x = " << this->pose.x/this->scale_factor << ", y = " << this->pose.y/this->scale_factor << ", heading = " << this->pose.heading << std::endl;
+  out << "          pose: x = " << tmp_point.x << ", y = " << tmp_point.y << ", heading = " << tmp_point.heading << std::endl;
 }
 
 void COpendriveGeometry::load(const planView::geometry_type &geometry_info)
@@ -43,6 +45,23 @@ void COpendriveGeometry::set_scale_factor(double scale)
   this->scale_factor=scale;
 }
 
+void COpendriveGeometry::set_start_point(TOpendriveWorldPoint &point)
+{
+  this->pose.x=point.x*this->scale_factor;
+  this->pose.y=point.y*this->scale_factor;
+  this->pose.heading=point.heading;
+}
+
+void COpendriveGeometry::set_max_s(double s)
+{
+  this->max_s=s*this->scale_factor;
+}
+
+void COpendriveGeometry::set_min_s(double s)
+{
+  this->min_s=s*this->scale_factor;
+}
+
 bool COpendriveGeometry::get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const
 {
   return this->transform_local_pose(track,local);
@@ -76,6 +95,27 @@ double COpendriveGeometry::get_length(void) const
   return (this->max_s-this->min_s)/this->scale_factor;
 }
 
+double COpendriveGeometry::get_max_s(void) const
+{
+  return this->max_s/this->scale_factor;
+}
+
+double COpendriveGeometry::get_min_s(void) const
+{
+  return this->min_s/this->scale_factor;
+}
+
+TOpendriveWorldPoint COpendriveGeometry::get_start_point(void) const
+{
+  TOpendriveWorldPoint tmp_point;
+  
+  tmp_point.x=this->pose.x/this->scale_factor;
+  tmp_point.y=this->pose.y/this->scale_factor;
+  tmp_point.heading=this->pose.heading;
+
+  return tmp_point;
+}
+
 void COpendriveGeometry::operator=(const COpendriveGeometry &object)
 {
   this->min_s = object.min_s;
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index abbb4616d574767d83b56ee13f8f859008880695..f93c25d3090bcbb73f41d26b5d7d3b2aa3220fba 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -42,29 +42,35 @@ COpendriveLane::COpendriveLane(const COpendriveLane &object)
 
 void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
 {
+  unsigned int min_num_nodes;
+
   if(lane!=NULL)
   {
-    if(this->get_num_nodes()!=lane->get_num_nodes())
-      return;
-    if(this->get_num_nodes()==0 || lane->get_num_nodes()==0)
-      return;
-    for(unsigned int i=0;i<this->get_num_nodes()-1;i++)
+    if(this->get_num_nodes()<lane->get_num_nodes())
+      min_num_nodes=this->get_num_nodes();
+    else
+      min_num_nodes=lane->get_num_nodes();
+    for(unsigned int i=0;i<min_num_nodes-1;i++)
     {
       this->nodes[i]->add_link(lane->nodes[i+1],this->left_mark,this->segment,this);
-      lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark,this->segment,this);
+      lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark,this->segment,lane);
       if(this->id>0)// left lanes
         this->right_mark=lane->left_mark;
       else// right lanes
         this->left_mark=lane->right_mark;
     } 
-    this->left_lane=lane;
-    lane->right_lane=this;
+    if(min_num_nodes>0)
+    {
+      this->left_lane=lane;
+      lane->right_lane=this;
+    }
   }
 }
 
 void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start, bool to_start)
 {
   COpendriveRoadNode *start,*end;
+  std::stringstream error;
 
   if(lane!=NULL)
   {
@@ -118,7 +124,10 @@ void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool f
       lane->add_prev_lane(this);
     }
     else 
-      throw CException(_HERE_,"One of the lanes to link has no nodes");
+    {
+      error << "Lane " << this->id << " of segment " << this->segment->get_name() << " or lane " << lane->get_id() << " of segment " << lane->get_segment().get_name() << " has no nodes";
+      throw CException(_HERE_,error.str());
+    }
   }
 }
 
@@ -142,6 +151,33 @@ void COpendriveLane::add_prev_lane(COpendriveLane *lane)
   this->prev.push_back(lane);
 }
 
+void COpendriveLane::remove_lane(COpendriveLane *lane)
+{
+  std::vector<COpendriveLane *>::iterator lane_it;
+
+  // remove the reference from neightbour lanes
+  if(this->right_lane==lane)
+    this->right_lane=NULL;
+  if(this->left_lane==lane)
+    this->left_lane=NULL;
+  // remove the reference from next lanes
+  for(lane_it=this->next.begin();lane_it!=this->next.end();)
+  {
+    if((*lane_it)==lane)
+      lane_it=this->next.erase(lane_it);
+     else
+      lane_it++;
+  }
+  // remove the reference from next lanes
+  for(lane_it=this->prev.begin();lane_it!=this->prev.end();)
+  {
+    if((*lane_it)==lane)
+      lane_it=this->prev.erase(lane_it);
+     else
+      lane_it++;
+  }
+}
+
 void COpendriveLane::add_node(COpendriveRoadNode *node)
 {
   if(this->has_node(node))
@@ -162,7 +198,7 @@ bool COpendriveLane::has_node(COpendriveRoadNode *node)
   return false;
 }
 
-bool COpendriveLane::has_node(unsigned int index)
+bool COpendriveLane::has_node_with_index(unsigned int index)
 {
   for(unsigned int i=0;i<this->nodes.size();i++)
     if(this->nodes[i]->get_index()==index)
@@ -176,6 +212,18 @@ void COpendriveLane::set_parent_segment(COpendriveRoadSegment *segment)
   this->segment=segment;
 }
 
+void COpendriveLane::set_left_lane(COpendriveLane *lane,opendrive_mark_t mark)
+{
+  this->left_lane=lane;
+  this->left_mark=mark;
+}
+
+void COpendriveLane::set_right_lane(COpendriveLane *lane,opendrive_mark_t mark)
+{
+  this->right_lane=lane;
+  this->right_mark=mark;
+}
+
 void COpendriveLane::set_resolution(double res)
 {
   this->resolution=res;
@@ -269,13 +317,7 @@ void COpendriveLane::clean_references(segment_up_ref_t &segment_refs,lane_up_ref
   {
     for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
     {
-      if(node_refs.find(*node_it)!=node_refs.end())
-      {
-        (*node_it)=node_refs[*node_it];
-        (*node_it)->clean_references(segment_refs,lane_refs,node_refs);
-        node_it++;
-      }
-      else if((*node_it)->updated(node_refs))
+      if((*node_it)->updated(node_refs))
       {
         (*node_it)->clean_references(segment_refs,lane_refs,node_refs);
         node_it++;
@@ -285,24 +327,14 @@ void COpendriveLane::clean_references(segment_up_ref_t &segment_refs,lane_up_ref
     }
     for(lane_it=this->next.begin();lane_it!=this->next.end();)
     {
-      if(lane_refs.find(*lane_it)!=lane_refs.end())
-      {
-        (*lane_it)=lane_refs[*lane_it];
-        lane_it++;
-      }
-      else if(!(*lane_it)->updated(lane_refs))
+      if(!(*lane_it)->updated(lane_refs))
         lane_it=this->next.erase(lane_it);
       else
         lane_it++;
     }
     for(lane_it=this->prev.begin();lane_it!=this->prev.end();)
     {
-      if(lane_refs.find(*lane_it)!=lane_refs.end())
-      {
-        (*lane_it)=lane_refs[*lane_it];
-        lane_it++;
-      }
-      else if(!(*lane_it)->updated(lane_refs))
+      if(!(*lane_it)->updated(lane_refs))
         lane_it=this->prev.erase(lane_it);
       else
         lane_it++;
@@ -322,39 +354,33 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t
 {
   std::vector<COpendriveRoadNode *>::iterator it;
   segment_up_ref_t new_segment_ref;
-  node_up_ref_t::iterator exist_it;
   unsigned int start_node_index,i;
   COpendriveRoadNode *new_node;
-  bool exists;
+  std::stringstream error;
   
   if(start==NULL)
     return;
-  start_node_index=this->get_closest_node(*start,3.14159);
+  start_node_index=this->get_closest_node_index(*start,3.14159);
+  if(start_node_index==(unsigned int)-1)
+  {
+    error << "No node close to (x=" << start->x << ",y=" << start->y << ",heading=" << start->heading << ") in lane " << this->id << " of segment " << this->segment->get_name();
+    throw CException(_HERE_,error.str());
+  }
   // eliminate all the node before the start one
   for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
   {
     if(i<start_node_index)
     {
-      for(exist_it=new_node_ref.begin();exist_it!=new_node_ref.end();exist_it++)
-        if((*it)==exist_it->second)
-        {
-          delete *it;
-          new_node_ref.erase(exist_it);
-          break;
-        }
+      if((*it)->updated(new_node_ref))
+      {
+        new_node_ref.erase((*it)->get_original_node(new_node_ref));
+        delete *it;
+      }
       it=this->nodes.erase(it);
     }
     else
     {
-      exists=false;
-      // avoid creating the node for a second time
-      for(exist_it=new_node_ref.begin();exist_it!=new_node_ref.end();exist_it++)
-        if((*it)==exist_it->second)
-        {
-          exists=true;
-          break;
-        } 
-      if(!exists)
+      if(!(*it)->updated(new_node_ref))
       {
         new_node=new COpendriveRoadNode(**it);
         new_node_ref[*it]=new_node;
@@ -371,39 +397,33 @@ void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &
 {
   std::vector<COpendriveRoadNode *>::iterator it;
   segment_up_ref_t new_segment_ref;
-  node_up_ref_t::iterator exist_it;
   unsigned int end_node_index,i;
   COpendriveRoadNode *new_node;
-  bool exists;
+  std::stringstream error;
     
   if(end==NULL)
     return;
-  end_node_index=this->get_closest_node(*end,3.14159);
+  end_node_index=this->get_closest_node_index(*end,3.14159);
+  if(end_node_index==(unsigned int)-1)
+  {
+    error << "No node close to (x=" << end->x << ",y=" << end->y << ",heading=" << end->heading << ") in lane " << this->id << " of segment " << this->segment->get_name();
+    throw CException(_HERE_,error.str());
+  }
   // eliminate all the node before the start one
   for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
   {
     if(i>end_node_index)
     {
-      for(exist_it=new_node_ref.begin();exist_it!=new_node_ref.end();exist_it++)
-        if((*it)==exist_it->second)
-        {
-          delete *it;
-          new_node_ref.erase(exist_it);
-          break;
-        }
+      if((*it)->updated(new_node_ref))
+      {
+        new_node_ref.erase((*it)->get_original_node(new_node_ref));
+        delete *it;
+      }
       it=this->nodes.erase(it);
     }
     else
     {
-      exists=false;
-      // avoid creating the node for a second time
-      for(exist_it=new_node_ref.begin();exist_it!=new_node_ref.end();exist_it++)
-        if((*it)==exist_it->second)
-        {
-          exists=true;
-          break;
-        } 
-      if(!exists)
+      if(!(*it)->updated(new_node_ref))
       {
         new_node=new COpendriveRoadNode(**it);
         new_node_ref[*it]=new_node;
@@ -457,33 +477,40 @@ COpendriveLane *COpendriveLane::clone(node_up_ref_t &new_node_ref,lane_up_ref_t
   return new_lane;
 }
 
-void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegment *segment)
+void COpendriveLane::load(const ::lane &lane_info,COpendriveRoadSegment *segment)
 {
   std::stringstream error;
   opendrive_mark_t mark;
 
-  for(unsigned int i=0;i<this->nodes.size();i++)
-  {
-    delete this->nodes[i];
-    this->nodes[i]=NULL;
-  }
   this->nodes.clear();
+  this->prev.clear();
+  this->next.clear();
   this->left_lane=NULL;
   this->left_mark=OD_MARK_NONE;
   this->right_lane=NULL;
   this->right_mark=OD_MARK_NONE;
   this->set_id(lane_info.id().get());
   // import lane width
-  if(lane_info.width().size()!=1)
+  if(lane_info.width().size()<1)
+  {
+    error << "Lane " << this->id << " of segment " << segment->get_name() << " has no width record";
+    throw CException(_HERE_,error.str());
+  }
+  else if(lane_info.width().size()>1)
   {
-    error << "Only one width record supported for lane " << this->id;
+    error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one width record";
     throw CException(_HERE_,error.str());
   }
   this->set_width(lane_info.width().begin()->a().get());
   // import lane speed
-  if(lane_info.speed().size()!=1)
+  if(lane_info.speed().size()<1)
   {
-    error << "Only one speed record supported for lane " << this->id;
+    error << "Lane " << this->id << " of segment " << segment->get_name() << " has no speed record";
+    throw CException(_HERE_,error.str());
+  }
+  else if(lane_info.speed().size()>1)
+  {
+    error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one speed record";
     throw CException(_HERE_,error.str());
   }
   this->set_speed(lane_info.speed().begin()->max().get());
@@ -492,7 +519,7 @@ void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegmen
     mark=OD_MARK_NONE;
   else if(lane_info.roadMark().size()>1)
   {
-    error << "Only one road mark supported for lane " << this->id;
+    error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one road mark record";
     throw CException(_HERE_,error.str());
   }
   else if(lane_info.roadMark().size()==1)
@@ -534,10 +561,15 @@ unsigned int COpendriveLane::get_num_nodes(void) const
 
 const COpendriveRoadNode &COpendriveLane::get_node(unsigned int index) const
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->nodes.size())
     return *this->nodes[index];
   else
-    throw CException(_HERE_,"Invalid node index");
+  {
+    error << "Invalid node index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
+    throw CException(_HERE_,error.str());
+  }
 }
 
 unsigned int COpendriveLane::get_num_next_lanes(void) const
@@ -547,10 +579,15 @@ unsigned int COpendriveLane::get_num_next_lanes(void) const
 
 const COpendriveLane &COpendriveLane::get_next_lane(unsigned int index) const
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->next.size())
     return *this->next[index];
   else
-    throw CException(_HERE_,"Invalid next lane index");
+  {
+    error << "Invalid next lane index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
+    throw CException(_HERE_,error.str());
+  }
 }
 
 unsigned int COpendriveLane::get_num_prev_lanes(void) const
@@ -560,10 +597,15 @@ unsigned int COpendriveLane::get_num_prev_lanes(void) const
 
 const COpendriveLane &COpendriveLane::get_prev_lane(unsigned int index) const
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->prev.size())
     return *this->prev[index];
   else
-    throw CException(_HERE_,"Invalid previous lane index");
+  {
+    error << "Invalid previous lane index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
+    throw CException(_HERE_,error.str());
+  }
 }
 
 const COpendriveRoadSegment &COpendriveLane::get_segment(void) const
@@ -600,25 +642,39 @@ TOpendriveWorldPoint COpendriveLane::get_start_point(void) const
 {
   TOpendriveTrackPoint track_point;
   TOpendriveWorldPoint world_point;
+  TOpendriveRoadNodeParent *parent;
+  std::stringstream error;
 
   track_point.heading=0.0;
   if(this->id<0)// right lane
   {
     track_point.t=this->get_offset()-this->get_width()/2.0;
     track_point.s=0.0;
-    for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
-      if(&this->nodes[0]->get_lane(i)==this)
-        this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
+    parent=this->nodes[0]->get_parent_with_lane(this);
+    if(parent!=NULL)
+      parent->geometry->get_world_pose(track_point,world_point);
+    else
+    {
+      error << "Node " << this->nodes[0]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
+      throw CException(_HERE_,error.str());
+    }
   }
   else
   {
     track_point.t=this->get_offset()+this->get_width()/2.0;
-    track_point.s=0.0;
-    for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
-      if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
-        this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
+    parent=this->nodes[0]->get_parent_with_lane(this);
+    if(parent!=NULL)
+    {
+      track_point.s=parent->geometry->get_length();
+      parent->geometry->get_world_pose(track_point,world_point);
+    }
+    else
+    {
+      error << "Node " << this->nodes[0]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
+      throw CException(_HERE_,error.str());
+    }
   }
-  if(this->get_id()>0)
+  if(this->id>0)
     world_point.heading=normalize_angle(world_point.heading+3.14159);
 
   return world_point;
@@ -628,29 +684,39 @@ TOpendriveWorldPoint COpendriveLane::get_end_point(void) const
 {
   TOpendriveTrackPoint track_point;
   TOpendriveWorldPoint world_point;
+  TOpendriveRoadNodeParent *parent;
+  std::stringstream error;
   
   track_point.heading=0.0;
   if(this->id>0)// left lane
   { 
     track_point.t=this->get_offset()+this->get_width()/2.0;
-    for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
-      if(&this->nodes[0]->get_lane(i)==this)
-      {
-        track_point.s=this->nodes[0]->get_geometry(i).get_length();;
-        this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
-      }
+    track_point.s=0.0;
+    parent=this->nodes[this->nodes.size()-1]->get_parent_with_lane(this);
+    if(parent!=NULL)
+      parent->geometry->get_world_pose(track_point,world_point);
+    else
+    {
+      error << "Node " << this->nodes[this->nodes.size()-1]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
+      throw CException(_HERE_,error.str());
+    }
   }
   else
   { 
     track_point.t=this->get_offset()-this->get_width()/2.0;
-    for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
-      if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
-      {
-        track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
-        this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
-      }
+    parent=this->nodes[this->nodes.size()-1]->get_parent_with_lane(this);
+    if(parent!=NULL)
+    {
+      track_point.s=parent->geometry->get_length();
+      parent->geometry->get_world_pose(track_point,world_point);
+    }
+    else
+    {
+      error << "Node " << this->nodes[this->nodes.size()-1]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
+      throw CException(_HERE_,error.str());
+    }
   }
-  if(this->get_id()>0)
+  if(this->id>0)
     world_point.heading=normalize_angle(world_point.heading+3.14159);
 
   return world_point;
@@ -658,14 +724,19 @@ TOpendriveWorldPoint COpendriveLane::get_end_point(void) const
 
 double COpendriveLane::get_length(void) const
 {
+  TOpendriveRoadNodeParent *parent;
+  std::stringstream error;
   double length=0.0;
 
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
-    for(unsigned int j=0;j<this->nodes[i]->get_num_lanes();j++)
+    parent=this->nodes[i]->get_parent_with_lane(this);
+    if(parent!=NULL)
+      length+=parent->geometry->get_length();
+    else
     {
-      if(this->nodes[i]->get_lane(j).get_id()==this->id)
-        length+=this->nodes[i]->get_geometry(j).get_length();
+      error << "Node " << this->nodes[i]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
+      throw CException(_HERE_,error.str());
     }
   }
 
@@ -675,62 +746,86 @@ double COpendriveLane::get_length(void) const
 TOpendriveLocalPoint COpendriveLane::transform_to_local_point(TOpendriveTrackPoint &track)
 {
   TOpendriveLocalPoint local;
+  TOpendriveRoadNodeParent *parent;
+  std::stringstream error;
 
   if(track.s<0.0)
-    throw CException(_HERE_,"Invalid track point");
+  {
+    error << "Invalid track point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")";
+    throw CException(_HERE_,error.str());
+  }
   for(unsigned int i=0;i<this->nodes.size();i++)
   { 
-    for(unsigned int j=0;j<this->nodes[i]->get_num_lanes();j++)
-    { 
-      if(this->nodes[i]->get_lane(j).get_id()==this->id)
+    parent=this->nodes[i]->get_parent_with_lane(this);
+    if(parent!=NULL)
+    {
+      if(track.s<=parent->geometry->get_length())
       { 
-        if(track.s<=this->nodes[i]->get_geometry(j).get_length())
-        { 
-          if(!this->nodes[i]->get_geometry(j).get_local_pose(track,local))
-            throw CException(_HERE_,"Impossible to transform to local coordinates");
-          return local;
+        if(!parent->geometry->get_local_pose(track,local))
+        {
+          error << "Impossible to transform to local coordinates point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in lane " << parent->lane->get_id() << " of segment " << parent->segment->get_name();
+          throw CException(_HERE_,error.str());
         }
-        else
-          track.s-=this->nodes[i]->get_geometry(j).get_length();
+        return local;
       }
+      else
+        track.s-=parent->geometry->get_length();
+    }
+    else
+    {
+      error << "Node " << this->nodes[i]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
+      throw CException(_HERE_,error.str());
     }
   }
 
-  throw CException(_HERE_,"Track point does not belong to lane");
+  error << "Track point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to lane  " << this->id << " of segment " << this->segment->get_name();
+  throw CException(_HERE_,error.str());
 }
 
 TOpendriveWorldPoint COpendriveLane::transform_to_world_point(TOpendriveTrackPoint &track)
 { 
   TOpendriveWorldPoint world;
+  TOpendriveRoadNodeParent *parent;
+  std::stringstream error;
 
   if(track.s<0.0)
-    throw CException(_HERE_,"Invalid track point");
+  {
+    error << "Invalid track point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")";
+    throw CException(_HERE_,error.str());
+  }
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
-    for(unsigned int j=0;j<this->nodes[i]->get_num_lanes();j++)
+    parent=this->nodes[i]->get_parent_with_lane(this);
+    if(parent!=NULL)
     {
-      if(this->nodes[i]->get_lane(j).get_id()==this->id)
+      if(track.s<=parent->geometry->get_length())
       {
-        if(track.s<=this->nodes[i]->get_geometry(j).get_length())
+        if(!parent->geometry->get_world_pose(track,world))
         {
-          if(!this->nodes[i]->get_geometry(j).get_world_pose(track,world))
-            throw CException(_HERE_,"Impossible to transform to world coordinates");
-          return world;
+          error << "Impossible to transform to local coordinates point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") in lane " << parent->lane->get_id() << " of segment " << parent->segment->get_name();
+          throw CException(_HERE_,error.str());
         }
-        else
-          track.s-=this->nodes[i]->get_geometry(j).get_length();
+        return world;
       }
+      else
+        track.s-=parent->geometry->get_length();
+    }
+    else
+    {
+      error << "Node " << this->nodes[i]->get_index() << " does not have lane " << this->id << " of segment " << this->segment->get_name() << " as a parent";
+      throw CException(_HERE_,error.str());
     }
   }
 
-  throw CException(_HERE_,"Track point does not belong to lane");
+  error << "Track point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ") does not belong to lane  " << this->id << " of segment " << this->segment->get_name();
+  throw CException(_HERE_,error.str());
 }
 
-unsigned int COpendriveLane::get_closest_node(TOpendriveWorldPoint &point,double angle_tol)
+unsigned int COpendriveLane::get_closest_node_index(TOpendriveWorldPoint &point,double angle_tol)
 {
   double dist,min_dist=std::numeric_limits<double>::max();
   TOpendriveWorldPoint found_point;
-  unsigned int closest_index;
+  unsigned int closest_index=-1;
 
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
@@ -833,5 +928,6 @@ COpendriveLane::~COpendriveLane()
   this->width=0.0;
   this->speed=0.0;
   this->offset=0.0;
+  this->id=0;
   this->index=-1;
 }
diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp
index 14dbf64737142e18ac6a93eda6df8917e40c791c..4e8ca602a2137ec324ae6e02a4e08aa78d19435b 100644
--- a/src/opendrive_link.cpp
+++ b/src/opendrive_link.cpp
@@ -65,12 +65,12 @@ void COpendriveLink::generate(double start_curvature,double end_curvature,double
   TOpendriveWorldPoint node_start,node_end;
   TPoint start,end;
 
-  node_start=this->prev->get_start_pose();
+  node_start=this->prev->get_point();
   start.x=node_start.x;
   start.y=node_start.y;
   start.heading=node_start.heading;
   start.curvature=start_curvature;
-  node_end=this->next->get_start_pose();
+  node_end=this->next->get_point();
   end.x=node_end.x;
   end.y=node_end.y;
   end.heading=node_end.heading;
@@ -107,27 +107,33 @@ bool COpendriveLink::clean_references(node_up_ref_t &refs)
 
 void COpendriveLink::update_start_pose(TOpendriveWorldPoint *start)
 {
-  TPoint spline_start;
+  TPoint start_point;
   double length;
 
   if(start==NULL)
     return;
-  length=this->find_closest_world_point(*start,spline_start);
-  length=this->spline->get_length()-length;
-  this->spline->set_start_point(spline_start);
-  this->spline->generate(this->resolution,length);
+  if(this->spline!=NULL)
+  {
+    length=this->spline->find_closest_point(start->x,start->y,start_point);
+    length=this->spline->get_length()-length;
+    this->spline->set_start_point(start_point);
+    this->spline->generate(this->resolution,length);
+  }
 }
 
 void COpendriveLink::update_end_pose(TOpendriveWorldPoint *end)
 {
-  TPoint spline_end;
+  TPoint end_point;
   double length;
 
   if(end==NULL)
     return;
-  length=this->find_closest_world_point(*end,spline_end);
-  this->spline->set_end_point(spline_end);
-  this->spline->generate(this->resolution,length);
+  if(this->spline!=NULL)
+  {
+    length=this->spline->find_closest_point(end->x,end->y,end_point);
+    this->spline->set_end_point(end_point);
+    this->spline->generate(this->resolution,length);
+  }
 }
 
 const COpendriveRoadNode &COpendriveLink::get_prev(void) const
@@ -165,14 +171,17 @@ double COpendriveLink::get_scale_factor(void) const
   return this->scale_factor;
 }
 
-double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point)
+double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TOpendriveWorldPoint &point)
 {
+  TPoint spline_point;
   double length;
 
   if(this->spline!=NULL)
   {
-    length=this->spline->find_closest_point(world.x,world.y,point);
-    point.heading=normalize_angle(point.heading);
+    length=this->spline->find_closest_point(world.x,world.y,spline_point);
+    point.x=spline_point.x;
+    point.y=spline_point.y;
+    point.heading=normalize_angle(spline_point.heading);
   }
   else
     length=std::numeric_limits<double>::max();
@@ -180,16 +189,6 @@ double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoi
   return length;
 }
 
-double COpendriveLink::get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world)
-{
-
-}
-
-double COpendriveLink::get_track_point_local(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local)
-{
-
-}
-
 void COpendriveLink::get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length, double end_length) const
 {
   std::vector<double> curvature,heading;
@@ -234,6 +233,7 @@ std::ostream& operator<<(std::ostream& out, COpendriveLink &link)
 {
   out << "        Previous node: " << link.get_prev().get_index() << std::endl;
   out << "        Next node: " << link.get_next().get_index() << std::endl;
+  out << "        Parent segment: " << link.get_parent_segment().get_name() << " (lane " << link.get_parent_lane().get_id() << ")" << std::endl;
   out << "        Road mark: ";
   switch(link.get_road_mark())
   {
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index 4149774986fa6ba0de7cd3a60c6631429ecbc362..aa29deea163dc9aafd7407041b33900a703006ab 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -46,17 +46,21 @@ COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
     new_segment_ref[object.segments[i]]=segment;
   }
   // update references
+  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
 }
 
-COpendriveRoadSegment &COpendriveRoad::operator[](std::string &key)
+COpendriveRoadSegment *COpendriveRoad::get_segment_by_id(std::string &id)
 {
+  std::stringstream error;
+
   for(unsigned int i=0;i<this->segments.size();i++)
   {
-    if(this->segments[i]->get_id()==(unsigned int)std::stoi(key))
-      return *this->segments[i];
+    if(this->segments[i]->get_id()==(unsigned int)std::stoi(id))
+      return this->segments[i];
   }
 
-  throw CException(_HERE_,"No road segment with the given ID");
+  error << "No road segment with the " << id << " ID";
+  throw CException(_HERE_,error.str());
 }
 
 void COpendriveRoad::free(void)
@@ -85,11 +89,13 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
 {
   std::string predecessor_id,successor_id;
   std::string predecessor_contact,successor_contact;
+  COpendriveRoadSegment *prev_road,*road,*next_road;
+  std::stringstream error;
 
   for(OpenDRIVE::road_iterator road_it(open_drive.road().begin());road_it!=open_drive.road().end();++road_it)
   {
     // get current segment
-    COpendriveRoadSegment &road=(*this)[road_it->id().get()];
+    road=this->get_segment_by_id(road_it->id().get());
     // get predecessor and successor
     if(road_it->lane_link().present())
     {
@@ -114,14 +120,14 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
     {
       if(!predecessor_id.empty())
       {
-        COpendriveRoadSegment &prev_road=(*this)[predecessor_id];
-        prev_road.link_segment(road);
+        prev_road=this->get_segment_by_id(predecessor_id);
+        prev_road->link_segment(road);
         predecessor_id.clear();
       }
       if(!successor_id.empty())
       {
-        COpendriveRoadSegment &next_road=(*this)[successor_id];
-        road.link_segment(next_road);
+        next_road=this->get_segment_by_id(successor_id);
+        road->link_segment(next_road);
         successor_id.clear();
       }
     }
@@ -136,15 +142,21 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
           if(connection_it->incomingRoad().present())
             incoming_road_id=connection_it->incomingRoad().get();
           else
-            throw CException(_HERE_,"Connectivity information missing");
+          {
+            error << "Connectivity information missing for segment " << road->get_name() << ": No incomming segment";
+            throw CException(_HERE_,error.str());
+          }
           if(connection_it->connectingRoad().present())
             connecting_road_id=connection_it->connectingRoad().get();
           else
-            throw CException(_HERE_,"Connectivity information missing");
+          {
+            error << "Connectivity information missing for segment " << road->get_name() << ": No connecting segment";
+            throw CException(_HERE_,error.str());
+          }
           if(predecessor_id.compare(incoming_road_id)==0 && successor_id.compare(connecting_road_id)==0)// this is the connection
           {
-            COpendriveRoadSegment &prev_road=(*this)[predecessor_id];
-            COpendriveRoadSegment &next_road=(*this)[successor_id];
+            prev_road=this->get_segment_by_id(predecessor_id);
+            next_road=this->get_segment_by_id(successor_id);
             for(connection::laneLink_iterator lane_link_it(connection_it->laneLink().begin()); lane_link_it!=connection_it->laneLink().end();++lane_link_it)
             {
               int from_lane_id;
@@ -152,27 +164,40 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
               if(lane_link_it->from().present())
                 from_lane_id=lane_link_it->from().get();
               else
-                throw CException(_HERE_,"Connectivity information missing");
+              {
+                error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing from identifier";
+                throw CException(_HERE_,error.str());
+              }
               if(lane_link_it->to().present())
                 to_lane_id=lane_link_it->to().get();
               else
-                throw CException(_HERE_,"Connectivity information missing");
+              {
+                error << "Connectivity information missing for segment " << road->get_name() << ": lane link missing to identifier";
+                throw CException(_HERE_,error.str());
+              }
               if(predecessor_contact=="end")
-                prev_road.link_segment(road,from_lane_id,false,-1,true);
+                prev_road->link_segment(road,from_lane_id,false,-1,true);
               else 
-                prev_road.link_segment(road,from_lane_id,true,-1,true);
-              TOpendriveWorldPoint end=road.get_lane(-1).get_end_point();
+                prev_road->link_segment(road,from_lane_id,true,-1,true);
+              TOpendriveWorldPoint end=road->get_lane(-1).get_end_point();
+              TOpendriveWorldPoint start;
               if(successor_contact=="end")
               {
-                TOpendriveWorldPoint start=next_road.get_lane(to_lane_id).get_end_point();
+                if(to_lane_id<0)
+                  start=next_road->get_lane(to_lane_id).get_end_point();
+                else
+                  start=next_road->get_lane(to_lane_id).get_start_point();
                 if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
-                  road.link_segment(next_road,-1,false,to_lane_id,false);
+                  road->link_segment(next_road,-1,false,to_lane_id,false);
               }
               else
               {
-                TOpendriveWorldPoint start=next_road.get_lane(to_lane_id).get_start_point();
+                if(to_lane_id<0)
+                  start=next_road->get_lane(to_lane_id).get_start_point();
+                else
+                  start=next_road->get_lane(to_lane_id).get_end_point();
                 if(fabs(end.x-start.x)<this->resolution && fabs(end.y-start.y)<this->resolution)
-                  road.link_segment(next_road,-1,false,to_lane_id,true);
+                  road->link_segment(next_road,-1,false,to_lane_id,true);
               }
             }
           }
@@ -184,17 +209,22 @@ void COpendriveRoad::link_segments(OpenDRIVE &open_drive)
 
 unsigned int COpendriveRoad::add_node(COpendriveRoadNode *node)
 {
+  std::stringstream error;
+
   for(unsigned int i=0;i<this->nodes.size();i++)
   {
     if(this->nodes[i]==node)
-      throw CException(_HERE_,"Node already present");
+    {
+      error << "Node " << node->get_index() << " already present";
+      throw CException(_HERE_,error.str());
+    }
   }
   this->nodes.push_back(node);
 
   return this->nodes.size()-1;
 }
 
-void COpendriveRoad::remove_node(COpendriveRoadNode *node)
+bool COpendriveRoad::remove_node(COpendriveRoadNode *node)
 {
   std::vector<COpendriveRoadNode *>::iterator it;
 
@@ -204,26 +234,33 @@ void COpendriveRoad::remove_node(COpendriveRoadNode *node)
     {
       delete *it;
       it=this->nodes.erase(it);
-      break;
+      return true;
     }
     else
       it++;
   }
+
+  return false;
 }
 
 unsigned int COpendriveRoad::add_lane(COpendriveLane *lane)
 {
+  std::stringstream error;
+
   for(unsigned int i=0;i<this->lanes.size();i++)
   {
     if(this->lanes[i]==lane)
-      throw CException(_HERE_,"Lane already present");
+    {
+      error << "Lane " << lane->get_index() << " already present";
+      throw CException(_HERE_,error.str());
+    }
   }
   this->lanes.push_back(lane);
 
   return this->lanes.size()-1;
 }
 
-void COpendriveRoad::remove_lane(COpendriveLane *lane)
+bool COpendriveRoad::remove_lane(COpendriveLane *lane)
 {
   std::vector<COpendriveLane *>::iterator it;
 
@@ -233,44 +270,37 @@ void COpendriveRoad::remove_lane(COpendriveLane *lane)
     {
       delete *it;
       it=this->lanes.erase(it);
-      break;
+      return true;
     }
     else
       it++;
   }
+
+  return false;
 }
 
 void COpendriveRoad::complete_open_lanes(void)
 {
   std::vector<COpendriveLane *>::iterator lane_it;
-  COpendriveRoadNode *new_node;
   TOpendriveWorldPoint end_point;
-  unsigned int new_node_index;
 
   // remove all nodes and lanes not present in the path
   for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
   {
     if((*lane_it)->next.empty())// lane is not connected
     {
-      if((*lane_it)->get_id()<0)
+      try{
         end_point=(*lane_it)->get_end_point();
-      else
-        end_point=(*lane_it)->get_start_point();
-      if(!this->node_exists_at(end_point))// there is no node at the end point
-      {
-        new_node=new COpendriveRoadNode();
-        new_node->set_resolution(this->resolution);
-        new_node->set_scale_factor(this->scale_factor);
-        new_node->set_start_point(end_point);
-        new_node_index=this->add_node(new_node);
-        new_node->set_index(new_node_index);
-        (*lane_it)->add_node(new_node);
-        (*lane_it)->segment->add_node(new_node);
-        (*lane_it)->link_neightbour_lane((*lane_it)->left_lane);
-        (*lane_it)->link_neightbour_lane((*lane_it)->right_lane);
-      }
-      else
+        if(!this->node_exists_at(end_point))// there is no node at the end point
+        {
+          (*lane_it)->segment->add_empty_node(end_point,(*lane_it));
+          (*lane_it)->segment->link_neightbour_lanes();
+        }
+        else
+          lane_it++;
+      }catch(CException &e){
         lane_it++;
+      }
     }
     else
       lane_it++;
@@ -313,6 +343,8 @@ void COpendriveRoad::add_segment(COpendriveRoadSegment *segment,std::vector<unsi
     segment->nodes[i]->set_index(this->nodes.size());
     this->nodes.push_back(segment->nodes[i]);
   }
+  // update the road reference
+  segment->set_parent_road(this);
 }
 
 void COpendriveRoad::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
@@ -347,13 +379,7 @@ void COpendriveRoad::clean_references(segment_up_ref_t &segment_refs,lane_up_ref
 
   for(seg_it=this->segments.begin();seg_it!=this->segments.end();)
   {
-    if(segment_refs.find(*seg_it)!=segment_refs.end())
-    {
-      (*seg_it)=segment_refs[*seg_it];
-      (*seg_it)->clean_references(segment_refs,lane_refs,node_refs);
-      seg_it++;
-    }
-    else if((*seg_it)->updated(segment_refs))
+    if((*seg_it)->updated(segment_refs))
     {
       (*seg_it)->clean_references(segment_refs,lane_refs,node_refs);
       seg_it++;
@@ -363,24 +389,14 @@ void COpendriveRoad::clean_references(segment_up_ref_t &segment_refs,lane_up_ref
   }
   for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
   {
-    if(lane_refs.find(*lane_it)!=lane_refs.end())
-    {
-      (*lane_it)=lane_refs[*lane_it];
-      lane_it++;
-    }
-    else if(!(*lane_it)->updated(lane_refs))
+    if(!(*lane_it)->updated(lane_refs))
       lane_it=this->lanes.erase(lane_it);
     else
       lane_it++;
   }
   for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
   {
-    if(node_refs.find(*node_it)!=node_refs.end())
-    {
-      (*node_it)=node_refs[*node_it];
-      node_it++;
-    }
-    else if(!(*node_it)->updated(node_refs))
+    if(!(*node_it)->updated(node_refs))
       node_it=this->nodes.erase(node_it);
     else
       node_it++;
@@ -399,7 +415,7 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
     present=false;
     for(unsigned int i=0;i<path_nodes.size();i++)
     {
-      if((*lane_it)->has_node(path_nodes[i]))
+      if((*lane_it)->has_node_with_index(path_nodes[i]))
       {
         present=true;
         break;
@@ -412,7 +428,7 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
       {
         for(unsigned int i=0;i<path_nodes.size();i++)
         {
-          if(neightbour_lane->has_node(path_nodes[i]))
+          if(neightbour_lane->has_node_with_index(path_nodes[i]))
           {
             present=true;
             break;
@@ -427,7 +443,7 @@ void COpendriveRoad::prune(std::vector<unsigned int> &path_nodes)
       {
         for(unsigned int i=0;i<path_nodes.size();i++)
         {
-          if(neightbour_lane->has_node(path_nodes[i]))
+          if(neightbour_lane->has_node_with_index(path_nodes[i]))
           {
             present=true;
             break;
@@ -478,7 +494,7 @@ bool COpendriveRoad::node_exists_at(TOpendriveWorldPoint &pose)
 
   for(unsigned int i=0;i<nodes.size();i++)
   {
-    node_pose=this->nodes[i]->get_start_pose();
+    node_pose=this->nodes[i]->get_point();
     if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01)
       return true;
   }
@@ -492,7 +508,7 @@ COpendriveRoadNode* COpendriveRoad::get_node_at(TOpendriveWorldPoint &pose)
 
   for(unsigned int i=0;i<nodes.size();i++)
   {
-    node_pose=this->nodes[i]->get_start_pose();
+    node_pose=this->nodes[i]->get_point();
     if(std::abs(pose.x-node_pose.x)<0.01 && std::abs(pose.y-node_pose.y)<0.01)
       return this->nodes[i];
   }
@@ -528,7 +544,6 @@ void COpendriveRoad::load(const std::string &filename)
       }
       // link segments
       this->link_segments(*open_drive);
-      // process junctions
     }catch (const xml_schema::exception& e){
       std::ostringstream os;
       os << e;
@@ -588,10 +603,15 @@ unsigned int COpendriveRoad::get_num_segments(void) const
 
 const COpendriveRoadSegment& COpendriveRoad::get_segment(unsigned int index) const
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->segments.size())
     return *this->segments[index];
   else
-    throw CException(_HERE_,"Invalid segment index");
+  {
+    error << "Invalid segment index " << index;
+    throw CException(_HERE_,error.str());
+  }
 }
 
 unsigned int COpendriveRoad::get_num_nodes(void) const
@@ -601,21 +621,30 @@ unsigned int COpendriveRoad::get_num_nodes(void) const
 
 const COpendriveRoadNode& COpendriveRoad::get_node(unsigned int index) const
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->nodes.size())
     return *this->nodes[index];
   else
-    throw CException(_HERE_,"Invalid node index");
+  {
+    error << "Invalid node index " << index;
+    throw CException(_HERE_,error.str());
+  }
 }
 
 const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->segments.size())
     return *this->segments[index];
   else
-    throw CException(_HERE_,"Invalid segment index");
+  {
+    error << "Invalid node index " << index;
+    throw CException(_HERE_,error.str());
+  }
 }
 
-
 unsigned int COpendriveRoad::get_closest_node(TOpendriveWorldPoint &point,double angle_tol)
 {
   double dist,min_dist=std::numeric_limits<double>::max();
@@ -681,7 +710,8 @@ std::vector<unsigned int> COpendriveRoad::get_sub_road(std::vector<unsigned int>
   lane_up_ref_t new_lane_ref;
   node_up_ref_t new_node_ref;
   COpendriveRoadNode *node,*next_node;
-  COpendriveRoadSegment *segment,*new_segment,*original_seg1,original_seg2;
+  COpendriveRoadSegment *segment,*new_segment;
+//  CopendriveRoadSegment *original_seg1,*original_seg2;
   COpendriveLink *link;
   std::vector<unsigned int> new_path_nodes;
   unsigned int link_index;
@@ -793,6 +823,7 @@ void COpendriveRoad::operator=(const COpendriveRoad& object)
     new_segment_ref[object.segments[i]]=segment;
   }
   // update references
+  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
 }
 
 std::ostream& operator<<(std::ostream& out, COpendriveRoad &road)
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index f6a3e74db47bd57a85205dc05bde248c674b1e5c..d37638d5722e2aca11059cf81bb6d8aae4dbc600 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -7,13 +7,12 @@ COpendriveRoadNode::COpendriveRoadNode()
 {
   this->resolution=DEFAULT_RESOLUTION;
   this->scale_factor=DEFAULT_SCALE_FACTOR;
-  this->start_point.x=0.0;
-  this->start_point.y=0.0;
-  this->start_point.heading=0.0;
-  this->lanes.clear();
-  this->geometries.clear();
-  this->index=-1;
+  this->point.x=0.0;
+  this->point.y=0.0;
+  this->point.heading=0.0;
   this->parents.clear();
+  this->links.clear();
+  this->index=-1;
 }
 
 COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
@@ -22,13 +21,14 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
 
   this->resolution=object.resolution;
   this->scale_factor=object.scale_factor;
-  this->start_point.x=object.start_point.x;
-  this->start_point.y=object.start_point.y;
-  this->start_point.heading=object.start_point.heading;
-  this->lanes=object.lanes;
-  this->geometries.resize(object.geometries.size());
-  for(unsigned int i=0;i<object.geometries.size();i++)
-    this->geometries[i]=object.geometries[i]->clone();
+  this->point=object.point;
+  this->parents.resize(object.parents.size());
+  for(unsigned int i=0;i<object.parents.size();i++)
+  {
+    this->parents[i].geometry=object.parents[i].geometry->clone();
+    this->parents[i].lane=object.parents[i].lane;
+    this->parents[i].segment=object.parents[i].segment;
+  }
   this->links.clear();
   for(unsigned int i=0;i<object.links.size();i++)
   {
@@ -36,19 +36,17 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
     this->links.push_back(new_link);
   }
   this->index=object.index;
-  this->parents=object.parents;
 }
 
-void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane)
+bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane)
 {
-  TOpendriveWorldPoint lane_end,node_start;
+  TOpendriveWorldPoint lane_end,node_point;
   double start_curvature,end_curvature,length;
   COpendriveLink *new_link;
   bool lane_found=false;
 
-  for(unsigned int i=0;i<this->links.size();i++)
-    if(this->links[i]->prev==this && this->links[i]->next==node)
-      return;
+  if(this->has_link_with(node) || node->has_link_with(this))
+    return false;
   new_link=new COpendriveLink();
   new_link->set_prev(this);
   new_link->set_next(node);
@@ -58,44 +56,44 @@ void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
   new_link->set_parent_segment(segment);
   new_link->set_parent_lane(lane);
   // get the curvature
-  node_start=node->get_start_pose();
-  for(unsigned int i=0;i<this->lanes.size();i++)
+  node_point=node->get_point();
+  for(unsigned int i=0;i<this->parents.size();i++)
   {
-    if(this->lanes[i]->get_id()<0)
-      lane_end=this->lanes[i]->get_end_point();
-    else
-      lane_end=this->lanes[i]->get_start_point();
-    if(fabs(lane_end.x-node_start.x)<this->resolution && fabs(lane_end.y-node_start.y)<this->resolution)
-    {
-      this->geometries[i]->get_curvature(start_curvature,end_curvature);
-      length=this->geometries[i]->get_length();
-      if(this->lanes[i]->get_id()>0)
+    try{
+      lane_end=this->parents[i].lane->get_end_point();
+      if(fabs(lane_end.x-node_point.x)<this->resolution && fabs(lane_end.y-node_point.y)<this->resolution)
       {
-        start_curvature*=-1.0;
-        end_curvature*=-1.0;
+        this->parents[i].geometry->get_curvature(start_curvature,end_curvature);
+        length=this->parents[i].geometry->get_length();
+        if(this->parents[i].lane->get_id()>0)
+        {
+          start_curvature*=-1.0;
+          end_curvature*=-1.0;
+        }
+        lane_found=true;
+        break;
       }
-      lane_found=true;
-      break;
+    }
+    catch(CException &e){
     }
   }
   if(!lane_found)
   {
     start_curvature=0.0;
     end_curvature=0.0;
-    length=sqrt(pow(this->start_point.x-node_start.x,2.0)+pow(this->start_point.y-node_start.y,2.0));
+    length=sqrt(pow(this->point.x-node_point.x,2.0)+pow(this->point.y-node_point.y,2.0));
   }
   new_link->generate(start_curvature,end_curvature,length,lane_found);
-  this->links.push_back(new_link);
-  node->links.push_back(new_link);
+  this->add_link(new_link);
+  node->add_link(new_link);
+
+  return true;
 }
 
 void COpendriveRoadNode::add_link(COpendriveLink *link)
 {
-  for(unsigned int i=0;i<this->links.size();i++)
-    if(this->links[i]==link)
-      return;
-
-  this->links.push_back(link);
+  if(!this->has_link(link))
+    this->links.push_back(link);
 }
 
 void COpendriveRoadNode::remove_link(COpendriveLink *link)
@@ -115,6 +113,35 @@ void COpendriveRoadNode::remove_link(COpendriveLink *link)
   }
 }
 
+bool COpendriveRoadNode::has_link(COpendriveLink *link)
+{
+  for(unsigned int i=0;i<this->links.size();i++)
+    if(this->links[i]==link)
+      return true;
+
+  return false;
+}
+
+bool COpendriveRoadNode::has_link_with(COpendriveRoadNode *node)
+{
+  for(unsigned int i=0;i<this->links.size();i++)
+    if(this->links[i]->prev==this && this->links[i]->next==node)
+      return true;
+
+  return false;
+}
+
+COpendriveLink *COpendriveRoadNode::get_link_with(COpendriveRoadNode *node)
+{
+  for(unsigned int i=0;i<this->links.size();i++)
+  {
+    if(this->links[i]->prev==node || this->links[i]->next==node)
+      return this->links[i];
+  }
+
+  return NULL;
+}
+
 void COpendriveRoadNode::set_resolution(double res)
 {
   this->resolution=res;
@@ -136,23 +163,15 @@ void COpendriveRoadNode::set_index(unsigned int index)
   this->index=index;
 }
 
-void COpendriveRoadNode::set_start_point(TOpendriveWorldPoint &start)
+void COpendriveRoadNode::set_point(TOpendriveWorldPoint &start)
 {
-  this->start_point=start;
+  this->point=start;
 }
 
-void COpendriveRoadNode::add_parent_segment(COpendriveRoadSegment *segment)
-{
-  for(unsigned int i=0;i<this->parents.size();i++)
-    if(this->parents[i]==segment)
-      return;
-
-  this->parents.push_back(segment);
-}
-
-void COpendriveRoadNode::add_lane(const planView::geometry_type &geom_info,COpendriveLane *lane)
+void COpendriveRoadNode::add_parent(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment)
 {
   COpendriveGeometry *geometry;
+  std::stringstream text;
 
   // import geometry
   if(geom_info.line().present())
@@ -164,24 +183,74 @@ void COpendriveRoadNode::add_lane(const planView::geometry_type &geom_info,COpen
   else if(geom_info.paramPoly3().present())
     geometry=new COpendriveParamPoly3();
   else
-    throw CException(_HERE_,"Unsupported or Missing geometry");
+  {
+    text << "Unsupported or missing geometry in road " << segment->get_name() << " (lane " << lane->get_id() << ")";
+    throw CException(_HERE_,text.str());
+  }
   geometry->set_scale_factor(this->scale_factor);
   geometry->load(geom_info);
-  this->geometries.push_back(geometry);
-  this->lanes.push_back(lane);
+  this->add_parent(geometry,lane,segment);
 }
 
-void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriveLane *lane)
+void COpendriveRoadNode::add_parent(COpendriveGeometry *geometry,COpendriveLane *lane,COpendriveRoadSegment *segment)
 {
-  TOpendriveTrackPoint track_point;
-  COpendriveGeometry *geometry;
+  TOpendriveRoadNodeParent new_parent;
+
+  for(unsigned int i=0;i<this->parents.size();i++)
+    if(parents[i].geometry==geometry &&this->parents[i].lane==lane && this->parents[i].segment==segment)
+      return;
+
+  new_parent.lane=lane;
+  new_parent.segment=segment;
+  new_parent.geometry=geometry;
+  this->parents.push_back(new_parent);
+}
+
+TOpendriveRoadNodeParent *COpendriveRoadNode::get_parent_with_lane(const COpendriveLane *lane)
+{
+  for(unsigned int i=0;i<this->parents.size();i++)
+    if(this->parents[i].lane==lane)
+      return &this->parents[i];
+
+  return NULL;
+}
+
+TOpendriveRoadNodeParent *COpendriveRoadNode::get_parent_with_segment(const COpendriveRoadSegment *segment)
+{
+  for(unsigned int i=0;i<this->parents.size();i++)
+    if(this->parents[i].segment==segment)
+      return &this->parents[i];
+
+  return NULL;
+}
+
 
-  for(unsigned int i=0;i<this->geometries.size();i++)
+void COpendriveRoadNode::free(void)
+{
+  for(unsigned int i=0;i<this->parents.size();i++)
+  {
+    delete this->parents[i].geometry;
+    this->parents[i].geometry=NULL;
+  }
+  this->parents.clear();
+  for(unsigned int i=0;i<this->links.size();i++)
   {
-    delete this->geometries[i];
-    this->geometries[i]=NULL;
+    if(this->links[i]->prev==this)
+    {
+      delete this->links[i];
+      this->links[i]=NULL;
+    }
   }
-  this->geometries.clear();
+  this->links.clear();
+}
+
+void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriveLane *lane,COpendriveRoadSegment *segment)
+{
+  TOpendriveTrackPoint track_point;
+  COpendriveGeometry *geometry;
+  std::stringstream text;
+
+  this->free();
   // import geometry
   if(geom_info.line().present())
     geometry=new COpendriveLine();
@@ -192,7 +261,10 @@ void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriv
   else if(geom_info.paramPoly3().present())
     geometry=new COpendriveParamPoly3();
   else
-    throw CException(_HERE_,"Unsupported or Missing geometry");
+  {
+    text << "Unsupported or missing geometry in road " << segment->get_name() << " (lane " << lane->get_id() << ")";
+    throw CException(_HERE_,text.str());
+  }
   geometry->set_scale_factor(this->scale_factor);
   geometry->load(geom_info);
   // import start position
@@ -207,26 +279,18 @@ void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriv
     track_point.s=geometry->get_length();
   }
   track_point.heading=0.0;
-  if(!geometry->get_world_pose(track_point,this->start_point))
+  if(!geometry->get_world_pose(track_point,this->point))
   {
     delete geometry;
-    geometry=NULL;
-    throw CException(_HERE_,"Impossible to get world coordinates");
+    text << "Impossible to get world coordinates " << segment->get_name() << " (lane " << lane->get_id() << ")"; 
+    throw CException(_HERE_,text.str());
   }
   else
   {
     if(lane->get_id()>0)
-      this->start_point.heading=normalize_angle(this->start_point.heading+3.14159);
-  }
-  this->geometries.push_back(geometry);
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    delete this->links[i];
-    this->links[i]=NULL;
+      this->point.heading=normalize_angle(this->point.heading+3.14159);
   }
-  this->links.clear();
-  this->lanes.clear();
-  this->lanes.push_back(lane);
+  this->add_parent(geometry,lane,segment);
 }
 
 bool COpendriveRoadNode::updated(node_up_ref_t &refs)
@@ -240,31 +304,39 @@ bool COpendriveRoadNode::updated(node_up_ref_t &refs)
   return false;
 }
 
+COpendriveRoadNode *COpendriveRoadNode::get_original_node(node_up_ref_t &node_refs)
+{
+  node_up_ref_t::iterator updated_it;
+
+  for(updated_it=node_refs.begin();updated_it!=node_refs.end();updated_it++)
+    if(updated_it->second==this)
+      return updated_it->first;
+
+  return NULL;
+}
+
 void COpendriveRoadNode::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
 {
-  std::vector<COpendriveRoadSegment *>::iterator seg_it;
-  std::vector<COpendriveLane *>::iterator lane_it;
   std::vector<COpendriveLink *>::iterator link_it;
 
   if(this->updated(node_refs))
   {
     for(link_it=this->links.begin();link_it!=this->links.end();link_it++)
       (*link_it)->update_references(segment_refs,lane_refs,node_refs);
-    for(seg_it=this->parents.begin();seg_it!=this->parents.end();seg_it++)
-      if(segment_refs.find(*seg_it)!=segment_refs.end())
-        (*seg_it)=segment_refs[*seg_it];
-    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
-      if(lane_refs.find(*lane_it)!=lane_refs.end())
-        (*lane_it)=lane_refs[*lane_it];
+    for(unsigned int i=0;i<this->parents.size();i++)
+    {
+      if(segment_refs.find(this->parents[i].segment)!=segment_refs.end())
+        this->parents[i].segment=segment_refs[this->parents[i].segment];
+      if(lane_refs.find(this->parents[i].lane)!=lane_refs.end())
+        this->parents[i].lane=lane_refs[this->parents[i].lane];
+    }
   }
 }
 
 void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
 {
-  std::vector<COpendriveRoadSegment *>::iterator seg_it;
-  std::vector<COpendriveLane *>::iterator lane_it;
+  std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
   std::vector<COpendriveLink *>::iterator link_it;
-  unsigned int index=0;
 
   if(this->updated(node_refs))
   {
@@ -275,68 +347,45 @@ void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up
       else
         link_it++;
     }
-    for(seg_it=this->parents.begin();seg_it!=this->parents.end();)
+    for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
     {
-      if(segment_refs.find(*seg_it)!=segment_refs.end())
+      if(!parent_it->segment->updated(segment_refs) || !parent_it->lane->updated(lane_refs))
       {
-        (*seg_it)=segment_refs[*seg_it];
-        seg_it++;
+        delete parent_it->geometry;
+        parent_it=this->parents.erase(parent_it);
       }
-      else if(!(*seg_it)->updated(segment_refs))
-        seg_it=this->parents.erase(seg_it);
       else
-        seg_it++;
-    }
-    for(index=0,lane_it=this->lanes.begin();lane_it!=this->lanes.end();index++)
-    {
-      if(lane_refs.find(*lane_it)!=lane_refs.end())
-      {
-        (*lane_it)=lane_refs[*lane_it];
-        lane_it++;
-      }
-      else if(!(*lane_it)->updated(lane_refs))
-      {
-        lane_it=this->lanes.erase(lane_it);
-        this->geometries.erase(this->geometries.begin()+index);
-      }
-      else
-        lane_it++;
+        parent_it++;
     }
   }
 }
 
 void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPoint *start)
 {
-  std::vector<COpendriveLane *>::iterator lane_it;
+  std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
   std::vector<COpendriveLink *>::iterator link_it;
-  unsigned int i;
   double length;
 
   if(start==NULL)
     return;
   // remove the references to all lanes and segments except for lane
-  for(i=0,lane_it=this->lanes.begin();lane_it!=this->lanes.end();i++)
+  for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
   {
-    if(*lane_it!=lane)
+    if(parent_it->lane!=lane)
     {
-      lane_it=this->lanes.erase(lane_it);
-      this->geometries.erase(this->geometries.begin()+i);
-      this->parents.erase(this->parents.begin()+i);
+      delete parent_it->geometry;
+      parent_it=this->parents.erase(parent_it);
     }
     else
-      lane_it++;
+      parent_it++;
   }
-  length=this->get_closest_point(*start,this->start_point,3.14159);
+  length=this->get_closest_point(*start,this->point,3.14159);
   // update geometry 
-  for(unsigned int i=0;i<this->geometries.size();i++)
+  for(unsigned int i=0;i<this->parents.size();i++)
   {
     if(lane->get_id()<0)
-    {
-      this->geometries[i]->pose.x=this->start_point.x*this->scale_factor;
-      this->geometries[i]->pose.y=this->start_point.y*this->scale_factor;
-      this->geometries[i]->pose.heading=this->start_point.heading;
-    }
-    this->geometries[i]->max_s-=length*this->scale_factor;
+      this->parents[i].geometry->set_start_point(point);
+    this->parents[i].geometry->set_max_s(this->parents[i].geometry->get_max_s()-length);
   }
   // update the links
   for(link_it=this->links.begin();link_it!=this->links.end();)
@@ -356,30 +405,28 @@ void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldP
 
 void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPoint *end)
 {
-  std::vector<COpendriveLane *>::iterator lane_it;
+  std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
   std::vector<COpendriveLink *>::iterator link_it;
   TOpendriveWorldPoint end_point;
-  unsigned int i;
   double length;
 
   if(end==NULL)
     return;
   // remove the references to all lanes and segments except for lane
-  for(i=0,lane_it=this->lanes.begin();lane_it!=this->lanes.end();i++)
+  for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
   { 
-    if(*lane_it!=lane)
+    if(parent_it->lane!=lane)
     {
-      lane_it=this->lanes.erase(lane_it);
-      this->geometries.erase(this->geometries.begin()+i);
-      this->parents.erase(this->parents.begin()+i);
+      delete parent_it->geometry;
+      parent_it=this->parents.erase(parent_it);
     }
     else
-      lane_it++;
+      parent_it++;
   }
   length=this->get_closest_point(*end,end_point,3.14159);
   // update geometry 
-  for(unsigned int i=0;i<this->geometries.size();i++)
-    this->geometries[i]->max_s=length*this->scale_factor;
+  for(unsigned int i=0;i<this->parents.size();i++)
+    this->parents[i].geometry->set_max_s(length);;
   // update the links
   for(link_it=this->links.begin();link_it!=this->links.end();)
   {
@@ -396,17 +443,6 @@ void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPoi
   }
 }
 
-COpendriveLink *COpendriveRoadNode::get_link_with(COpendriveRoadNode *end)
-{
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    if(this->links[i]->prev==end || this->links[i]->next==end)
-      return this->links[i];
-  }
-
-  return NULL;
-}
-
 double COpendriveRoadNode::get_resolution(void) const
 {
   return this->resolution;
@@ -422,22 +458,53 @@ unsigned int COpendriveRoadNode::get_index(void) const
  return this->index;
 }
 
-unsigned int COpendriveRoadNode::get_num_parent_segments(void) const
+unsigned int COpendriveRoadNode::get_num_parents(void) const
 {
   return this->parents.size();
 }
 
 const COpendriveRoadSegment &COpendriveRoadNode::get_parent_segment(unsigned int index) const
 {
+  std::stringstream text;
+
   if(index>=0 && index<this->parents.size())
-    return *this->parents[index];
+    return *this->parents[index].segment;
   else
-    throw CException(_HERE_,"Invalid parent index");
+  {
+    text << "Invalid parent index " << index << " for node " << this->index << " (has " << this->parents.size() << " parents)";
+    throw CException(_HERE_,text.str());
+  }
 }
 
-TOpendriveWorldPoint COpendriveRoadNode::get_start_pose(void) const
+const COpendriveLane &COpendriveRoadNode::get_parent_lane(unsigned int index) const
 {
-  return this->start_point;
+  std::stringstream text;
+
+  if(index>=0 && index<this->parents.size())
+    return *this->parents[index].lane;
+  else
+  {
+    text << "Invalid parent index " << index << " for node " << this->index << " (has " << this->parents.size() << " parents)";
+    throw CException(_HERE_,text.str());
+  }
+}
+
+const COpendriveGeometry &COpendriveRoadNode::get_geometry(unsigned int index) const
+{
+  std::stringstream text;
+
+  if(index>=0 && index<this->parents.size())
+    return *this->parents[index].geometry;
+  else
+  {
+    text << "Invalid parent index " << index << " for node " << this->index << " (has " << this->parents.size() << " parents)";
+    throw CException(_HERE_,text.str());
+  }
+}
+
+TOpendriveWorldPoint COpendriveRoadNode::get_point(void) const
+{
+  return this->point;
 }
 
 unsigned int COpendriveRoadNode::get_num_links(void) const
@@ -447,47 +514,29 @@ unsigned int COpendriveRoadNode::get_num_links(void) const
 
 const COpendriveLink &COpendriveRoadNode::get_link(unsigned int index) const
 {
+  std::stringstream text;
+
   if(index>=0 && index<this->links.size())
     return *this->links[index];
   else
-    throw CException(_HERE_,"Invalid link index");
+  {
+    text << "Invalid link index " << index << " for node " << this->index << " (has " << this->links.size() << " links)";
+    throw CException(_HERE_,text.str());
+  }
 }
 
 const COpendriveLink &COpendriveRoadNode::get_link_ending_at(unsigned int node_index) const
 {
+  std::stringstream text;
+
   for(unsigned int i=0;i<this->links.size();i++)
   {
     if(this->links[i]->get_next().get_index()==node_index)
       return *this->links[i];
   }
 
-  throw CException(_HERE_,"No link in this node ends at the desired node");
-}
-
-unsigned int COpendriveRoadNode::get_num_lanes(void) const
-{
-  return this->lanes.size();
-}
-
-const COpendriveLane &COpendriveRoadNode::get_lane(unsigned int index) const
-{
-  if(index>=0 && index<this->lanes.size())
-    return *this->lanes[index];
-  else
-    throw CException(_HERE_,"Invalid lane index");
-}
-
-unsigned int COpendriveRoadNode::get_num_geometries(void) const
-{
-  return this->geometries.size();
-}
-
-const COpendriveGeometry &COpendriveRoadNode::get_geometry(unsigned int index) const
-{
-  if(index>=0 && index<this->geometries.size())
-    return *this->geometries[index];
-  else
-    throw CException(_HERE_,"Invalid geometry index");
+  text << "No link in node " << this->index << " ends at node " << node_index;;
+  throw CException(_HERE_,text.str());
 }
 
 unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPoint &point,double angle_tol)const 
@@ -495,17 +544,17 @@ unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPoint &point,do
   double dist,min_dist=std::numeric_limits<double>::max();
   unsigned int closest_index=-1;
   double angle;
-  TPoint spline_point;
+  TOpendriveWorldPoint closest_point;
 
   for(unsigned int i=0;i<this->links.size();i++)
   {
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
-      this->links[i]->find_closest_world_point(point,spline_point);
-      angle=diff_angle(normalize_angle(point.heading),spline_point.heading);
+      this->links[i]->find_closest_world_point(point,closest_point);
+      angle=diff_angle(normalize_angle(point.heading),closest_point.heading);
       if(fabs(angle)<angle_tol)
       {
-        dist=sqrt(pow(spline_point.x-point.x,2.0)+pow(spline_point.y-point.y,2.0));
+        dist=sqrt(pow(closest_point.x-point.x,2.0)+pow(closest_point.y-point.y,2.0));
         if(dist<min_dist)
         {
           min_dist=dist;
@@ -522,17 +571,17 @@ double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPoint &point,doub
 {
   double dist,min_dist=std::numeric_limits<double>::max();
   double angle;
-  TPoint spline_point;
+  TOpendriveWorldPoint closest_point;
 
   for(unsigned int i=0;i<this->links.size();i++)
   {
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
-      this->links[i]->find_closest_world_point(point,spline_point);
-      angle=diff_angle(normalize_angle(point.heading),spline_point.heading);
+      this->links[i]->find_closest_world_point(point,closest_point);
+      angle=diff_angle(normalize_angle(point.heading),closest_point.heading);
       if(fabs(angle)<angle_tol)
       {
-        dist=sqrt(pow(spline_point.x-point.x,2.0)+pow(spline_point.y-point.y,2.0));
+        dist=sqrt(pow(closest_point.x-point.x,2.0)+pow(closest_point.y-point.y,2.0));
         if(dist<min_dist)
           min_dist=dist;
       }
@@ -547,7 +596,6 @@ double COpendriveRoadNode::get_closest_point(TOpendriveWorldPoint &point,TOpendr
   double dist,min_dist=std::numeric_limits<double>::max();
   double angle;
   double length,closest_length=std::numeric_limits<double>::max();
-  TPoint spline_point;
 
   closest_point.x=std::numeric_limits<double>::max();
   closest_point.y=std::numeric_limits<double>::max();
@@ -556,17 +604,14 @@ double COpendriveRoadNode::get_closest_point(TOpendriveWorldPoint &point,TOpendr
   {
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
-      length=this->links[i]->find_closest_world_point(point,spline_point);
-      angle=diff_angle(normalize_angle(point.heading),spline_point.heading);
+      length=this->links[i]->find_closest_world_point(point,closest_point);
+      angle=diff_angle(normalize_angle(point.heading),closest_point.heading);
       if(fabs(angle)<angle_tol)
       {
-        dist=sqrt(pow(spline_point.x-point.x,2.0)+pow(spline_point.y-point.y,2.0));
+        dist=sqrt(pow(closest_point.x-point.x,2.0)+pow(closest_point.y-point.y,2.0));
         if(dist<min_dist)
         {
           min_dist=dist;
-          closest_point.x=spline_point.x;
-          closest_point.y=spline_point.y;
-          closest_point.heading=spline_point.heading;
           closest_length=length;
         }
       }
@@ -581,8 +626,7 @@ void COpendriveRoadNode::get_closest_points(TOpendriveWorldPoint &point,std::vec
   double dist;
   double angle;
   double length;
-  TOpendriveWorldPoint new_point;
-  TPoint spline_point;
+  TOpendriveWorldPoint closest_point;
 
   closest_points.clear();
   closest_lengths.clear();
@@ -590,17 +634,14 @@ void COpendriveRoadNode::get_closest_points(TOpendriveWorldPoint &point,std::vec
   {
     if(&this->links[i]->get_prev()==this)// links starting at this node
     {
-      length=this->links[i]->find_closest_world_point(point,spline_point);
-      angle=diff_angle(normalize_angle(point.heading),spline_point.heading);
+      length=this->links[i]->find_closest_world_point(point,closest_point);
+      angle=diff_angle(normalize_angle(point.heading),closest_point.heading);
       if(fabs(angle)<angle_tol)
       {
-        dist=sqrt(pow(spline_point.x-point.x,2.0)+pow(spline_point.y-point.y,2.0));
+        dist=sqrt(pow(closest_point.x-point.x,2.0)+pow(closest_point.y-point.y,2.0));
         if(dist<=dist_tol)
         {
-          new_point.x=spline_point.x;
-          new_point.y=spline_point.y;
-          new_point.heading=spline_point.heading;
-          closest_points.push_back(new_point);
+          closest_points.push_back(closest_point);
           closest_lengths.push_back(length);
         }
       }
@@ -611,16 +652,13 @@ void COpendriveRoadNode::get_closest_points(TOpendriveWorldPoint &point,std::vec
 std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
 {
   out << "    Node: " << node.get_index() << std::endl;
-  out << "      start point: x: " << node.start_point.x << " y: " << node.start_point.y << " heading: " << node.start_point.heading << std::endl;
-  out << "      Number of parent segments: " << node.get_num_parent_segments() << std::endl;
-  for(unsigned int i=0;i<node.get_num_parent_segments();i++)
-    out << "        " << node.get_parent_segment(i).get_name() << std::endl; 
-  out << "      Number of parent lanes: " << node.lanes.size() << std::endl;
-  for(unsigned int i=0;i<node.lanes.size();i++)
+  out << "      start point: x: " << node.point.x << " y: " << node.point.y << " heading: " << node.point.heading << std::endl;
+  out << "      Number of parent: " << node.get_num_parents() << std::endl;
+  for(unsigned int i=0;i<node.get_num_parents();i++)
   {
-    out << "      Lane " << i << ":" << std::endl;
-    out << "        lane id: " << node.lanes[i]->get_id() << std::endl;
-    out << *node.geometries[i];
+    out << "      Parent " << i << ":" << std::endl;
+    out << "        segment " << node.get_parent_segment(i).get_name() << " (lane " << node.get_parent_lane(i).get_id() << ")" << std::endl; 
+    out << *node.parents[i].geometry;
   }
   out << "      Number of links: " << node.links.size() << std::endl;
   for(unsigned int i=0;i<node.links.size();i++)
@@ -635,25 +673,9 @@ COpendriveRoadNode::~COpendriveRoadNode()
 {
   this->resolution=DEFAULT_RESOLUTION;
   this->scale_factor=DEFAULT_SCALE_FACTOR;
-  this->start_point.x=0.0;
-  this->start_point.y=0.0;
-  this->start_point.heading=0.0;
-  this->lanes.clear();
-  for(unsigned int i=0;i<this->geometries.size();i++)
-  {
-    delete this->geometries[i];
-    this->geometries[i]=NULL;
-  }
-  this->geometries.clear();
-  for(unsigned int i=0;i<this->links.size();i++)
-  {
-    if(this->links[i]->prev==this)
-    {
-      delete this->links[i];
-      this->links[i]=NULL;
-    }
-  }
-  this->parents.clear();
-  this->links.clear();
+  this->point.x=0.0;
+  this->point.y=0.0;
+  this->point.heading=0.0;
+  this->free();
 }
 
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index d8e3b3ae037cb8d26306d2cf9ee4db038ee440cc..9f56769e7d9b2458edaa0aa14124f439903b25fb 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -6,6 +6,7 @@ COpendriveRoadSegment::COpendriveRoadSegment()
 {
   this->name="";
   this->id=-1;
+  this->center_mark=OD_MARK_NONE;
   this->lanes.clear();
   this->num_left_lanes=0;
   this->num_right_lanes=0;
@@ -182,13 +183,7 @@ void COpendriveRoadSegment::clean_references(segment_up_ref_t &segment_refs,lane
   {
     for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
     {
-      if(lane_refs.find((*lane_it).second)!=lane_refs.end())
-      {
-        lane_it->second=lane_refs[lane_it->second];
-        lane_it->second->clean_references(segment_refs,lane_refs,node_refs);
-        lane_it++;
-      }
-      else if(lane_it->second->updated(lane_refs))
+      if(lane_it->second->updated(lane_refs))
       {
         lane_it->second->clean_references(segment_refs,lane_refs,node_refs);
         lane_it++;
@@ -198,24 +193,14 @@ void COpendriveRoadSegment::clean_references(segment_up_ref_t &segment_refs,lane
     }
     for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
     {
-      if(node_refs.find(*node_it)!=node_refs.end())
-      {
-        (*node_it)=node_refs[*node_it];
-        node_it++;
-      }
-      else if(!(*node_it)->updated(node_refs))
+      if(!(*node_it)->updated(node_refs))
         node_it=this->nodes.erase(node_it);
       else
         node_it++;
     }
     for(seg_it=this->connecting.begin();seg_it!=this->connecting.end();)
     {
-      if(segment_refs.find(*seg_it)!=segment_refs.end())
-      {
-        (*seg_it)=segment_refs[*seg_it];
-        seg_it++;
-      }
-      else if(!(*seg_it)->updated(segment_refs))
+      if(!(*seg_it)->updated(segment_refs))
         seg_it=this->connecting.erase(seg_it);
       else
         seg_it++;
@@ -227,28 +212,14 @@ void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
 {
   right::lane_iterator r_lane_it;
   left::lane_iterator l_lane_it;
-  COpendriveLane *new_lane;
-  std::stringstream text;
-  unsigned int lane_index;
 
   // right lanes
   if(lane_section.right().present())
   {
     for(r_lane_it=lane_section.right()->lane().begin();r_lane_it!=lane_section.right()->lane().end();r_lane_it++)
     {
-      try{
-        new_lane=new COpendriveLane();
-        new_lane->set_resolution(this->resolution);
-        new_lane->set_scale_factor(this->scale_factor);
-        new_lane->load(*r_lane_it,this);
-        this->lanes[new_lane->get_id()]=new_lane;
-        lane_index=parent_road->add_lane(new_lane);
-        new_lane->set_index(lane_index);
-        this->num_right_lanes++;
-      }catch(CException &e){
-        text << e.what() << " in road " << this->name;
-        throw CException(_HERE_,text.str());
-      }
+      this->add_lane(*r_lane_it);
+      this->num_right_lanes++;
     }
   }
   // left lanes
@@ -256,23 +227,42 @@ void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
   {
     for(l_lane_it=lane_section.left()->lane().begin();l_lane_it!=lane_section.left()->lane().end();l_lane_it++)
     {
-      try{
-        new_lane=new COpendriveLane();
-        new_lane->set_resolution(this->resolution);
-        new_lane->set_scale_factor(this->scale_factor);
-        new_lane->load(*l_lane_it,this);
-        this->lanes[new_lane->get_id()]=new_lane;
-        lane_index=parent_road->add_lane(new_lane);
-        new_lane->set_index(lane_index);
-        this->num_left_lanes++;
-      }catch(CException &e){
-        text << e.what() << " in road " << this->name;
-        throw CException(_HERE_,text.str());
-      }
+      this->add_lane(*l_lane_it);
+      this->num_left_lanes++;
     }
   }
 }
 
+void COpendriveRoadSegment::add_lane(const ::lane &lane_info)
+{
+  std::map<int,COpendriveLane *>::iterator lane_it; 
+  COpendriveLane *new_lane;
+  unsigned int lane_index;
+  bool exist=false;
+
+  try{
+    new_lane=new COpendriveLane();
+    new_lane->set_resolution(this->resolution);
+    new_lane->set_scale_factor(this->scale_factor);
+    new_lane->load(lane_info,this);
+    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
+      if(lane_it->first==new_lane->get_id())
+      {
+        exist=true;
+        break;
+      }
+    if(!exist)
+      this->lanes[new_lane->get_id()]=new_lane;
+    lane_index=parent_road->add_lane(new_lane);
+    new_lane->set_index(lane_index);
+  }catch(CException &e){
+    this->free();
+    if(!this->parent_road->remove_lane(new_lane))
+      delete new_lane;
+    throw e;
+  }
+}
+
 void COpendriveRoadSegment::remove_lane(COpendriveLane *lane)
 {
   std::map<int,COpendriveLane *>::iterator lane_it;
@@ -291,27 +281,15 @@ void COpendriveRoadSegment::remove_lane(COpendriveLane *lane)
         this->remove_node(lane->nodes[i]);
       // remove the reference from neightbour lanes
       if(lane_it->second->left_lane!=NULL)
-        lane_it->second->left_lane->right_lane=NULL;
+        lane_it->second->left_lane->remove_lane(lane);
       if(lane_it->second->right_lane!=NULL)
-        lane_it->second->right_lane->left_lane=NULL;
+        lane_it->second->right_lane->remove_lane(lane);
       // remove the reference from next lanes
       for(unsigned int i=0;i<lane_it->second->next.size();i++)
-        for(other_lane_it=lane_it->second->next[i]->prev.begin();other_lane_it!=lane_it->second->next[i]->prev.end();)
-        {
-          if((*other_lane_it)==lane)
-            other_lane_it=lane_it->second->next[i]->prev.erase(other_lane_it);
-          else
-            other_lane_it++;
-        }
+        lane_it->second->next[i]->remove_lane(lane);
       // remove the reference from prev lanes
       for(unsigned int i=0;i<lane_it->second->prev.size();i++)
-        for(other_lane_it=lane_it->second->prev[i]->next.begin();other_lane_it!=lane_it->second->prev[i]->next.end();)
-        {
-          if((*other_lane_it)==lane)
-            other_lane_it=lane_it->second->prev[i]->next.erase(other_lane_it);
-          else
-            other_lane_it++;
-        }
+        lane_it->second->prev[i]->remove_lane(lane);
       lane_it=this->lanes.erase(lane_it);
       break;
     }
@@ -322,92 +300,104 @@ void COpendriveRoadSegment::remove_lane(COpendriveLane *lane)
 
 void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
 {
-  std::map<int,COpendriveLane *>::const_iterator lane_it;
   planView::geometry_iterator geom_it;
+  double lane_offset;
+
+  for(geom_it=road_info.planView().geometry().begin(); geom_it!=road_info.planView().geometry().end(); ++geom_it)
+  {
+    lane_offset=0.0;
+    for(int i=-1;i>=-this->num_right_lanes;i--)
+    {
+      this->lanes[i]->set_offset(lane_offset);
+      this->add_node(*geom_it,this->lanes[i]);
+      lane_offset-=this->lanes[i]->width;
+    }
+  }
+  for(geom_it=road_info.planView().geometry().end(); geom_it!=road_info.planView().geometry().begin();)
+  {
+    geom_it--;
+    lane_offset=0.0;
+    for(int i=1;i<=this->num_left_lanes;i++)
+    {
+      this->lanes[i]->set_offset(lane_offset);
+      this->add_node(*geom_it,this->lanes[i]);
+      lane_offset+=this->lanes[i]->width;
+    }
+  }
+}
+
+void COpendriveRoadSegment::add_node(const planView::geometry_type &geom_info,COpendriveLane *lane)
+{
   COpendriveRoadNode *new_node;
   TOpendriveWorldPoint node_pose;
-  double lane_offset;
-  unsigned int index;
+  unsigned int node_index;
+  bool exist=false;
 
   try{
-    for(geom_it=road_info.planView().geometry().begin(); geom_it!=road_info.planView().geometry().end(); ++geom_it)
+    new_node=new COpendriveRoadNode();
+    new_node->set_resolution(this->resolution);
+    new_node->set_scale_factor(this->scale_factor);
+    new_node->load(geom_info,lane,this);
+    if(new_node->get_geometry(0).get_length()>this->min_road_length)
     {
-      lane_offset=0.0;
-      for(int i=-1;i>=-this->num_right_lanes;i--)
+      node_pose=new_node->get_point();
+      if(this->parent_road->node_exists_at(node_pose))
       {
-        new_node=new COpendriveRoadNode();
-        new_node->set_resolution(this->resolution);
-        new_node->set_scale_factor(this->scale_factor);
-        this->lanes[i]->set_offset(lane_offset);
-        new_node->load(*geom_it,this->lanes[i]);
-        if(new_node->get_geometry(0).get_length()>this->min_road_length)
-        {
-          node_pose=new_node->get_start_pose();
-          if(this->parent_road->node_exists_at(node_pose))
-          {
-            delete new_node;
-            new_node=this->parent_road->get_node_at(node_pose);
-            new_node->add_lane(*geom_it,this->lanes[i]);
-          }
-          else
-          {
-            index=this->parent_road->add_node(new_node);
-            new_node->set_index(index);
-          }
-          new_node->add_parent_segment(this);
-          this->add_node(new_node);
-          this->lanes[i]->add_node(new_node);
-        }
-        lane_offset-=this->lanes[i]->width;
+        delete new_node;
+        new_node=this->parent_road->get_node_at(node_pose);
+        new_node->add_parent(geom_info,lane,this);
       }
-    }
-    for(geom_it=road_info.planView().geometry().end(); geom_it!=road_info.planView().geometry().begin();)
-    {
-      geom_it--;
-      lane_offset=0.0;
-      for(int i=1;i<=this->num_left_lanes;i++)
+      else
       {
-        new_node=new COpendriveRoadNode();
-        new_node->set_resolution(this->resolution);
-        new_node->set_scale_factor(this->scale_factor);
-        this->lanes[i]->set_offset(lane_offset);
-        new_node->load(*geom_it,this->lanes[i]);
-        if(new_node->get_geometry(0).get_length()>this->min_road_length)
+        node_index=this->parent_road->add_node(new_node);
+        new_node->set_index(node_index);
+      }
+      for(unsigned int i=0;i<this->nodes.size();i++)
+        if(this->nodes[i]==new_node)
         {
-          node_pose=new_node->get_start_pose();
-          if(this->parent_road->node_exists_at(node_pose))
-          {
-            delete new_node;
-            new_node=this->parent_road->get_node_at(node_pose);
-            new_node->add_lane(*geom_it,this->lanes[i]);
-          }
-          else
-          {
-            index=this->parent_road->add_node(new_node);
-            new_node->set_index(index);
-          }
-          new_node->add_parent_segment(this);
-          this->add_node(new_node);
-          this->lanes[i]->add_node(new_node);
+          exist=true;
+          break;
         }
-        lane_offset+=this->lanes[i]->width;
-      }
+      if(!exist)
+        this->nodes.push_back(new_node);
+      lane->add_node(new_node);
     }
   }catch(CException &e){
-    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
-      delete lane_it->second;
-    this->lanes.clear();
+    this->free();
+    if(!this->parent_road->remove_node(new_node))
+      delete new_node;
     throw e;
   }
 }
 
-void COpendriveRoadSegment::add_node(COpendriveRoadNode *node)
+void COpendriveRoadSegment::add_empty_node(TOpendriveWorldPoint &point,COpendriveLane *lane)
 {
-  for(unsigned int i=0;i<this->nodes.size();i++)
-    if(this->nodes[i]==node)
-      return;
-  //add the node
-  this->nodes.push_back(node);
+  COpendriveRoadNode *new_node;
+  unsigned int node_index;
+  bool exist=false;
+
+  try{
+    new_node=new COpendriveRoadNode();
+    new_node->set_resolution(this->resolution);
+    new_node->set_scale_factor(this->scale_factor);
+    new_node->set_point(point);
+    node_index=this->parent_road->add_node(new_node);
+    new_node->set_index(node_index);
+    for(unsigned int i=0;i<this->nodes.size();i++)
+      if(this->nodes[i]==new_node)
+      {
+        exist=true;
+        break;
+      }
+    if(!exist)
+      this->nodes.push_back(new_node);
+    lane->add_node(new_node);
+  }catch(CException &e){
+    this->free();
+    if(!this->parent_road->remove_node(new_node))
+      delete new_node;
+    throw e;
+  }
 }
 
 void COpendriveRoadSegment::remove_node(COpendriveRoadNode *node)
@@ -438,6 +428,7 @@ bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node)
 int COpendriveRoadSegment::get_node_side(COpendriveRoadNode *node)
 {
   std::map<int,COpendriveLane *>::iterator it;
+  std::stringstream error;
 
   if(this->has_node(node))
   {
@@ -445,10 +436,14 @@ int COpendriveRoadSegment::get_node_side(COpendriveRoadNode *node)
       if(it->second->has_node(node))
         return it->first;
 
-    return 0;
+    error << "Segment " << this->name << " does not include node " << node->get_index();
+    throw CException(_HERE_,error.str());
   }
   else
-    throw CException(_HERE_,"Road segment does not include the given node");
+  {
+    error << "Segment " << this->name << " does not include node " << node->get_index();
+    throw CException(_HERE_,error.str());
+  }
 }
 
 void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section)
@@ -487,97 +482,81 @@ void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_
         this->set_center_mark(OD_MARK_NONE);
     }
   }
+  this->link_neightbour_lanes();
+}
+
+void COpendriveRoadSegment::link_neightbour_lanes(void)
+{
   for(int i=-this->num_right_lanes;i<0;i++)
   {
     if(this->lanes.find(i+1)!=this->lanes.end())// if the lane exists 
       this->lanes[i]->link_neightbour_lane(this->lanes[i+1]);
     else
-      this->lanes[i]->left_mark=this->center_mark;
+      this->lanes[i]->set_left_lane(NULL,this->center_mark);
   }
   for(int i=this->num_left_lanes;i>0;i--)
   {
     if(this->lanes.find(i-1)!=this->lanes.end())// if the lane exists 
       this->lanes[i]->link_neightbour_lane(this->lanes[i-1]);
     else
-      this->lanes[i]->right_mark=this->center_mark;
+      this->lanes[i]->set_right_lane(NULL,this->center_mark);
   }
 }
 
-void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment)
+void COpendriveRoadSegment::link_segment(COpendriveRoadSegment *segment)
 {
-  for(unsigned int i=0;i<this->connecting.size();i++)
-    if(this->connecting[i]->get_id()==segment.get_id())// the segment is already included
-      return;
-  for(unsigned int i=0;i<segment.connecting.size();i++)
-    if(segment.connecting[i]->get_id()==this->id)
-      return;
-  this->connecting.push_back(&segment);
-  segment.connecting.push_back(this);
+  if(this->connects_to(segment) || segment->connects_to(this))
+    return;
+  this->connecting.push_back(segment);
+  segment->connecting.push_back(this);
   // link lanes
   for(int i=-this->num_right_lanes;i<0;i++)
   {
     for(int j=i-1;j<=i+1;j++)
     {
-      if(segment.lanes.find(j)!=segment.lanes.end())
+      if(segment->lanes.find(j)!=segment->lanes.end())
       {
         if(j==i-1)
-          this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->right_mark,false,true);
+          this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->right_mark,false,true);
         else if(j==i)
-          this->lanes[i]->link_lane(segment.lanes[j],OD_MARK_NONE,false,true);
+          this->lanes[i]->link_lane(segment->lanes[j],OD_MARK_NONE,false,true);
         else
-          this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->left_mark,false,true);
+          this->lanes[i]->link_lane(segment->lanes[j],this->lanes[i]->left_mark,false,true);
       }
     }
   }
-  for(int i=1;i<=segment.num_left_lanes;i++)
+  for(int i=1;i<=segment->num_left_lanes;i++)
   {
     for(int j=i-1;j<=i+1;j++)
     {
       if(this->lanes.find(j)!=this->lanes.end())
       {
         if(j==i-1)
-          segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->right_mark,false,true);
+          segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->right_mark,false,true);
         else if(j==i)
-          segment.lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE,false,true);
+          segment->lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE,false,true);
         else
-          segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->left_mark,false,true);
+          segment->lanes[i]->link_lane(this->lanes[j],segment->lanes[i]->left_mark,false,true);
       }
     }
   }
 }
 
-void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment,int from,bool from_start,int to,bool to_start)
+void COpendriveRoadSegment::link_segment(COpendriveRoadSegment *segment,int from,bool from_start,int to,bool to_start)
 {
-  bool connect=true;
-
-  for(unsigned int i=0;i<this->connecting.size();i++)
-  {
-    if(this->connecting[i]->get_id()==segment.get_id())// the segment is already included
-    {
-      for(unsigned int j=0;j<segment.connecting.size();j++)
-      {
-        if(segment.connecting[j]->get_id()==this->id)
-        {
-          connect=false;
-          break;
-        }
-      }
-    }
-  }
-  if(connect)
-  {
-    this->connecting.push_back(&segment);
-    segment.connecting.push_back(this);
-  }
+  if(!this->connects_to(segment))
+    this->connecting.push_back(segment);
+  if(!segment->connects_to(this))
+    segment->connecting.push_back(this);
   // link lanes
-  if(segment.lanes.find(to)!=segment.lanes.end())
+  if(segment->lanes.find(to)!=segment->lanes.end())
   {
     if(this->lanes.find(from-1)!=this->lanes.end())
-      this->lanes[from-1]->link_lane(segment.lanes[to],this->lanes[from-1]->right_mark,from_start,to_start);
+      this->lanes[from-1]->link_lane(segment->lanes[to],this->lanes[from-1]->right_mark,from_start,to_start);
     if(this->lanes.find(from)!=this->lanes.end())
-      this->lanes[from]->link_lane(segment.lanes[to],OD_MARK_NONE,from_start,to_start);
+      this->lanes[from]->link_lane(segment->lanes[to],OD_MARK_NONE,from_start,to_start);
     if(this->lanes.find(from+1)!=this->lanes.end())
-      this->lanes[from+1]->link_lane(segment.lanes[to],this->lanes[from+1]->left_mark,from_start,to_start);
+      this->lanes[from+1]->link_lane(segment->lanes[to],this->lanes[from+1]->left_mark,from_start,to_start);
   }
 /*
   if(segment.lanes.find(to-1)!=segment.lanes.end())
@@ -592,7 +571,7 @@ void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment,int from
 bool COpendriveRoadSegment::connects_to(COpendriveRoadSegment* segment)
 {
   for(unsigned int i=0;i<this->connecting.size();i++)
-    if(this->connecting[i]==segment)
+    if(this->connecting[i]->get_id()==segment->get_id())
       return true;
 
   return false;
@@ -667,15 +646,22 @@ void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
   lanes::laneSection_iterator lane_section;
   COpendriveSignal *new_signal;
   COpendriveObject *new_object;
+  std::stringstream error;
 
   this->free();
   this->set_name(road_info.name().get());
   this->set_id(std::stoi(road_info.id().get()));
   // only one lane section is supported
   if(road_info.lanes().laneSection().size()<1)
-    throw CException(_HERE_,"No lane sections defined for road "+road_info.id().get());
+  {
+    error << "No lane sections defined for segment " << this->name;
+    throw CException(_HERE_,error.str());
+  }
   else if(road_info.lanes().laneSection().size()>1)
-    throw CException(_HERE_,"Road "+road_info.id().get()+" has more than one lane section");
+  {
+    error << "Segment " << this->name << " has more than one lane section";
+    throw CException(_HERE_,error.str());
+  }
   else
     lane_section=road_info.lanes().laneSection().begin();
 
@@ -737,13 +723,16 @@ unsigned int COpendriveRoadSegment::get_num_left_lanes(void) const
 const COpendriveLane &COpendriveRoadSegment::get_lane(int index) const
 {
   std::map<int,COpendriveLane *>::const_iterator lane_it;
+  std::stringstream error;
 
   for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
   {
     if(lane_it->second->get_id()==index)
       return *lane_it->second;
   }
-  throw CException(_HERE_,"invalid lane index");
+
+  error << "Invalid lane index (" << index << ") for segment " << this->name;
+  throw CException(_HERE_,error.str());
 }
 
 unsigned int COpendriveRoadSegment::get_num_nodes(void) const
@@ -753,10 +742,15 @@ unsigned int COpendriveRoadSegment::get_num_nodes(void) const
 
 const COpendriveRoadNode &COpendriveRoadSegment::get_node(unsigned int index) const
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->nodes.size())
     return *this->nodes[index];
   else
-    throw CException(_HERE_,"Invalid signal index");
+  {
+    error << "Invalid node index (" << index << ") for segment " << this->name;
+    throw CException(_HERE_,error.str());
+  }
 }
 
 const COpendriveRoad &COpendriveRoadSegment::get_parent_road(void) const
@@ -771,10 +765,15 @@ unsigned int COpendriveRoadSegment::get_num_signals(void) const
 
 const COpendriveSignal &COpendriveRoadSegment::get_signal(unsigned int index) const
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->signals.size())
     return *this->signals[index];
   else
-    throw CException(_HERE_,"Invalid signal index");
+  {
+    error << "Invalid signal index (" << index << ") for segment " << this->name;
+    throw CException(_HERE_,error.str());
+  }
 }
 
 unsigned int COpendriveRoadSegment::get_num_objects(void) const
@@ -784,10 +783,15 @@ unsigned int COpendriveRoadSegment::get_num_objects(void) const
 
 const COpendriveObject &COpendriveRoadSegment::get_object(unsigned int index) const
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->objects.size())
     return *this->objects[index];
   else
-    throw CException(_HERE_,"Invalid object index");
+  {
+    error << "Invalid object index (" << index << ") for segment " << this->name;
+    throw CException(_HERE_,error.str());
+  }
 }
 
 unsigned int COpendriveRoadSegment::get_num_connecting(void) const
@@ -797,19 +801,28 @@ unsigned int COpendriveRoadSegment::get_num_connecting(void) const
 
 const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int index) const
 {
+  std::stringstream error;
+
   if(index>=0 && index<this->connecting.size())
     return *this->connecting[index];
   else
-    throw CException(_HERE_,"Invalid connecting segment index");
+  {
+    error << "Invalid connecting segment index (" << index << ") for segment " << this->name;
+    throw CException(_HERE_,error.str());
+  }
 }
 
 TOpendriveLocalPoint COpendriveRoadSegment::transform_to_local_point(const TOpendriveTrackPoint &track)
 {
   TOpendriveTrackPoint point;
   TOpendriveLocalPoint local;
+  std::stringstream error;
 
   if(track.s<0.0)
-    throw CException(_HERE_,"Invalid track point");
+  {
+    error << "Invalid track point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")";
+    throw CException(_HERE_,error.str());
+  }
   point=track;
   if(this->num_right_lanes>0)
     local=this->lanes[-1]->transform_to_local_point(point);
@@ -827,9 +840,13 @@ TOpendriveWorldPoint COpendriveRoadSegment::transform_to_world_point(const TOpen
 {
   TOpendriveTrackPoint point;
   TOpendriveWorldPoint world;
+  std::stringstream error;
 
   if(track.s<0.0)
-    throw CException(_HERE_,"Invalid track point");
+  {
+    error << "Invalid track point (s=" << track.s << ",t=" << track.t << ",heading=" << track.heading << ")";
+    throw CException(_HERE_,error.str());
+  }
   point=track;
   if(this->num_right_lanes>0)
     world=this->lanes[-1]->transform_to_world_point(point);
@@ -860,10 +877,10 @@ std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
     out << *segment.lanes[i];
   out << "  Number of signals: " << segment.signals.size() << std::endl;
   for(unsigned int i=0;i<segment.signals.size();i++)
-    std::cout << *segment.signals[i];
+    out << *segment.signals[i];
   out << "  Number of objects: " << segment.objects.size() << std::endl;
   for(unsigned int i=0;i<segment.objects.size();i++)
-    std::cout << *segment.objects[i];
+    out << *segment.objects[i];
 
   return out;
 }
@@ -873,6 +890,7 @@ COpendriveRoadSegment::~COpendriveRoadSegment()
   this->free();
   this->name="";
   this->id=-1;
+  this->center_mark=OD_MARK_NONE;
   this->resolution=DEFAULT_RESOLUTION;
   this->scale_factor=DEFAULT_SCALE_FACTOR;
   this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;