diff --git a/include/opendrive_arc.h b/include/opendrive_arc.h
index 4b51b3a6ba9d81837b18b1bc755c3ccc890a410b..94ee22b58c6474bf8f3e43cb61e3101375ab06e6 100644
--- a/include/opendrive_arc.h
+++ b/include/opendrive_arc.h
@@ -8,9 +8,10 @@ class COpendriveArc : public COpendriveGeometry
   private:
     double curvature;
   protected:
-    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
+    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
     virtual void print(std::ostream &out);
     virtual void load_params(const planView::geometry_type &geometry_info);
+    virtual std::string get_name(void);
   public:
     COpendriveArc();
     COpendriveArc(double min_s, double max_s, double x, double y, double heading, double curvature);
diff --git a/include/opendrive_geometry.h b/include/opendrive_geometry.h
index f88dfbb19a98acb0d14d604958b02e43cbeed6d0..1ca5939c82b7dace15748416e07f38a28fb72d3a 100644
--- a/include/opendrive_geometry.h
+++ b/include/opendrive_geometry.h
@@ -10,24 +10,28 @@
 
 class COpendriveGeometry
 {
+  friend class COpendriveRoadNode;
   private:
   protected:
+    double scale_factor;
     double min_s; ///< Starting track coordenate "s" for the geometry.
     double max_s; ///< Ending track coordenate "s" for the geometry.
     TOpendriveWorldPoint pose;
-    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) = 0;
+    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const = 0;
     virtual void print(std::ostream &out);
     virtual void load_params(const planView::geometry_type &geometry_info) = 0;
+    virtual std::string get_name(void)=0;
+    void set_scale_factor(double scale);
   public:
     COpendriveGeometry();
     COpendriveGeometry(double min_s, double max_s, double x, double y, double heading);
     COpendriveGeometry(const COpendriveGeometry &object);
     virtual COpendriveGeometry *clone(void) = 0;
     void load(const planView::geometry_type &geometry_info);
-    bool get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
-    bool get_world_pose(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world);
-    bool in_range(double s);
-    double get_length(void);
+    bool get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
+    bool get_world_pose(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world) const;
+    bool in_range(double s) const;
+    double get_length(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 8914c6ab6d7303992675e0b263b3e1785a5ee94e..48b7381b0738e0c7c6b93450b416c8898079b76e 100644
--- a/include/opendrive_lane.h
+++ b/include/opendrive_lane.h
@@ -6,6 +6,9 @@
 #endif
 
 #include "opendrive_road_node.h"
+#include "opendrive_road_segment.h"
+
+class COpendriveRoadSegment;
 
 class COpendriveLane
 {
@@ -14,25 +17,28 @@ class COpendriveLane
     std::vector<COpendriveRoadNode *> nodes;
     COpendriveRoadSegment *segment;
     double scale_factor;
+    double resolution;
     double width;
     double speed;
     double offset;
+    int id;
   protected:
     void add_node(COpendriveRoadNode *node);
     void set_resolution(double res);
     void set_scale_factor(double scale);
     void set_offset(double offset);
-    void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs)
+    void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs);
   public:
     COpendriveLane();
     COpendriveLane(const COpendriveLane &object);
     void load(const right::lane_type &lane_info,COpendriveRoadSegment *segment);
-    unsigned int get_num_nodes(void);
-    const COpendriveRoadNode &get_node(unsigned int index);
-    const COpendriveRoadSegment &get_segment(void);
-    double get_width(void);
-    double get_speed(void);
-    double get_offset(void);
+    unsigned int get_num_nodes(void) const;
+    const COpendriveRoadNode &get_node(unsigned int index) const;
+    const COpendriveRoadSegment &get_segment(void) const;
+    double get_width(void) const;
+    double get_speed(void) const;
+    double get_offset(void) const;
+    int get_id(void) const;
     void operator=(const COpendriveLane &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane);
     ~COpendriveLane();
diff --git a/include/opendrive_line.h b/include/opendrive_line.h
index d9dd05f3a57f83ac8b1f03ef9219b8bf49312084..02b87ee8d86ad45ee89b772a259bbc2d2fad600c 100644
--- a/include/opendrive_line.h
+++ b/include/opendrive_line.h
@@ -7,9 +7,10 @@ class COpendriveLine : public COpendriveGeometry
 {
   private:
   protected:
-    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
+    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
     virtual void print(std::ostream &out);
     virtual void load_params(const planView::geometry_type &geometry_info);
+    virtual std::string get_name(void);
   public:
     COpendriveLine();
     COpendriveLine(double min_s, double max_s, double x, double y, double heading);
diff --git a/include/opendrive_link.h b/include/opendrive_link.h
index b89c14e1bfff9e05dbad6b468e9ae264d77e57d6..bd4d7cae5654b68e4a457190649a6f60e4b99ea9 100644
--- a/include/opendrive_link.h
+++ b/include/opendrive_link.h
@@ -1,15 +1,17 @@
 #ifndef _OPNEDRIVE_LINK_H
 #define _OPENDRIVE_LINK_H
 
-#include "opendrive_road_node.h"
 #include "g2_spline.h"
 #include "opendrive_line.h"
 #include "opendrive_spiral.h"
 #include "opendrive_arc.h"
 #include "opendrive_param_poly3.h"
 
+class COpendriveRoadNode;
+
 class COpendriveLink
 {
+  friend class COpendriveRoadNode;
   private:
     COpendriveRoadNode *prev;
     COpendriveRoadNode *next;
@@ -26,16 +28,16 @@ class COpendriveLink
   public:
     COpendriveLink();
     COpendriveLink(const COpendriveLink &object);
-    const COpendriveRoadNode &get_prev(void);
-    const COpendriveRoadNode &get_next(void);
-    opendrive_mark_t get_road_mark(void);
-    double get_resolution(void);
-    double get_scale_factor(void);
+    const COpendriveRoadNode &get_prev(void) const;
+    const COpendriveRoadNode &get_next(void) const;
+    opendrive_mark_t get_road_mark(void) const;
+    double get_resolution(void) const;
+    double get_scale_factor(void) const;
     double find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point);
     double find_closest_local_point(TOpendriveLocalPoint &local,TPoint &point);
     double get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world);
     double get_track_point_local(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
-    double get_length(void);
+    double get_length(void) const;
     void operator=(const COpendriveLink &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveLink &link);
     ~COpendriveLink();
diff --git a/include/opendrive_object.h b/include/opendrive_object.h
index ca8f6ae8304a5c01cacf0bd1a220b48389fed5d6..7db50f2ea1019083c08a8e835b328334d7337342 100644
--- a/include/opendrive_object.h
+++ b/include/opendrive_object.h
@@ -51,15 +51,15 @@ class COpendriveObject
     COpendriveObject();
     COpendriveObject(const COpendriveObject& object);
     void load(std::unique_ptr<objects::object_type> &object_info);
-    TOpendriveTrackPoint get_pose(void);
-    std::string get_type(void);
-    std::string get_name(void);
-    bool is_box(void);
-    bool is_cylinder(void);
-    bool is_polygon(void);
-    TOpendriveBox get_box_data(void);
-    TOpendriveCylinder get_cylinder_data(void);
-    TOpendrivePolygon get_polygon_data(void);
+    TOpendriveTrackPoint get_pose(void) const;
+    std::string get_type(void) const;
+    std::string get_name(void) const;
+    bool is_box(void) const;
+    bool is_cylinder(void) const;
+    bool is_polygon(void) const;
+    TOpendriveBox get_box_data(void) const;
+    TOpendriveCylinder get_cylinder_data(void) const;
+    TOpendrivePolygon get_polygon_data(void) const;
     void operator=(const COpendriveObject &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveObject &object);
     ~COpendriveObject();
diff --git a/include/opendrive_param_poly3.h b/include/opendrive_param_poly3.h
index d366b85fc1d79ec5804a512ba4f6657d968cd10f..8613d06c039095687b0b6b4dff1c07030d6e89f3 100644
--- a/include/opendrive_param_poly3.h
+++ b/include/opendrive_param_poly3.h
@@ -18,9 +18,10 @@ class COpendriveParamPoly3 : public COpendriveGeometry
     TOpendrivePoly3Params v;
     bool normalized;
   protected:
-    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
+    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
     virtual void print(std::ostream &out);
     virtual void load_params(const planView::geometry_type &geometry_info);
+    virtual std::string get_name(void);
   public:
     COpendriveParamPoly3();
     COpendriveParamPoly3(double min_s, double max_s, double x, double y, double heading,TOpendrivePoly3Params &u,TOpendrivePoly3Params &v,bool normalized);
diff --git a/include/opendrive_road.h b/include/opendrive_road.h
index efd6145fa53372d03bcdce636e48f2b3745afa59..0d75497a39e981df49d376cfa09dac5214346e5e 100644
--- a/include/opendrive_road.h
+++ b/include/opendrive_road.h
@@ -16,18 +16,18 @@ class COpendriveRoad
     double min_road_length;
   protected:
     void add_road_nodes(std::unique_ptr<OpenDRIVE::road_type> &road_info);
-    void set_resolution(double res);
-    void set_scale_factor(double scale);
-    void set_min_road_length(double length);
   public:
     COpendriveRoad();
     COpendriveRoad(const COpendriveRoad& object);
     void load(const std::string &filename);
-    double get_resolution(void);
-    double get_scale_factor(void);
-    double get_min_road_length(void);
-    unsigned int get_num_segments(void);
-    const COpendriveRoadSegment& get_segment(unsigned int index);
+    void set_resolution(double res);
+    void set_scale_factor(double scale);
+    void set_min_road_length(double length);
+    double get_resolution(void) const;
+    double get_scale_factor(void) const;
+    double get_min_road_length(void) const;
+    unsigned int get_num_segments(void) const;
+    const COpendriveRoadSegment& get_segment(unsigned int index) const;
     const COpendriveRoadSegment& operator[](std::size_t index);
     void operator=(const COpendriveRoad& object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index 7b16a7f00353ef418db02e326e3f9620af0d832a..e7d704d272a0e1e1ccbf72871298e9a984c458eb 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -5,8 +5,12 @@
 #include "opendrive_link.h"
 #include "opendrive_lane.h"
 
+class COpendriveLane;
+
 class COpendriveRoadNode
 {
+  friend class COpendriveLane;
+  friend class COpendriveRoadSegment;
   private:
     std::vector<COpendriveLink *> links;
     double resolution;
@@ -22,12 +26,13 @@ class COpendriveRoadNode
     COpendriveRoadNode();
     COpendriveRoadNode(const COpendriveRoadNode &object);
     void load(const planView::geometry_type &node_info,COpendriveLane *lane);
-    double get_resolution(void);
-    double get_scale_factor(void);
-    TOpendriveWorldPoint get_start_pose(void);
-    unsigned int get_num_links(void);
-    const COpendriveLink &get_link(unsigned int index);
-    const COpendriveLane &get_lane(void);
+    double get_resolution(void) const;
+    double get_scale_factor(void) const;
+    TOpendriveWorldPoint get_start_pose(void) const;
+    unsigned int get_num_links(void) const;
+    const COpendriveLink &get_link(unsigned int index) const;
+    const COpendriveLane &get_lane(void) const;
+    const COpendriveGeometry &get_geometry(void) const;
     void operator=(const COpendriveRoadNode &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
     ~COpendriveRoadNode();
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index c66c649d0bc2f1c5b9e8b58f3f5ae647523a178c..3824abd3a38c7d60e237723a640fb998649f0543 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -9,11 +9,16 @@
 #include "opendrive_signal.h"
 #include "opendrive_object.h"
 
+class COpendriveLane;
+
 class COpendriveRoadSegment
 {
   friend class COpendriveRoad;
   private:
     std::map<int,COpendriveLane *> lanes;
+    double resolution;
+    double scale_factor;
+    double min_road_length;
     unsigned int num_right_lanes;
     unsigned int num_left_lanes;
     std::vector<COpendriveSignal *> signals;
@@ -25,26 +30,27 @@ class COpendriveRoadSegment
   protected:
     void set_resolution(double res);
     void set_scale_factor(double scale);
+    void set_min_road_length(double length);
     void update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs);
   public:
     COpendriveRoadSegment();
     COpendriveRoadSegment(const COpendriveRoadSegment &object);
-    void load_road(OpenDRIVE::road_type &road_info);
-    std::string get_name(void);
-    unsigned int get_id(void);
-    unsigned int get_num_right_lanes(void);
-    unsigned int get_num_left_lanes(void);
-    const COpendriveLane &get_lane(int index);
-    unsigned int get_num_signals(void);
-    const COpendriveSignal &get_signal(unsigned int index);
-    unsigned int get_num_obstacles(void);
-    const COpendriveObject &get_object(unsigned int index);
-    unsigned int get_num_previous(void);
-    const COpendriveRoadSegment &get_previous(unsigned int index);
-    unsigned int get_num_next(void);
-    const COpendriveRoadSegment &get_next(unsigned int index);
+    void load(OpenDRIVE::road_type &road_info);
+    std::string get_name(void) const;
+    unsigned int get_id(void) const;
+    unsigned int get_num_right_lanes(void) const;
+    unsigned int get_num_left_lanes(void) const;
+    const COpendriveLane &get_lane(int index) const;
+    unsigned int get_num_signals(void) const;
+    const COpendriveSignal &get_signal(unsigned int index) const;
+    unsigned int get_num_obstacles(void) const;
+    const COpendriveObject &get_object(unsigned int index) const;
+    unsigned int get_num_previous(void) const;
+    const COpendriveRoadSegment &get_previous(unsigned int index) const;
+    unsigned int get_num_next(void) const;
+    const COpendriveRoadSegment &get_next(unsigned int index) const;
     void operator=(const COpendriveRoadSegment &object);
-    friend std::ostream& operator<<(std::ostream& out, COpendriveRoadsegment &segment);
+    friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment);
     ~COpendriveRoadSegment();
 };
 
diff --git a/include/opendrive_signal.h b/include/opendrive_signal.h
index 07049851d68911f8f249fe4cb7b9b94bb559cb57..2048dbe3c38605c442ed98d22cfe812f3c1013fb 100644
--- a/include/opendrive_signal.h
+++ b/include/opendrive_signal.h
@@ -21,11 +21,11 @@ class COpendriveSignal
     COpendriveSignal(const COpendriveSignal &object);
     COpendriveSignal(int id, double s, double t, double heading, std::string type, std::string sub_type, int value, std::string text, bool reverse = false);
     void load(std::unique_ptr<signals::signal_type> &signal_info);
-    int get_id(void);
-    TOpendriveTrackPoint get_pose(void);
-    void get_type(std::string &type, std::string &sub_type);
-    int get_value(void);
-    std::string get_text(void);
+    int get_id(void) const;
+    TOpendriveTrackPoint get_pose(void) const;
+    void get_type(std::string &type, std::string &sub_type) const;
+    int get_value(void) const;
+    std::string get_text(void) const;
     void operator=(const COpendriveSignal &object);
     friend std::ostream& operator<<(std::ostream& out, COpendriveSignal &signal);
     ~COpendriveSignal();
diff --git a/include/opendrive_spiral.h b/include/opendrive_spiral.h
index 05c2a9fec85672aa20bb8b79877849f8840f67f7..a97e3bbb5ceffc6695cc15d2934c609cd04a59ab 100644
--- a/include/opendrive_spiral.h
+++ b/include/opendrive_spiral.h
@@ -9,9 +9,10 @@ class COpendriveSpiral : public COpendriveGeometry
     double start_curvature;
     double end_curvature;
   protected:
-    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
+    virtual bool transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const;
     virtual void print(std::ostream &out);
     virtual void load_params(const planView::geometry_type &geometry_info);
+    virtual std::string get_name(void);
   public:
     COpendriveSpiral();
     COpendriveSpiral(double min_s, double max_s, double x, double y, double heading,double start_curv,double end_curv);
diff --git a/src/opendrive_arc.cpp b/src/opendrive_arc.cpp
index 63c46451a99d8ae63ad76a368cbf5ed82b16490f..9d4fe8737c32da86ee75d53fd51f8f0d527fc1ac 100644
--- a/src/opendrive_arc.cpp
+++ b/src/opendrive_arc.cpp
@@ -16,15 +16,15 @@ COpendriveArc::COpendriveArc(const COpendriveArc &object) : COpendriveGeometry(o
   this->curvature=object.curvature;
 }
 
-bool COpendriveArc::transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local)
+bool COpendriveArc::transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const
 {
   double alpha;
   bool pos_arc;
 
-  alpha = std::fabs((track.s-this->min_s)*this->curvature);
+  alpha = std::fabs((track.s-this->min_s/this->scale_factor)*this->curvature*this->scale_factor);
   pos_arc = (this->curvature < 0.0 ? false : true);
-  local.u = std::sin(alpha)/this->curvature - track.t*std::sin(alpha)*(pos_arc ? 1 : -1);
-  local.v = (1 - std::cos(alpha))*(pos_arc ? 1 : -1)/this->curvature + track.t*std::cos(alpha);
+  local.u = std::sin(alpha)/(this->curvature*this->scale_factor) - track.t*std::sin(alpha)*(pos_arc ? 1 : -1);
+  local.v = (1 - std::cos(alpha))*(pos_arc ? 1 : -1)/(this->curvature*this->scale_factor) + track.t*std::cos(alpha);
   local.heading = normalize_angle(track.heading + alpha*(pos_arc ? 1 : -1));
 
   return true;
@@ -32,7 +32,13 @@ bool COpendriveArc::transform_local_pose(TOpendriveTrackPoint &track,TOpendriveL
 
 void COpendriveArc::print(std::ostream &out)
 {
-  out << "  curvature = " << this->curvature << std::endl;
+  COpendriveGeometry::print(out);
+  out << "        curvature = " << this->get_curvature() << std::endl;
+}
+
+std::string COpendriveArc::get_name(void)
+{
+  return std::string("arc");
 }
 
 void COpendriveArc::load_params(const planView::geometry_type &geometry_info)
@@ -49,7 +55,7 @@ COpendriveGeometry *COpendriveArc::clone(void)
 
 double COpendriveArc::get_curvature(void)
 {
-  return this->curvature;
+  return this->curvature*this->scale_factor;
 }
 
 void COpendriveArc::operator=(const COpendriveArc &object)
diff --git a/src/opendrive_geometry.cpp b/src/opendrive_geometry.cpp
index b76bf6961e31a43e0dc764d82b0ee9e09b2eeca5..8f96f6ea94e87db2e4ec1b3e76d838a59dc23020 100644
--- a/src/opendrive_geometry.cpp
+++ b/src/opendrive_geometry.cpp
@@ -8,6 +8,7 @@ COpendriveGeometry::COpendriveGeometry()
   this->pose.x = 0.0;
   this->pose.y = 0.0;
   this->pose.heading = 0.0;
+  this->scale_factor=DEFAULT_SCALE_FACTOR;
 }
 
 COpendriveGeometry::COpendriveGeometry(double min_s, double max_s, double x, double y, double heading)
@@ -17,6 +18,7 @@ COpendriveGeometry::COpendriveGeometry(double min_s, double max_s, double x, dou
   this->pose.x = x;
   this->pose.y = y;
   this->pose.heading = heading;
+  this->scale_factor=DEFAULT_SCALE_FACTOR;
 }
 
 COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object)
@@ -26,12 +28,14 @@ COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object)
   this->pose.x = object.pose.x;
   this->pose.y = object.pose.y;
   this->pose.heading = object.pose.heading;
+  this->scale_factor = object.scale_factor;
 }
 
 void COpendriveGeometry::print(std::ostream& out)
 {
-  out << "  Geometry from " << this->min_s << " to " << this->max_s << std::endl;
-  out << "    pose: x = " << this->pose.x << ", y = " << this->pose.y << ", heading = " << this->pose.heading << std::endl;
+  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;
 }
 
 void COpendriveGeometry::load(const planView::geometry_type &geometry_info)
@@ -44,37 +48,42 @@ void COpendriveGeometry::load(const planView::geometry_type &geometry_info)
   this->load_params(geometry_info);
 }
 
-bool COpendriveGeometry::get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local)
+void COpendriveGeometry::set_scale_factor(double scale)
+{
+  this->scale_factor=scale;
+}
+
+bool COpendriveGeometry::get_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const
 {
   return this->transform_local_pose(track,local);
 }
 
-bool COpendriveGeometry::get_world_pose(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world)
+bool COpendriveGeometry::get_world_pose(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world) const
 {
   TOpendriveLocalPoint local;
 
   if(this->transform_local_pose(track,local))
   {
     world.heading=normalize_angle(this->pose.heading+local.heading);
-    world.x=local.u*std::cos(this->pose.heading)-local.v*std::sin(this->pose.heading)+this->pose.x;
-    world.y=local.u*std::sin(this->pose.heading)+local.v*std::cos(this->pose.heading)+this->pose.y;
+    world.x=local.u*std::cos(this->pose.heading)-local.v*std::sin(this->pose.heading)+this->pose.x/this->scale_factor;
+    world.y=local.u*std::sin(this->pose.heading)+local.v*std::cos(this->pose.heading)+this->pose.y/this->scale_factor;
     return true;
   }
   else
     return false;
 }
 
-bool COpendriveGeometry::in_range(double s)
+bool COpendriveGeometry::in_range(double s) const
 {
-  if((s<this->max_s) && (s>=this->min_s))
+  if((s<(this->max_s/this->scale_factor)) && (s>=(this->min_s/this->scale_factor)))
     return true;
   else 
     return false;
 }
 
-double COpendriveGeometry::get_length(void)
+double COpendriveGeometry::get_length(void) const
 {
-  return this->max_s-this->min_s;
+  return (this->max_s-this->min_s)/this->scale_factor;
 }
 
 void COpendriveGeometry::operator=(const COpendriveGeometry &object)
@@ -84,6 +93,7 @@ void COpendriveGeometry::operator=(const COpendriveGeometry &object)
   this->pose.x = object.pose.x;
   this->pose.y = object.pose.y;
   this->pose.heading = object.pose.heading;
+  this->scale_factor = object.scale_factor;
 }
 
 std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom)
@@ -99,5 +109,6 @@ COpendriveGeometry::~COpendriveGeometry()
   this->pose.x = 0.0;
   this->pose.y = 0.0;
   this->pose.heading = 0.0;
+  this->scale_factor = DEFAULT_SCALE_FACTOR;
 }
 
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 384fbe4f73cff48c2b2ebcd1c13356d51ad4c6ac..735204de3a559fa76d3b9041275cb5cf969e79c1 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -1,13 +1,16 @@
 #include "opendrive_lane.h"
+#include "exceptions.h"
 
 COpendriveLane::COpendriveLane()
 {
   this->nodes.clear();
   this->segment=NULL;
+  this->resolution=DEFAULT_RESOLUTION;
   this->scale_factor=DEFAULT_SCALE_FACTOR;
   this->width=0.0;
   this->speed=0.0;
   this->offset=0.0;
+  this->id=0;
 }
 
 COpendriveLane::COpendriveLane(const COpendriveLane &object)
@@ -20,18 +23,28 @@ COpendriveLane::COpendriveLane(const COpendriveLane &object)
     node=new COpendriveRoadNode(*object.nodes[i]);
     this->nodes.push_back(node);
   }
+  this->segment=object.segment;
+  this->resolution=object.resolution;
+  this->scale_factor=object.scale_factor;
   this->width=object.width;
   this->speed=object.speed;
   this->offset=object.offset;
+  this->id=object.id;
 }
 
 void COpendriveLane::add_node(COpendriveRoadNode *node)
 {
-
+  for(unsigned int i=0;i<this->nodes.size();i++)
+    if(this->nodes[i]==node)
+      return;
+  // add the new node
+  this->nodes.push_back(node);
 }
 
 void COpendriveLane::set_resolution(double res)
-{ 
+{
+  this->resolution=res;
+
   for(unsigned int i=0;i<this->nodes.size();i++)
     this->nodes[i]->set_resolution(res);
 }
@@ -64,27 +77,28 @@ void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegmen
     this->nodes[i]=NULL;
   }
   this->nodes.clear();
+  this->id=lane_info.id().get();
   // import lane width
   if(lane_info.width().size()<1)
   {
-    error << "No width record present for lane " << lane_info.id().get() << " for road " << road_name;
+    error << "No width record present for lane " << this->id;
     throw CException(_HERE_,error.str());
   }
   else if(lane_info.width().size()>1)
   {
-    error << "More than one width record present for lane " << lane_info.id().get() << " for road " << road_name;
+    error << "More than one width record present for lane " << this->id;
     throw CException(_HERE_,error.str());
   }
-  this->width=lane_info.width().begin()->a().get()/this->scale_factor;
+  this->width=lane_info.width().begin()->a().get();
   // import lane speed
   if(lane_info.speed().size()<1)
   {
-    error << "No speed record present for lane " << lane_info.id().get() << " for road " << road_name;
+    error << "No speed record present for lane " << this->id;
     throw CException(_HERE_,error.str());
   }
   else if(lane_info.speed().size()>1)
   {
-    error << "More than one speed record present for lane " << lane_info.id().get() << " for road " << road_name;
+    error << "More than one speed record present for lane " << this->id;
     throw CException(_HERE_,error.str());
   }
   this->speed=lane_info.speed().begin()->max().get();
@@ -92,47 +106,91 @@ void COpendriveLane::load(const right::lane_type &lane_info,COpendriveRoadSegmen
   this->segment=segment;
 }
 
-unsigned int COpendriveLane::get_num_nodes(void)
+unsigned int COpendriveLane::get_num_nodes(void) const
 {
   return this->nodes.size();
 }
 
-const COpendriveRoadNode &COpendriveLane::get_node(unsigned int index)
+const COpendriveRoadNode &COpendriveLane::get_node(unsigned int index) const
 {
-
+  if(index>=0 && index<this->nodes.size())
+    return *this->nodes[index];
+  else
+    throw CException(_HERE_,"Invalid node index");
 }
 
-const COpendriveRoadSegment &COpendriveLane::get_segment(void)
+const COpendriveRoadSegment &COpendriveLane::get_segment(void) const
 {
-
+  return *this->segment;
 }
 
-double COpendriveLane::get_width(void)
+double COpendriveLane::get_width(void) const
 {
-  return this->width;
+  return this->width/this->scale_factor;
 }
 
-double COpendriveLane::get_speed(void)
+double COpendriveLane::get_speed(void) const
 {
   return this->speed;
 }
 
-double COpendriveLane::get_offset(void)
+double COpendriveLane::get_offset(void) const
+{
+  return this->offset/this->scale_factor;
+}
+
+int COpendriveLane::get_id(void) const
 {
-  return this->offset;
+  return this->id;
 }
 
 void COpendriveLane::operator=(const COpendriveLane &object)
 {
+  COpendriveRoadNode *node;
 
+  this->nodes.clear();
+  for(unsigned int i=0;i<object.nodes.size();i++)
+  {
+    node=new COpendriveRoadNode(*object.nodes[i]);
+    this->nodes.push_back(node);
+  }
+  this->segment=object.segment;
+  this->scale_factor=object.scale_factor;
+  this->resolution=object.resolution;
+  this->width=object.width;
+  this->speed=object.speed;
+  this->offset=object.offset;
+  this->id=object.id;
 }
 
-friend std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
+std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
 {
+  out << "    id: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
+  out << "    width: " << lane.get_width() << std::endl;
+  out << "    speed: " << lane.get_speed() << std::endl;
+  if(lane.segment!=NULL)
+    out << "    Parent road segment: " << lane.segment->get_name() << std::endl;
+  else 
+    out << "    No parent segment" << std::endl;
+  out << "    Number of nodes: " << lane.nodes.size() << std::endl;
+  for(unsigned int i=0;i<lane.nodes.size();i++)
+    out << *lane.nodes[i] << std::endl;
 
+  return out;
 }
 
 COpendriveLane::~COpendriveLane()
 {
-
+  for(unsigned int i=0;i<this->nodes.size();i++)
+  {
+    delete this->nodes[i];
+    this->nodes[i]=NULL;
+  }
+  this->nodes.clear();
+  this->segment=NULL;
+  this->resolution=DEFAULT_RESOLUTION;
+  this->scale_factor=DEFAULT_SCALE_FACTOR;
+  this->width=0.0;
+  this->speed=0.0;
+  this->offset=0.0;
 }
diff --git a/src/opendrive_line.cpp b/src/opendrive_line.cpp
index 38f4f35ee59b123219ff6cdbb6df047dc196d50c..d0933ccea2cf92796d9fdeedeb566b07b9f78c36 100644
--- a/src/opendrive_line.cpp
+++ b/src/opendrive_line.cpp
@@ -15,9 +15,9 @@ COpendriveLine::COpendriveLine(const COpendriveGeometry &object) : COpendriveGeo
 
 }
 
-bool COpendriveLine::transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local)
+bool COpendriveLine::transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const
 {
-  local.u=track.s-this->min_s;
+  local.u=track.s-this->min_s/this->scale_factor;
   local.v=track.t;
   local.heading=track.heading;
 
@@ -29,6 +29,11 @@ void COpendriveLine::print(std::ostream &out)
   COpendriveGeometry::print(out);
 }
 
+std::string COpendriveLine::get_name(void)
+{
+  return std::string("line");
+}
+
 void COpendriveLine::load_params(const planView::geometry_type &geometry_info)
 {
 
diff --git a/src/opendrive_link.cpp b/src/opendrive_link.cpp
index 8f3131c820ba816b25a1025a55e3c6ab2b6e80eb..724da05228e5930216d67fada645590892a2131f 100644
--- a/src/opendrive_link.cpp
+++ b/src/opendrive_link.cpp
@@ -35,27 +35,27 @@ void COpendriveLink::set_scale_factor(double scale)
 
 }
 
-const COpendriveRoadNode &COpendriveLink::get_prev(void)
+const COpendriveRoadNode &COpendriveLink::get_prev(void) const
 {
 
 }
 
-const COpendriveRoadNode &COpendriveLink::get_next(void)
+const COpendriveRoadNode &COpendriveLink::get_next(void) const
 {
 
 }
 
-opendrive_mark_t COpendriveLink::get_road_mark(void)
+opendrive_mark_t COpendriveLink::get_road_mark(void) const
 {
 
 }
 
-double COpendriveLink::get_resolution(void)
+double COpendriveLink::get_resolution(void) const
 {
 
 }
 
-double COpendriveLink::get_scale_factor(void)
+double COpendriveLink::get_scale_factor(void) const
 {
 
 }
@@ -80,7 +80,7 @@ double COpendriveLink::get_track_point_local(TOpendriveTrackPoint &track,TOpendr
 
 }
 
-double COpendriveLink::get_length(void)
+double COpendriveLink::get_length(void) const
 {
 
 }
@@ -90,7 +90,7 @@ void COpendriveLink::operator=(const COpendriveLink &object)
 
 }
 
-friend std::ostream& operator<<(std::ostream& out, COpendriveLink &link)
+std::ostream& operator<<(std::ostream& out, COpendriveLink &link)
 {
 
 }
diff --git a/src/opendrive_object.cpp b/src/opendrive_object.cpp
index b6db96c2064232906934a0a87fa29d20d53101b8..2cff04433e5f744a5486759767757e9cdb3b91ff 100644
--- a/src/opendrive_object.cpp
+++ b/src/opendrive_object.cpp
@@ -102,22 +102,22 @@ void COpendriveObject::load(std::unique_ptr<objects::object_type> &object_info)
     this->object_type=OD_NONE;
 }
 
-TOpendriveTrackPoint COpendriveObject::get_pose(void)
+TOpendriveTrackPoint COpendriveObject::get_pose(void) const
 {
   return this->pose;
 }
 
-std::string COpendriveObject::get_type(void)
+std::string COpendriveObject::get_type(void) const
 {
   return this->type;
 }
 
-std::string COpendriveObject::get_name(void)
+std::string COpendriveObject::get_name(void) const
 {
   return this->name;
 }
 
-bool COpendriveObject::is_box(void)
+bool COpendriveObject::is_box(void) const
 {
   if(this->object_type==OD_BOX)
     return true;
@@ -125,7 +125,7 @@ bool COpendriveObject::is_box(void)
     return false;
 }
 
-bool COpendriveObject::is_cylinder(void)
+bool COpendriveObject::is_cylinder(void) const
 {
   if(this->object_type==OD_CYLINDER)
     return true;
@@ -133,7 +133,7 @@ bool COpendriveObject::is_cylinder(void)
     return false;
 }
 
-bool COpendriveObject::is_polygon(void)
+bool COpendriveObject::is_polygon(void) const
 {
   if(this->object_type==OD_POLYGON)
     return true;
@@ -141,17 +141,17 @@ bool COpendriveObject::is_polygon(void)
     return false;
 }
 
-TOpendriveBox COpendriveObject::get_box_data(void)
+TOpendriveBox COpendriveObject::get_box_data(void) const
 {
   return this->object.box;
 }
 
-TOpendriveCylinder COpendriveObject::get_cylinder_data(void)
+TOpendriveCylinder COpendriveObject::get_cylinder_data(void) const
 {
   return this->object.cylinder;
 }
 
-TOpendrivePolygon COpendriveObject::get_polygon_data(void)
+TOpendrivePolygon COpendriveObject::get_polygon_data(void) const
 {
   return this->object.polygon;
 }
diff --git a/src/opendrive_param_poly3.cpp b/src/opendrive_param_poly3.cpp
index 8be71e644eff4ca030100a4fc36c8c9290779047..4d66b3a557044733b21ac27643d1ffeef903019b 100644
--- a/src/opendrive_param_poly3.cpp
+++ b/src/opendrive_param_poly3.cpp
@@ -40,7 +40,7 @@ COpendriveParamPoly3::COpendriveParamPoly3(const COpendriveParamPoly3 &object) :
   this->normalized=object.normalized;
 }
 
-bool COpendriveParamPoly3::transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local)
+bool COpendriveParamPoly3::transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const
 {
   double p = (this->normalized ? (track.s - this->min_s)/(this->max_s - this->min_s): (track.s - this->min_s));
   double p2 = p*p;
@@ -57,12 +57,18 @@ bool COpendriveParamPoly3::transform_local_pose(TOpendriveTrackPoint &track,TOpe
 
 void COpendriveParamPoly3::print(std::ostream &out)
 {
-  std::cout << "  U params: a = " << this->u.a << ", b = " << this->u.b  << ", c = " << this->u.c  << ", d = " << this->u.d << std::endl;
-  std::cout << "  V params: a = " << this->v.a << ", b = " << this->v.b  << ", c = " << this->v.c  << ", d = " << this->v.d << std::endl;
+  COpendriveGeometry::print(out);
+  std::cout << "        U params: a = " << this->u.a << ", b = " << this->u.b  << ", c = " << this->u.c  << ", d = " << this->u.d << std::endl;
+  std::cout << "        V params: a = " << this->v.a << ", b = " << this->v.b  << ", c = " << this->v.c  << ", d = " << this->v.d << std::endl;
   if(this->normalized)
-    std::cout << "  Normalized" << std::endl;
+    std::cout << "        Normalized" << std::endl;
   else
-    std::cout << "  Not normalized" << std::endl;
+    std::cout << "         Not normalized" << std::endl;
+}
+
+std::string COpendriveParamPoly3::get_name(void)
+{
+  return std::string("Parametric polynomial 3th degree");
 }
 
 void COpendriveParamPoly3::load_params(const planView::geometry_type &geometry_info)
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index dea20a4f3cb2677d1296d3ccb8fe3073f7099027..5c33438a4ed354bb4fbb853c2f62520f63885f93 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -22,12 +22,12 @@ COpendriveRoad::COpendriveRoad(const COpendriveRoad& object)
   this->segments.clear();
   for(unsigned int i=0;i<object.segments.size();i++)
   {
-    segment=new COpendriveSegment(*object.segment[i]);
+    segment=new COpendriveRoadSegment(*object.segments[i]);
     this->segments.push_back(segment);
-    new_references[object.segment[i]]=segment;
+    new_references[object.segments[i]]=segment;
   }
   for(unsigned int i=0;i<this->segments.size();i++)
-    this->segments[i]->update_referecnes(new_references);
+    this->segments[i]->update_references(new_references);
 }
 
 void COpendriveRoad::load(const std::string &filename)
@@ -46,9 +46,15 @@ void COpendriveRoad::load(const std::string &filename)
       std::unique_ptr<OpenDRIVE> open_drive(OpenDRIVE_(filename.c_str(), xml_schema::flags::dont_validate));
       for (OpenDRIVE::road_iterator road_it(open_drive->road().begin()); road_it != open_drive->road().end(); ++road_it)
       {
-        segment=new COpendriveRoadSegment();
-        segment->load(*road_it);
-        this->segments.push_back(segment);
+        try{
+          segment=new COpendriveRoadSegment();
+          segment->set_resolution(this->resolution);
+          segment->set_scale_factor(this->scale_factor);
+          segment->load(*road_it);
+          this->segments.push_back(segment);
+        }catch(CException &e){
+          std::cout << e.what() << std::endl;
+        }
       }
     }catch (const xml_schema::exception& e){
       std::ostringstream os;
@@ -70,7 +76,7 @@ void COpendriveRoad::set_resolution(double res)
     this->segments[i]->set_resolution(res);
 }
 
-double COpendriveRoad::get_resolution(void)
+double COpendriveRoad::get_resolution(void) const
 {
   return this->resolution;
 }
@@ -84,7 +90,7 @@ void COpendriveRoad::set_scale_factor(double scale)
     this->segments[i]->set_scale_factor(scale);
 }
 
-double COpendriveRoad::get_scale_factor(void)
+double COpendriveRoad::get_scale_factor(void) const
 {
   return this->scale_factor;
 }
@@ -92,19 +98,22 @@ double COpendriveRoad::get_scale_factor(void)
 void COpendriveRoad::set_min_road_length(double length)
 {
   this->min_road_length=length;
+
+  for(unsigned int i=0;i<this->segments.size();i++)
+    this->segments[i]->set_min_road_length(length);
 }
 
-double COpendriveRoad::get_min_road_length(void)
+double COpendriveRoad::get_min_road_length(void) const
 {
   return this->min_road_length;
 }
 
-unsigned int COpendriveRoad::get_num_segments(void)
+unsigned int COpendriveRoad::get_num_segments(void) const
 {
   return this->segments.size();
 }
 
-const COpendriveRoadNode& COpendriveRoad::get_segment(unsigned int index)
+const COpendriveRoadSegment& COpendriveRoad::get_segment(unsigned int index) const
 {
   if(index>=0 && index<this->segments.size())
     return *this->segments[index];
@@ -131,21 +140,21 @@ void COpendriveRoad::operator=(const COpendriveRoad& object)
   this->segments.clear();
   for(unsigned int i=0;i<object.segments.size();i++)
   {
-    segment=new COpendriveSegment(*object.segment[i]);
+    segment=new COpendriveRoadSegment(*object.segments[i]);
     this->segments.push_back(segment);
-    new_references[object.segment[i]]=segment;
+    new_references[object.segments[i]]=segment;
   }
   for(unsigned int i=0;i<this->segments.size();i++)
-    this->segments[i]->update_referecnes(new_references);
+    this->segments[i]->update_references(new_references);
 }
 
 std::ostream& operator<<(std::ostream& out, COpendriveRoad &road)
 {
-  out << "Resolution: " << road>resolution << std::endl;
-  out << "Scale factor: " << road->scale_factor << std::endl;
-  out << "Minimum road lenght: " << road->min_road_length << std::endl;
-  for(unsigned int i=0;i<road->segments;i++)
-    std::cout << road->segments[i] << std::endl;
+  out << "Resolution: " << road.resolution << std::endl;
+  out << "Scale factor: " << road.scale_factor << std::endl;
+  out << "Minimum road lenght: " << road.min_road_length << std::endl;
+  for(unsigned int i=0;i<road.segments.size();i++)
+    std::cout << *road.segments[i] << std::endl;
 
   return out;
 }
@@ -154,7 +163,7 @@ COpendriveRoad::~COpendriveRoad()
 {
   for(unsigned int i=0;i<this->segments.size();i++)
     delete this->segments[i];
-  this->nodes.segments();
+  this->segments.clear();
   this->resolution=DEFAULT_RESOLUTION;
   this->scale_factor=DEFAULT_SCALE_FACTOR;
   this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index 65e1f8e9302d5a7701857c993e5e082f8aa526d4..8a17bfc145b25c5e80e6a1a4f8e76aa9b895d9c6 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -22,31 +22,37 @@ COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
   this->start_point.y=object.start_point.y;
   this->start_point.heading=object.start_point.heading;
   this->lane=object.lane;
-  this->geometry=object.clone();
+  this->geometry=object.geometry->clone();
   this->links.clear();
   for(unsigned int i=0;i<object.links.size();i++)
   {
-    new_link=new COpendriveLink(*object.link[i]);
-    this->links.puahs_back(new_link);
+    new_link=new COpendriveLink(*object.links[i]);
+    this->links.push_back(new_link);
   }
 }
 
 unsigned int COpendriveRoadNode::add_link(COpendriveRoadNode *parent)
 {
-
+  return -1;
 }
 
 void COpendriveRoadNode::set_resolution(double res)
 {
   this->resolution=res;
+
+  for(unsigned int i=0;i<this->links.size();i++)
+    this->links[i]->set_resolution(res);
 }
 
 void COpendriveRoadNode::set_scale_factor(double scale)
 {
   this->scale_factor=scale;
+
+  for(unsigned int i=0;i<this->links.size();i++)
+    this->links[i]->set_scale_factor(scale);
 }
 
-void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendrive *lane)
+void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriveLane *lane)
 {
   TOpendriveTrackPoint track_point;
 
@@ -66,6 +72,7 @@ void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriv
     this->geometry=new COpendriveParamPoly3();
   else
     throw CException(_HERE_,"Unsupported or Missing geometry");
+  this->geometry->set_scale_factor(this->scale_factor);
   this->geometry->load(geom_info);
   // import start position
   track_point.s=0.0;
@@ -75,10 +82,8 @@ void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriv
   {
     delete this->geometry;
     this->geometry=NULL;
-    throw CException(_HERE_,"Impossible to get world coordinates for this-");
+    throw CException(_HERE_,"Impossible to get world coordinates");
   }
-  this->start_point.x/=this->scale_factor;
-  this->start_point.y/=this->scale_factor;
   for(unsigned int i=0;i<this->links.size();i++)
   {
     delete this->links[i];
@@ -88,27 +93,27 @@ void COpendriveRoadNode::load(const planView::geometry_type &geom_info,COpendriv
   this->lane=lane;
 }
 
-double COpendriveRoadNode::get_resolution(void)
+double COpendriveRoadNode::get_resolution(void) const
 {
   return this->resolution;
 }
 
-double COpendriveRoadNode::get_scale_factor(void)
+double COpendriveRoadNode::get_scale_factor(void) const
 {
   return this->scale_factor;
 }
 
-TOpendriveWorldPoint COpendriveRoadNode::get_start_pose(void)
+TOpendriveWorldPoint COpendriveRoadNode::get_start_pose(void) const
 {
   return this->start_point;
 }
 
-unsigned int COpendriveRoadNode::get_num_links(void)
+unsigned int COpendriveRoadNode::get_num_links(void) const
 {
   return this->links.size();
 }
 
-const COpendriveLink &COpendriveRoadNode::get_link(unsigned int index)
+const COpendriveLink &COpendriveRoadNode::get_link(unsigned int index) const
 {
   if(index>=0 && index<this->links.size())
     return *this->links[index];
@@ -116,12 +121,17 @@ const COpendriveLink &COpendriveRoadNode::get_link(unsigned int index)
     throw CException(_HERE_,"Invalid link index");
 }
 
-const COpendriveLane &COpendriveRoadNode::get_lane(void)
+const COpendriveLane &COpendriveRoadNode::get_lane(void) const
 {
   return *this->lane;
 }
 
-void COpendriveRoadNode::operator=(const COpendriveRoadNode &object)
+const COpendriveGeometry &COpendriveRoadNode::get_geometry(void) const
+{
+  return *this->geometry;
+}
+
+void COpendriveRoadNode::operator=(const COpendriveRoadNode &object) 
 {
   COpendriveLink *new_link;
 
@@ -131,7 +141,7 @@ void COpendriveRoadNode::operator=(const COpendriveRoadNode &object)
   this->start_point.y=object.start_point.y;
   this->start_point.heading=object.start_point.heading;
   this->lane=object.lane;
-  this->geometry=object.clone();
+  this->geometry=object.geometry->clone();
   for(unsigned int i=0;i<this->links.size();i++)
   {
     delete this->links[i];
@@ -140,13 +150,25 @@ void COpendriveRoadNode::operator=(const COpendriveRoadNode &object)
   this->links.clear();
   for(unsigned int i=0;i<object.links.size();i++)
   {
-    new_link=new COpendriveLink(*object.link[i]);
-    this->links.puahs_back(new_link);
+    new_link=new COpendriveLink(*object.links[i]);
+    this->links.push_back(new_link);
   }
 }
 
 std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node)
 {
+  out << "      start point: x: " << node.start_point.x << " y: " << node.start_point.y << " heading: " << node.start_point.heading << std::endl;
+  if(node.lane!=NULL)
+    out << "      lane id: " << node.lane->get_id() << std::endl;
+  else
+    out << "      Does not belong to a lane." << std::endl;
+  if(node.geometry!=NULL)
+    out << *node.geometry << std::endl;
+  else
+    out << "      Does not have any associated geometry." << std::endl;
+  out << "      Number of links: " << node.links.size() << std::endl;
+  for(unsigned int i=0;i<node.links.size();i++)
+    out << *node.links[i] << std::endl;
   return out;
 }
 
@@ -157,30 +179,17 @@ COpendriveRoadNode::~COpendriveRoadNode()
   this->start_point.x=0.0;
   this->start_point.y=0.0;
   this->start_point.heading=0.0;
-  this->index=-1;
-  this->lane=0;
-  this->station=-1;
-  this->name="";
-  this->parents.clear();
-  for(unsigned int i=0;i<this->children.size();i++)
-  {
-    delete this->children[i].spline;
-    this->children[i].spline=NULL;
-    delete this->children[i].geometry;
-    this->children[i].geometry=NULL;
-  }
-  this->children.clear();
-  for(unsigned int i=0;i<this->signals.size();i++)
+  this->lane=NULL;
+  if(this->geometry!=NULL)
   {
-    delete this->signals[i];
-    this->signals[i]=NULL;
+    delete this->geometry;
+    this->geometry=NULL;
   }
-  this->signals.clear();
-  for(unsigned int i=0;i<this->objects.size();i++)
+  for(unsigned int i=0;i<this->links.size();i++)
   {
-    delete this->objects[i];
-    this->objects[i]=NULL;
+    delete this->links[i];
+    this->links[i]=NULL;
   }
-  this->objects.clear();
+  this->links.clear();
 }
 
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 7b08c5cbe745ba36741de7bbf3da83b56a802dcd..d1ecdcf3bb2397c5e89b28f2d4c96278a4a322a1 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -1,4 +1,5 @@
 #include "opendrive_road_segment.h"
+#include "exceptions.h"
 
 COpendriveRoadSegment::COpendriveRoadSegment()
 {
@@ -11,33 +12,37 @@ COpendriveRoadSegment::COpendriveRoadSegment()
   this->objects.clear();
   this->next.clear();
   this->prev.clear();
+  this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
+  this->resolution=DEFAULT_RESOLUTION;
+  this->scale_factor=DEFAULT_SCALE_FACTOR;
 }
 
 COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object)
 {
-  COpendriveLane *lane;
-  COpendriveSignal *signal;
-  CopendriveObject *object;
+  COpendriveLane *new_lane;
+  COpendriveSignal *new_signal;
+  COpendriveObject *new_object;
+  std::map<int,COpendriveLane *>::const_iterator lane_it;
 
   this->name=object.name;
   this->id=object.id;
   this->lanes.clear();
-  for(unsigned int i=0;i<object.lanes.size();i++)
+  for(lane_it=object.lanes.begin();lane_it!=object.lanes.end();++lane_it)
   {
-    lane=new COpendriveLane(*object.lanes[i]);
-    this->lanes.push_back(lane);
+    new_lane=new COpendriveLane(*lane_it->second);
+    this->lanes[lane_it->first]=new_lane;
   }
   this->signals.clear();
   for(unsigned int i=0;i<object.signals.size();i++)
   {
-    signal=new COpendriveSignal(*object.signals[i]);
-    this->signals.push_back(signal);
+    new_signal=new COpendriveSignal(*object.signals[i]);
+    this->signals.push_back(new_signal);
   }
   this->objects.clear();
   for(unsigned int i=0;i<object.objects.size();i++)
   {
-    object=new COpendriveObject(*object.objects[i]);
-    this->objects.push_back(object);
+    new_object=new COpendriveObject(*object.objects[i]);
+    this->objects.push_back(new_object);
   }
   this->next.resize(object.next.size());
   for(unsigned int i=0;i<object.next.size();i++)
@@ -45,31 +50,50 @@ COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object
   this->next.resize(object.prev.size());
   for(unsigned int i=0;i<object.prev.size();i++)
     this->prev[i]=object.prev[i];
+  this->resolution=object.resolution;
+  this->scale_factor=object.scale_factor;
+  this->min_road_length=object.min_road_length;
 }
 
 void COpendriveRoadSegment::set_resolution(double res)
 {
-  for(unsigned int i=-this->num_right_lanes;i<=this->num_left_lines && i!=0;i++)
-    this->lanes[i]->set_resolution(res);
+  std::map<int,COpendriveLane *>::const_iterator lane_it;
+
+  this->resolution=res;
+
+  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
+    lane_it->second->set_resolution(res);
 }
 
 void COpendriveRoadSegment::set_scale_factor(double scale)
 {
-  for(unsigned int i=-this->num_right_lanes;i<=this->num_left_lines && i!=0;i++)
-    this->lanes[i]->set_scale_factor(scale);
+  std::map<int,COpendriveLane *>::const_iterator lane_it;
+
+  this->scale_factor=scale;
+
+  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
+    lane_it->second->set_scale_factor(scale);
+}
+
+void COpendriveRoadSegment::set_min_road_length(double length)
+{
+  this->min_road_length=length;
 }
 
 void COpendriveRoadSegment::update_references(std::map<COpendriveRoadSegment *,COpendriveRoadSegment *> refs)
 {
-  this->next=refs[this->next];
-  this->prev=refs[this->prev];
+  for(unsigned int i=0;i<this->next.size();i++)
+    this->next[i]=refs[this->next[i]];
+  for(unsigned int i=0;i<this->prev.size();i++)
+    this->prev[i]=refs[this->prev[i]];
 
   for(unsigned int i=0;i<this->lanes.size();i++)
-    this->lanes[i]->update_referecnes(refs);
+    this->lanes[i]->update_references(refs);
 }
 
-void COpendriveRoadSegment::load_road(OpenDRIVE::road_type &road_info)
+void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
 {
+  std::map<int,COpendriveLane *>::const_iterator lane_it;
   lanes::laneSection_iterator lane_section;
   right::lane_iterator r_lane_it;
   left::lane_iterator l_lane_it;
@@ -78,102 +102,131 @@ void COpendriveRoadSegment::load_road(OpenDRIVE::road_type &road_info)
   COpendriveRoadNode *new_node;
   double lane_offset;
 
-  this->name=road_info->name().get();
-  this->id=road_info->id().get();
+  this->name=road_info.name().get();
+  this->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());
-  else if(road_info->lanes().laneSection().size()>1)
-    throw CException(_HERE_,"Road "+road_info->id().get()+" has more than one lane section");
+  if(road_info.lanes().laneSection().size()<1)
+    throw CException(_HERE_,"No lane sections defined for road "+road_info.id().get());
+  else if(road_info.lanes().laneSection().size()>1)
+    throw CException(_HERE_,"Road "+road_info.id().get()+" has more than one lane section");
   else
-    lane_section=road_info->lanes().laneSection().begin();
+    lane_section=road_info.lanes().laneSection().begin();
 
   // right lanes
+  this->num_right_lanes=0;
   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;
+        this->num_right_lanes++;
       }catch(CException &e){
         std::cout << e.what() << std::endl;
       }
     }
   }
   // left lanes
+  this->num_left_lanes=0;
   if(lane_section->left().present())
   {
     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;
+        this->num_left_lanes++;
       }catch(CException &e){
         std::cout << e.what() << std::endl;
       }
     }
   }
   // add road nodes
-  for(geom_it=road_info->planView().geometry().begin(); geom_it!=road_info->planView().geometry().end(); ++geom_it)
-  {
-    lane_offset=0.0;
-    for(unsigned int i=-1;i>=-this->num_right_lines;i--)
-    {
-      new_node=new COpendriveRoadNode();
-      this->lanes[i]->set_offset(lane_offset);
-      new_node->load(*geom_it,this->lanes[i]);
-      this->lanes[i]->add_node(new_node);
-      lane_offset-=this->lanes[i]->get_width();
-    }
-    lane_offset=0.0;
-    for(unsigned int i=1;i<=this->num_left_lines;i++)
+  try{
+    for(geom_it=road_info.planView().geometry().begin(); geom_it!=road_info.planView().geometry().end(); ++geom_it)
     {
-      new_node=new COpendriveRoadNode();
-      this->lanes[i]->set_offset(lane_offset);
-      new_node->load(*geom_it,this->lanes[i]);
-      this->lanes[i]->add_node(new_node);
-      lane_offset+=this->lanes[i]->get_width();
+      lane_offset=0.0;
+      for(unsigned int i=-1;i>=-this->num_right_lanes;i--)
+      {
+        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().get_length()>this->min_road_length)
+        {
+          this->lanes[i]->add_node(new_node);
+          lane_offset-=this->lanes[i]->get_width();
+        }
+      }
+      lane_offset=0.0;
+      for(unsigned int i=1;i<=this->num_left_lanes;i++)
+      {
+        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().get_length()>this->min_road_length)
+        {
+          this->lanes[i]->add_node(new_node);
+          lane_offset+=this->lanes[i]->get_width();
+        }
+      }
     }
+  }catch(CException &e){
+    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
+      delete lane_it->second;
+    this->lanes.clear();
+    throw e;
   }
 }
 
-std::string COpendriveRoadSegment::get_name(void)
+std::string COpendriveRoadSegment::get_name(void) const
 {
   return this->name;
 }
 
-unsigned int COpendriveRoadSegment::get_id(void)
+unsigned int COpendriveRoadSegment::get_id(void) const
 {
   return this->id;
 }
 
-unsigned int COpendriveRoadSegment::get_num_right_lanes(void)
+unsigned int COpendriveRoadSegment::get_num_right_lanes(void) const
 {
   return this->num_right_lanes;
 }
 
-unsigned int COpendriveRoadSegment::get_num_left_lanes(void)
+unsigned int COpendriveRoadSegment::get_num_left_lanes(void) const
 {
   return this->num_left_lanes;
 }
 
-const COpendriveLane &COpendriveRoadSegment::get_lane(int index)
+const COpendriveLane &COpendriveRoadSegment::get_lane(int index) const
 {
-  if(index<-this->num_right_lanes || index>this->num_left_lanes)
-    CException(_HERE_,"invalid lane index");
-  else
-    return *this->lanes[index];
+  std::map<int,COpendriveLane *>::const_iterator lane_it;
+
+  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");
 }
 
-unsigned int COpendriveRoadSegment::get_num_signals(void)
+unsigned int COpendriveRoadSegment::get_num_signals(void) const
 {
   return this->signals.size();  
 }
 
-const COpendriveSignal &COpendriveRoadSegment::get_signal(unsigned int index)
+const COpendriveSignal &COpendriveRoadSegment::get_signal(unsigned int index) const
 {
   if(index>=0 && index<this->signals.size())
     return *this->signals[index];
@@ -181,12 +234,12 @@ const COpendriveSignal &COpendriveRoadSegment::get_signal(unsigned int index)
     throw CException(_HERE_,"Invalid signal index");
 }
 
-unsigned int COpendriveRoadSegment::get_num_obstacles(void)
+unsigned int COpendriveRoadSegment::get_num_obstacles(void) const
 {
   return this->objects.size();
 }
 
-const COpendriveObject &COpendriveRoadSegment::get_object(unsigned int index)
+const COpendriveObject &COpendriveRoadSegment::get_object(unsigned int index) const
 {
   if(index>=0 && index<this->objects.size())
     return *this->objects[index];
@@ -194,12 +247,12 @@ const COpendriveObject &COpendriveRoadSegment::get_object(unsigned int index)
     throw CException(_HERE_,"Invalid object index");
 }
 
-unsigned int COpendriveRoadSegment::get_num_previous(void)
+unsigned int COpendriveRoadSegment::get_num_previous(void) const
 {
   return this->prev.size();
 }
 
-const COpendriveRoadSegment &COpendriveRoadSegment::get_previous(unsigned int index)
+const COpendriveRoadSegment &COpendriveRoadSegment::get_previous(unsigned int index) const
 {
   if(index>=0 && index<this->prev.size())
     return *this->prev[index];
@@ -207,12 +260,12 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_previous(unsigned int in
     throw CException(_HERE_,"Invalid previous segment index");
 }
 
-unsigned int COpendriveRoadSegment::get_num_next(void)
+unsigned int COpendriveRoadSegment::get_num_next(void) const
 {
   return this->next.size();
 }
 
-const COpendriveRoadSegment &COpendriveRoadSegment::get_next(unsigned int index)
+const COpendriveRoadSegment &COpendriveRoadSegment::get_next(unsigned int index) const
 {
   if(index>=0 && index<this->next.size())
     return *this->next[index];
@@ -222,29 +275,30 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_next(unsigned int index)
 
 void COpendriveRoadSegment::operator=(const COpendriveRoadSegment &object)
 {
-  COpendriveLane *lane;
-  COpendriveSignal *signal;
-  CopendriveObject *object;
+  COpendriveLane *new_lane;
+  COpendriveSignal *new_signal;
+  COpendriveObject *new_object;
+  std::map<int,COpendriveLane *>::const_iterator lane_it;
 
   this->name=object.name;
   this->id=object.id;
   this->lanes.clear();
-  for(unsigned int i=0;i<object.lanes.size();i++)
+  for(lane_it=object.lanes.begin();lane_it!=object.lanes.end();++lane_it)
   {
-    lane=new COpendriveLane(*object.lanes[i]);
-    this->lanes.push_back(lane);
+    new_lane=new COpendriveLane(*lane_it->second);
+    this->lanes[lane_it->first]=new_lane;
   }
   this->signals.clear();
   for(unsigned int i=0;i<object.signals.size();i++)
   {
-    signal=new COpendriveSignal(*object.signals[i]);
-    this->signals.push_back(signal);
+    new_signal=new COpendriveSignal(*object.signals[i]);
+    this->signals.push_back(new_signal);
   }
   this->objects.clear();
   for(unsigned int i=0;i<object.objects.size();i++)
   {
-    object=new COpendriveObject(*object.objects[i]);
-    this->objects.push_back(object);
+    new_object=new COpendriveObject(*object.objects[i]);
+    this->objects.push_back(new_object);
   }
   this->next.resize(object.next.size());
   for(unsigned int i=0;i<object.next.size();i++)
@@ -252,18 +306,41 @@ void COpendriveRoadSegment::operator=(const COpendriveRoadSegment &object)
   this->next.resize(object.prev.size());
   for(unsigned int i=0;i<object.prev.size();i++)
     this->prev[i]=object.prev[i];
+  this->resolution=object.resolution;
+  this->scale_factor=object.scale_factor;
+  this->min_road_length=object.min_road_length;
 }
 
-friend std::ostream& operator<<(std::ostream& out, COpendriveRoadsegment &segment)
+std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
 {
+  out << "Road " << segment.get_name() << std::endl;
+  out << "  Previous road segments: " << segment.prev.size() << std::endl;
+  for(unsigned int i=0;i<segment.prev.size();i++)
+    out << "    " << segment.prev[i]->get_name() << std::endl;
+  out << "  Next road segments: " << segment.next.size() << std::endl;
+  for(unsigned int i=0;i<segment.next.size();i++)
+    out << "    " << segment.next[i]->get_name() << std::endl;
+  out << "  Number of right lanes: " << segment.num_right_lanes << std::endl;
+  for(unsigned int i=-1;i>=-segment.num_right_lanes;i--)
+    out << *segment.lanes[i] << std::endl;
+  out << "  Number of left lanes: " << segment.num_left_lanes << std::endl;
+  for(unsigned int i=1;i<=segment.num_left_lanes;i++)
+    out << *segment.lanes[i] << std::endl;
+  out << "  Number of signals: " << segment.signals.size() << std::endl;
+  for(unsigned int i=0;i<segment.signals.size();i++)
+    std::cout << *segment.signals[i] << std::endl;
+  out << "  Number of objects: " << segment.objects.size() << std::endl;
+  for(unsigned int i=0;i<segment.objects.size();i++)
+    std::cout << *segment.objects[i] << std::endl;
 
+  return out;
 }
 
 COpendriveRoadSegment::~COpendriveRoadSegment()
 {
   this->name="";
   this->id=-1;
-  for(unsigned int i=-this->num_right_lanes;i<=this->num_left_lines && i!=0;i++)
+  for(unsigned int i=-this->num_right_lanes;i<=this->num_left_lanes && i!=0;i++)
   {
     delete this->lanes[i];
     this->lanes[i]=NULL;
@@ -285,5 +362,8 @@ COpendriveRoadSegment::~COpendriveRoadSegment()
   this->objects.clear();
   this->next.clear();
   this->prev.clear();
+  this->resolution=DEFAULT_RESOLUTION;
+  this->scale_factor=DEFAULT_SCALE_FACTOR;
+  this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
 }
 
diff --git a/src/opendrive_signal.cpp b/src/opendrive_signal.cpp
index 39f1aab2153bf5954a0b0c337e862e2ecf65d42d..7e7d74473b7ccc0c6b81fcefd1828c3cbdf18d11 100644
--- a/src/opendrive_signal.cpp
+++ b/src/opendrive_signal.cpp
@@ -54,28 +54,28 @@ void COpendriveSignal::load(std::unique_ptr<signals::signal_type> &signal_info)
   this->pose.heading = normalize_angle(this->pose.heading + (reverse ? M_PI : 0.0));
 }
 
-int COpendriveSignal::get_id(void)
+int COpendriveSignal::get_id(void) const
 {
   return this->id;
 }
 
-TOpendriveTrackPoint COpendriveSignal::get_pose(void)
+TOpendriveTrackPoint COpendriveSignal::get_pose(void) const
 {
   return this->pose;
 }
 
-void COpendriveSignal::get_type(std::string &type, std::string &sub_type)
+void COpendriveSignal::get_type(std::string &type, std::string &sub_type) const
 {
   type=this->type;
   sub_type=this->sub_type;
 }
 
-int COpendriveSignal::get_value(void)
+int COpendriveSignal::get_value(void) const
 {
   return this->value;
 }
 
-std::string COpendriveSignal::get_text(void)
+std::string COpendriveSignal::get_text(void) const
 {
   return this->text;
 }
diff --git a/src/opendrive_spiral.cpp b/src/opendrive_spiral.cpp
index 5407939783d5d4c3f741106b522919332e14e8c5..47a686336acd4d1e01994ef2fef9982825c04b2a 100644
--- a/src/opendrive_spiral.cpp
+++ b/src/opendrive_spiral.cpp
@@ -18,14 +18,15 @@ COpendriveSpiral::COpendriveSpiral(const COpendriveSpiral &object) : COpendriveG
   this->end_curvature=object.end_curvature;
 }
 
-bool COpendriveSpiral::transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local)
+bool COpendriveSpiral::transform_local_pose(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local) const
 {
-  return false;
+  return true;
 }
 
 void COpendriveSpiral::print(std::ostream &out)
 {
-  out << "  start_curvature = " << this->start_curvature << ", end_curvature = " << this->end_curvature << std::endl;
+  COpendriveGeometry::print(out);
+  out << "        start_curvature = " << this->get_start_curvature() << ", end_curvature = " << this->get_end_curvature() << std::endl;
 }
 
 void COpendriveSpiral::load_params(const planView::geometry_type &geometry_info)
@@ -34,6 +35,11 @@ void COpendriveSpiral::load_params(const planView::geometry_type &geometry_info)
   this->end_curvature = (geometry_info.spiral().get().curvEnd().present() ? geometry_info.spiral().get().curvEnd().get() : 0.0);
 }
 
+std::string COpendriveSpiral::get_name(void)
+{
+  return std::string("spiral");
+}
+
 COpendriveGeometry *COpendriveSpiral::clone(void)
 {
   COpendriveSpiral *new_spiral=new COpendriveSpiral(*this);
@@ -43,12 +49,12 @@ COpendriveGeometry *COpendriveSpiral::clone(void)
 
 double COpendriveSpiral::get_start_curvature(void)
 {
-  return this->start_curvature;
+  return this->start_curvature*this->scale_factor;
 }
 
 double COpendriveSpiral::get_end_curvature(void)
 {
-  return this->end_curvature;
+  return this->end_curvature*this->scale_factor;
 }
 
 void COpendriveSpiral::operator=(const COpendriveSpiral &object)